Indoor non-GPS flight using AprilTags (ROS-based)

Google Summer of Code
,
1

In this blog, I will attempt to apply the awesome works that have been done by @anbello, @ppoirier, @chobitsfan with a new fiducial marker system: using AprilTags to facilitate indoor non-GPS vision-based positioning.

For those interested, here are the references to related articles:

1. Before we start

In order to make it work, you will need to know about ROS, Linux, Python, how to configure wifi network, ssh, etc. A plethora of problems can arise with these things, so if you are not familiar with these topics, head over to the dev wiki page as well as tons of other resources on google to get started.

One thing I have learned during the process of working on this lab is that details matter. Depends on your particular hardware, firmware and software setup, you will need to modify the commands to run, packages to install, and parameters to configure. Even the order in which you run commands can give you different results. In case the steps in this blog do not work for you, take a look at the abovementioned articles and the discussion in the blogs, which can give you some ideas on what to try.

To keep the blog brief, below I will lay out the main steps and refer to the relevant wiki pages where possible.

2. System setup

My hardware setup includes:

Software setup on RPi:

Prepare the tags board:

  • Here is the pdf file of the A3 paper that I used. Depends on your printer setting, the actual size and location of the tags might be slightly different.
  • You can design your own tags, print out, measure the true dimension and modify the layout file accordingly.

Camera calibration:

  • Follow the ROS wiki to obtain the calibration file for your camera.
  • Afterward, replace the camera_info_url param in the launch file with the correct path to the calibration file.

Configure parameters on Mission Planner:

  • Params:

AHRS_EKF_TYPE = 2
BRD_RTC_TYPES = 2
EK2_ENABLE = 1
EK3_ENABLE = 0
EK2_GPS_TYPE = 3
EK2_POSNE_M_NSE = 0.1
EK2_VELD_M_NSE = 0.1
EK2_VELNE_M_NSE = 0.1
GPS_TYPE = 0
COMPASS_USE = 0
COMPASS_USE2 = 0
COMPASS_USE3 = 0
SERIAL5_BAUD = 921 (the serial port used to connect to Raspberry Pi)
SERIAL5_PROTOCOL = 1
SYSID_MYGCS = 1 (to accept control from mavros)
VISO_TYPE = 0

  • For large frame, the offset between the position of the camera and vehicle’s center-of-gravity can be taken into account through VISO_POS params. I have not tried these params myself but you can read more about this in the wiki.

3. Ground test

