Extend AprilTag Localization
The goal of this project is to quickly build an accurate (calibration-free) localization system in indoor medium & large-scale scenarios.
In the project, we define the 0th Tag as the base tag for each Tag family. The output pose is related to the base tag. Thus, we use the method of common view to construct a series of April tag pose associations. After that, we will use the available tag in each frame to calculate the global camera pose and finish the localization task.
Uncertainty in the resulting position is about several centimeters.
For the usage, The project is deployed on two different platforms.
1. Raspberry Pi(3, 3b+, 4, zero 2w) + CSI MIPI Camera.
Raspberry pi Os 32bit LTS With Legacy Camera API
DLL shared library: AprilTag, Yaml-cpp, Glog, Ceres, Opencv4, Eigen3
2. Ros / Ros2 + Camera with Ros Driver.
Ros Noetic / Ros2 Humble with the same library installed
Camera Ros Driver delivers valid image msgs in a topic
The whole system consists of two parts: the Tag Generator module & the Localization module.
The Tag Generator module will get tag information (Tag Family, Tag Size / Tag print PPI, Extend Dot size/position) from the YAML file and generate a <Ready to Print> Tag image with Extend dot.
The localization module will get information (Tag info, Camera Intrinsic parameter/Distortion coefficient, Tag pre-calibration pose) from the YAML file and do the localization task. Finally, the output should be the camera pose related to the base tag.
The localization module also contains two parts front end & back end.
The front end is mainly responsible for processing each frame, including the detection of Apriltag and Extend dots and using the PnP solver to get the Pre-calculation initial guess to pose for backend usage. Finally, it will also be responsible for the output pose calculation. The front end will run at the frequency the same as the camera capture frame, typically 30hz.
The back end is mainly responsible for optimizing the pose of non-base tags with the common view observation. We use the edge sliding window (deque to limit excessive constraints) to build a residual block tree for non-linear optimization.
The optimization takes more time than the front end, it will run at a frequency lower than the camera capture frame, typically 5-30hz.
Test platform 2 is Raspberry Pi 4 with 4GB ram.
The camera is Raspberry Pi Cam V1 (Ov5640) 640 * 480 model.
Test platform 1 is Raspberry Pi Zero 2w
The camera is Raspberry Pi Cam V1 (Ov5640) 640 * 480 model.
Test platform 3 is X86 laptop with Ubuntu 20.04 and Ros Noetic.
The camera is Realsense D435i 1280 * 720 model.
Name | Discription | Module |
|---|---|---|
DebugPlot | Choose whether to plot the debug image (close to get better performance) | localization(Pi only) |
PosePublishTopic | The output camera pose topic | localization (Ros only) |
T_b_c | deprecate | / |
ImageDistrotion | Distrotion parameter of the camera
[k1, k2, p1, p2, k3] | localization |
ImageIntrinsic | Intrinsic parameter of the camera
[ fx, 0, cx, 0, f_y, c_y, 0, 0, 1] | localization |
ImageSize | The input image size [W, H] | localization |
ImageTopic | The input image topic | localization (Ros only) |
Tag ID as key | File : The raw tag input image name Size : The size of tag edge (mm) Canvas : The size of canvas (mm) [W, H] Extand_Dot : The position and size of extend dot (mm) [X, Y, R] Pixpermm : PPI for image Extrinsic : Pre-calibrate pose for tag (option) | localization & generator |
GenPath | The extend tag output image path | generator |
GenTags | The tag id used by tag generator | generator |
ImageCount | The raw tag input image path | generator |
ImagePath | The raw tag image path | generator |
TagFamily | The TagFamily used by both localization module and generator | localization |