Oh , By the way, this is also a contribution to the ArduPilot ecosystem as it is adding AprilTags as a demonstrated pose tracking configuration.
AprilTags are a visual fiducial system popular in robotics research. This experimentaion contains the most recent version of AprilTags, AprilTag 3, which includes a faster (>2x) detector, improved detection rate on small tags, flexible tag layouts, and pose estimation.
Congrats to @LuckyBird for the work done till now both on the system and on the documentation.
I only have a doubt on the performance comparison between AprilTags and Aruco.
On gitter I read: “apriltag (apriltag 3) compares it directly with ARUCO, and shows that apriltag3 runs faster”, but here I understand that you can send vision position estimation message at 2-3Hz in flight, meanwhile I had ~15Hz for the same message using Aruco board.
Maybe I didn’t understand well what I read.
[EDIT]
We use the same Companion Computer: Raspberry Pi 3, but I use a Raspberry Pi Camera V2 while you use an usb cam, maybe this difference is important.
As @LuckyBird wrote:
RPi can run AprilTags algorithm at 11Hz when the camera is stable but when there are movement, the rate reduced to only 2-3Hz
So is the difference is a higher rejection of AprilTag or the camera type or the integration on the platform (read vibration damping method).
Yes , that would be really interesting comparing both system on the same platform. We might get back to it at a later time as we keep momentum to push the RealSense T265 experiments.
@anbello would you be interested to give it a try ?
Yes, the only problem is little time to dedicate to this stuff, I don’t know when but I will do for sure.
Hi I wanted to give a brief overview of the project I am working on and ask some questions to help debug.
I am trying to develop a precision landing system on top of Apriltags with an Ardupilot drone. I have mounted a usb camera facing down on a custom built drone. I am running ardupilot on a pixhawk cube. I want the vision position estimate to augment GPS to allow a precision landing directly over the Apriltag. I am using QGround Control.
Currently using Apriltag_ros wrapper to detect the tags in ros.
Continuous detection of the Apriltags works great, outputs in RVIZ, and can see the drone moving with camera setup.
I followed the setup presented here to take this TF of the camera/tag frame into ardupilot vision position estimate:
Using mavros to send information to Ardupilot.
http://wiki.ros.org/mavros
Currently I am having trouble with the apriltags_to_mavros.launch file in vision_to_mavros.
The /mavros/vision_pose/pose topic in ros does not seem like its getting updated correctly. During flight test I can not successfully get the drone to loiter over the apriltags without GPS. In QGC I am getting 20 hz feed of the vision_position_estimate parameter, but the drone doesn’t converge over the apriltags.
During tests in RVIZ it seems like /mavros/vision_pose/pose doesn’t match up with /tf and the camera_frame.
Any advice on this setup or improvement will be helpful and I can answer more questions on my vision system.
@LuckyBird
Hi I was hoping to talk with you more about possible reasons I am not getting my drone to hover over the apriltags in loiter mode. Below I will post current parameters of the ardupilot setup and also our modified vision_to_mavros code.
An important thing is I am trying to augment GPS navigation with the vision system position. I had to make edits to the vision_to_mavros.cpp file in order to correct the orientations of the camera and image in respect to movements of the drone.
I am running the program on a Jetson Nano. The detection rate of the tags is about 25hz when the image is stable. The Vision_position_estimate parameter on ardupilot is about 20hz when the drone is stable but drops to 5-15hz when the drone starts moving around.
Currently the drone makes too violent of corrections and loses focus on the apriltags. Is there a way to reduce how much the EKF uses vision position estimate versus gps position?
April_Params.param (14.3 KB)
vision_to_mavros.cpp (6.4 KB)
Hi @jacksparkman, sorry for the late reply. Thank you for reaching out and test the system.
When you run the system, do you see both /mavros/vision_pose/pose and /mavros/local_position/pose, on RViz? Moving the camera around the AprilTag Board, you should see the two poses moving closely to each other. Is the tag board fixed on the ground with tag’s x-axis to the right?
Hey I am starting to figure out the coordinate systems on the drone. Currently local_position/pose does not line up with vision_pose/pose as shown in the image. Vision_pose’s origin is about the rviz origin but’s its coordinate frame is not aligned with camera frame.
This image was taken after I edited around with your vision_to_mavros.cpp. In the picture the land_pad is the apriltag, camera is the onboard camera facing down towards the tag. Both the land_pad’s and the camera’s x-axis is pointed to the right of the drone.
Currently vision_pose looks like the x-axis is pointed up, y-axis(green) is pointed towards the front of the drone, the z-axis is left.
What should be the proper direction for the EKF to read vision_pose properly?
Hi @jacksparkman, for the vision position data to be fused correctly, it should follow ENU coordinates as received by mavros. You can have an overview of all the frames with this picture. In your case the T265 node is equivalent to Apriltag ros node.
Thus, you need to modify the code so that vision pose’ x axis to the front, y to the right and z upwards.
Should I be editing just the roll_cam, pitch_cam, and yaw_cam parameters to get the vision pose in the correct frame? I was not sure what gamma_world does.
Editing roll_cam, pitch_cam, and yaw_cam, with gamma_world set to 0, should be enough. The gamma_world param is used to adjust the initial body frame’s heading in the world frame for other types of vision system, so it is not essential for what we are doing.
Thank you for your help with flying drones with Apriltags. The trouble with my setup seemed to be based on the USB camera not having great stability in flight. I have moved on to a realsense T265 setup.
Hi the link for the pdf of the A3 Apriltag and the layout file are broken. Moreover I have a question can I print tags on A4 and then stitch them together or is there any pitfalls? Thanks in advance.
Sorry about that, I will fix the links soon. In the mean time, you can just stitch the tags together so long as they are clearly visible and don’t overlap on one another (there should be at least one white “pixel” boundaries in all directions for each tag).
Hi I was able to get the A3 bundle tags. But can you provide the yaml file for it.
Moreover I get this error all the time: [ WARN] [1596404894.602932696]: “camera_odom_frame” passed to lookupTransform argument target_frame does not exist.
I have to hard-code the “target_frame_id, source_frame_id” to make it work. Do you know why rosparam was not able to retrieve the correct values ?
Thanks in advance
Hi @marknabil, the pdf and yaml file can be found here: https://github.com/thien94/vision_to_mavros/tree/apriltag/apriltag
Regarding the odom frame, maybe there are some differences between the ROS version you are using vs mine. I started this project awhile ago, it’s highly possible a lot of stuffs has changed.
Hi @LuckyBird , Thank you for all your projects !
Regarding apriltags ROS wrapper , in this project or the Landig, should we use the original one or the fork in your git ?
Regarding calibration, also T265 needs to be calibrated or is enough to use t265_fisheye_undistort_node ?
Thanks a lot for your guidance !
Hi @Gian
Both should be fine. The only difference is that my fork has some modifications for testing purposes.
Ideally, recalibration is not necessary because each T265 should be calibrated on the production line. You can just obtain the calibration data (using the t265_stereo.py or other rs- examples) and replace the data in the config file. The provided calibration file should work “fine” to try out and test that things are working, although the result might be slightly less accurate.
On that note, I think a better solution is to use librealsense APIs to obtain the calibration directly from the device to reduce artificial and operational error, which I have not gotten around to implement yet.
Hi Thien
Thank you for your reply . In order to set the calibration data I followed your instructions at Precision Landing with ROS, Realsense T265 Camera and AprilTag 3 - Part 2⁄2.
I have almost setup my non-GPS flight drone, and I am looking for a safe indoor location to try it 
Thank you guys , I am really grateful for your help and hope to contribute when I will gain experience.
Regards
gian
anyone i have some problems.
when i run roslaunch vision_to_mavros apriltags_to_mavros.launch
it’s shows “camera odom frame doesnt exist”, here’s the output :
CLEAR PARAMETERS
PARAMETERS
NODES
/
apriltag_ros_continuous_node (apriltag_ros/apriltag_ros_continuous_node)
apriltags_to_mavros (vision_to_mavros/vision_to_mavros_node)
usb_cam (usb_cam/usb_cam_node)
/usb_cam/
image_proc (image_proc/image_proc)
ROS_MASTER_URI=http://localhost:11311
process[usb_cam-1]: started with pid [3079]
process[usb_cam/image_proc-2]: started with pid [3080]
process[apriltag_ros_continuous_node-3]: started with pid [3081]
process[apriltags_to_mavros-4]: started with pid [3087]
[ INFO] [1700593740.284116499]: Initializing nodelet with 4 worker threads.
[ WARN] [1700593740.342449071]: Using default target_frame_id: /camera_odom_frame
[ WARN] [1700593740.359391722]: Using default source_frame_id: /camera_link
[ WARN] [1700593740.368333904]: Using default output_rate: 20.000000
[ WARN] [1700593740.373328442]: Using default gamma_world: -1.570796
[ WARN] [1700593740.379981271]: Using default roll_cam: 0.000000
[ WARN] [1700593740.383671261]: Using default pitch_cam: 0.000000
[ WARN] [1700593740.386560511]: Using default yaw_cam: 1.570796
[ INFO] [1700593740.394872057]: Precision landing disabled by default
[ INFO] [1700593741.273298336]: Loading tag bundle ‘a3_bundle’
[ INFO] [1700593741.273794498]: 0) id: 0, size: 0.078, p = [0,0,0], q = [1,0,0,0]
[ INFO] [1700593741.274017913]: 1) id: 1, size: 0.078, p = [0.095,0,0], q = [1,0,0,0]
[ INFO] [1700593741.274188590]: 2) id: 2, size: 0.078, p = [0.191,0,0], q = [1,0,0,0]
[ INFO] [1700593741.274344453]: 3) id: 3, size: 0.078, p = [0,-0.097,0], q = [1,0,0,0]
[ INFO] [1700593741.274519000]: 4) id: 4, size: 0.078, p = [0.095,-0.097,0], q = [1,0,0,0]
[ INFO] [1700593741.274665789]: 5) id: 5, size: 0.078, p = [0.191,-0.097,0], q = [1,0,0,0]
[ INFO] [1700593741.274792617]: 6) id: 6, size: 0.078, p = [0,-0.197,0], q = [1,0,0,0]
[ INFO] [1700593741.274910907]: 7) id: 7, size: 0.078, p = [0.095,-0.197,0], q = [1,0,0,0]
[ INFO] [1700593741.275035494]: 8) id: 8, size: 0.078, p = [0.191,-0.197,0], q = [1,0,0,0]
[ INFO] [1700593741.275137563]: 9) id: 9, size: 0.078, p = [0,-0.295,0], q = [1,0,0,0]
[ INFO] [1700593741.275244058]: 10) id: 10, size: 0.078, p = [0.095,-0.295,0], q = [1,0,0,0]
[ INFO] [1700593741.275361293]: 11) id: 11, size: 0.078, p = [0.191,-0.295,0], q = [1,0,0,0]
[ WARN] [1700593741.300778413]: remove_duplicates parameter not provided. Defaulting to true
[ INFO] [1700593741.922037383]: camera calibration URL: file:///home/buv/.ros/camera_info/head_camera.yaml
[ INFO] [1700593741.935996513]: Starting ‘usb_cam’ (/dev/video0) at 640x480 via mmap (yuyv) at 30 FPS
[ WARN] [1700593742.355470841]: unknown control ‘focus_auto’
[ INFO] [1700593742.490196643]: Camera model: fx = 572.913, fy = 650.912, cx = -22.6092, cy = 198.355
[ WARN] [1700593743.403325429]: “camera_odom_frame” passed to lookupTransform argument target_frame does not exist.
i already change the parameter target_frame and source_frame, here’s my launch file :
<!-- This node description you can take from usb_cam-test.launch -->
<node name="$(arg cam_name)" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="camera_name" value="$(arg cam_name)" />
<param name="camera_info_url" value="file:///home/buv/.ros/camera_info/head_camera.yaml" />
<param name="io_method" value="mmap"/>
</node>
<!-- This node provides image rectification -->
<node name="image_proc" pkg="image_proc" type="image_proc" ns="$(arg cam_name)"/>
<!-- This node will launch apriltag_ros -->
<arg name="launch_prefix" default="" />
<arg name="node_namespace" default="apriltag_ros_continuous_node" />
<arg name="camera_name" default="/$(arg cam_name)" />
<arg name="camera_frame" default="usb_cam" />
<arg name="image_topic" default="image_raw" />
<!-- Set parameters -->
<rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="$(arg node_namespace)" />
<rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="$(arg node_namespace)" />
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
<remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
<remap from="camera_info" to="$(arg camera_name)/camera_info" />
<param name="camera_frame" type="str" value="usb_cam" />
<param name="publish_tag_detections_image" type="bool" value="true" />
</node>
<!-- This node will launch frame conversion from vision pose (tf) to mavros pose -->
<node pkg="vision_to_mavros" type="vision_to_mavros_node" name="apriltags_to_mavros" output="screen" >
<param name="target_frame_id" value="$(arg tag_name)" />
<param name="source_frame_id" value="$(arg cam_name)" />
<remap from="vision_pose" to="/mavros/vision_pose/pose" />
</node>
i dont know why it’s still using default camera_odom_frame and camera_link