First, let’s test each ROS nodes separately and fix any problems that might arise:

  • Camera node:
    • On RPi: roslaunch usb_cam usb_cam-test.launch
    • If RPi is not connected to a display, view the raw image on PC with Linux Ubuntu: export ROS_MATER_URI=http://<rpi-ip>:11311 && rqt_image_view
    • Verify that there are images from the camera.
  • MAVROS node:
    • On RPi: roslaunch mavros apm.launch fcu_url:=<tty-port>:<baud-rate>
    • On another terminal: `rostopic echo /mavros/
    • Verify that MAVROS is running OK. For example, /mavros/status should show that FCU is “CONNECTED”.
  • AprilTags node and vision_to_mavros node:
    • Make sure the camera_info_url points to the correct path to your camera’s calibration file.
    • On RPi: roslaunch vision_to_mavros apriltags_to_mavros.launch
    • Open up RViz, view topics /tf and /mavros/vision_pose/pose to the display. With /tf, you should see the camera pose in the tags’ frame, with z-axis pointing downwards. If your camera’s x-axis is pointing to the right, then /mavros/vision_pose/pose will be aligned with body frame. If the camera’s x-axis is pointing in a different direction, you need to modify the params of vision_to_mavros accordingly.

If each node can run without any problem, you can perform ground test:

  • On RPi: roslaunch vision_to_mavros apriltags_to_mavros.launch
  • View the /mavros/vision_pose/pose on RViz. Move the vehicle around and see if the pose changes according to the movement, as demonstrated in the video by @anbello :
  • Additionally, you can verify vision_pose data on MP or QGC Analyse Widget with the topic VISION_POSITION_ESTIMATE.x and y. You should see the data coming in.
  • Before you can use the system, you need to send SET_GPS_GLOBAL_ORIGIN and SET_HOME_POSITION messages. There are a few ways to do this:
  1. On Mission Planner, right click on a point in the map and click Set EKF Origin Here. If data from /mavros/vision_pose/pose is available, the vehicle icon will appear on the map.

  1. Send a SET_GPS_GLOBAL_ORIGIN message, to set the origin for the EKF. You will have to hard-code the lat/long coordinates you wish to send. @anbello has a script that does exactly this. The commands to run this script can be found here.
  • After the origin of the EKF is set, hold the vehicle up, move around and observe the trajectory of the vehicle on MP.

If the last step is successful, you can go ahead with flight test.

4. Flight test

For external navigation data to be accepted by EKF, the data rate needs to be higher than a certain threshold (the number I saw from another discussion is minimum 10Hz, but I have not found the portion of the code to verify this). If you are also using RPi, firstly we need to tune the params related to computational costs to achieve higher detection rate. In my experiments, the RPi can run AprilTags algorithm at 11Hz when the camera is stable but when there are movement, the rate reduced to only 2-3Hz, which is not sufficient for hovering. The related tuning parameters for AprilTags algo are located in settings.yaml. Specifically:

  • Increase tag_decimate will increase dectection rate, at the expense of lower accuracy, and vice versa.
  • Increase tag_threads if you are not running anything else and have some CPU to spare.

Now for the flight tests, similar to what @ppoirier did in his blog on Visual Odometry - ROVIO:

  • Takeoff in Stabilize to check whether the quadcopter is stable.
  • At a height that the camera can have a good view of the tags, switch to Alt-Hold to adjust level position. Observe the feedback on RViz as well as Mission Planner to see if tags are detected.
  • Take a look at MP Map, confirm that AprilTags is still tracking.
  • Switch to Loiter, but always ready to switch back to Alt-Hold if anything goes awry.
  • Otherwise, the quadcopter should hover stably above the tags.

Conclusion

We have replicated the works of @anbello, @ppoirier with a new fiducial tags system, AprilTags. Due to its limited computational power, the RPi is borderline usable for this type of experiments. Even after a lot of parameters are tuned to facilitate stable hovering, there are just too many limitations in terms of detection range and flight movement. If your RPi cannot handle 10Hz or you need to run something else at the same time, or in general the performance of your experiments are not up to your expectation, I would suggest the following solutions:

  • Stream all images data to ground PC, perform all vision-related tasks on ground PC, then send commands back to ArduPilot. Note that the network bandwidth can be the next bottleneck in this case.
  • Upgrade from the RPi to a better single-board computer. Up board, NVIDIA Jetson Nano, Google Coral etc. so many new choices coming out. Here is a nice comparison if you are ready to move on from the Raspberry Pi.
  • Use a VIO sensor that incorporates both hardware and software, perform all algorithms and provide ready-to-use pose data. The new Intel Realsense T265 is a perfect candidate. And the procedure on how to use it with ArduPilot happens to be my 2019 ArduPilot GSoC proposal.

I hope this blog was helpful to anyone who is interested in this topic. As usual, feel free to comment and I am happy to help with what I know. Remember to check out the other blogs mentioned at the beginning for many, many more info that are not covered here.

8 Likes
2

Thanks, @LuckyBird that is a good start for your GSOC project:

3

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.

1 Like
4

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.

5

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 ?

6

Yes, the only problem is little time to dedicate to this stuff, I don’t know when but I will do for sure.

1 Like
7

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.

8

@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)

9

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?

10

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.

Image%20from%20iOS%20(1)
Image from iOS (1).jpg3024Ă—4032 600 KB

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?

11

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.

12

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.

13

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.

14

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.

1 Like
15

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.

16

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).

1 Like
17

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

18

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.

19

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 !

20

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.