Precision Landing with Realsense T265 Camera and AprilTag - Part 1/2

Blog
, ,
1

1. Introduction

The GSoC 2019 project has added to ArduPilot’s arsenal full support for the Intel RealSense Tracking Camera T265 to be used as a plug-and-play Visual-Inertial Odometry (VIO) sensor to achieve accurate localization and navigation. Hence freeing up resources for the onboard computer to utilize for other high-level tasks, which would pave the way for even more amazing applications to be built upon.

We now have a suite of sensors at our disposal:

  1. Standalone 6-dof (200Hz)
  2. Accel (200Hz) & Gyro (62.5Hz)
  3. Dual fisheye monochrome (8-bit) streams @ 848x800 30FPS
    (For additional info, here is the device’s datasheet).

In this project, we wil try our hand at a very old topic: precision landing. In a nutshell, image stream from one of the T265’s cameras will be processed to detect AprilTag visual marker, then we will follow MAVLink’s Landing Target Protocol that is supported by ArduPilot to perform precision landing. We will do it without (part 1, this blog) and with Robot Operating System (ROS) (part 2).

Let’s get started.

2. Prerequisite

Hardware

  • A copter frame of choice.
  • An onboard computer capable of polling image data from the T265, thus USB3 port is required. The Up board is what I am using, but the Jetson Nano or RPi 4 are also suitable alternatives for low budget setup. Unfortunately, the Raspberry Pi 3 which was used in previous blog posts is not suitable for this project.
  • A downward facing Realsense T265 camera, with USB port pointing to the right of the vehicle. Different camera orientations can be used as well, however some modifications are involved.

Software

3. How does it work?

How to perform localization and navigation

The main script t265_precland_apriltags.py is built on top of the existing t265_to_mavlink.py script, the content of which is thoroughly explained in this blog post, so check it out for explainations on how to attain accurate localization and navigation.

How to perform precision landing

Here’s an overview of the steps needed to add precision landing capabilities to our system:

  • First, we follow the t265_stereo.py example to optain the undistorted images from T265 fisheye images, intrinsic and extrinsic params in OpenCV.
    Raw
    Undistorted

  • Next, AprilTag detection algorithm is run on the undistorted images to obtain the tag’s pose relative to the fisheye lens. We will use the duckietown’s Python wrapper for AprilTag 3 to ease up the workflow in Python.
    Tag Detected

  • If the landing tag is detected in the image, precision landing is performed following MAVLink’s landing target protocol. For ArduPilot, only LANDING_TARGET fields relative to the captured image are supported.

Note: It is implicitly assumed that the camera’s x axis is aligned with the vehicle’s y axis (to the right) while the camera’s y axis aligned with the vehicle’s -x (to the back). This is the same as pitching the camera down 90 degrees from facing forward. If your camera is oriented differently, then a transformation is mandatory. For reference, here’s the T265’s coordinates (note the fisheye’s axes), whereas the FCU’s local frame is (X Forward, Y Right and Z Down).

4. Instructions

Prepare the tag

We will use the tag image as provided in rs-pose-apriltag sample in librealsense. Here is the link for the pdf.

Print the pdf file above on a letter size paper. The tag size (the black part of the tag) on the paper should be 0.144m. Fix the tag to some landing pad or fix the tag to the ground. Make sure the tag won’t get blown away when the copter is descending.

It goes without saying that the tag size and id can be anything according to your requirements. All you need to do is modify the corresponding params, which we will discuss later on.

A stupendous amount of other tag options can be found here.

Install AprilTag 3 library

Clone the official repo and install the library with:

cd ~
git clone https://github.com/AprilRobotics/apriltag.git
cd apriltag
cmake .
sudo make install

The default installation will place headers in /usr/local/include and shared library in /usr/local/lib. Python wrapper will also be installed if python3 is installed.

Install other required packages

pip3 install transformations
pip3 install dronekit
pip3 install apscheduler

Download the scripts

cd /path/to/some/directory
wget https://raw.githubusercontent.com/duckietown/apriltags3-py/master/apriltags3.py
wget https://raw.githubusercontent.com/thien94/vision_to_mavros/master/scripts/t265_precland_apriltags.py

chmod +x t265_precland_apriltags.py

Modify the main script t265_precland_apriltags.py

  • If you have sucessfully followed the VIO wiki, all of the params that you use in the t265_to_mavlink.py script can be brought over.

  • The following new params are related to AprilTag detection:

    • tag_landing_id, default is 0. We will only land on the tag with this id.
    • tag_landing_size, default is 0.144. This is the tag’s border size, measured in meter. Change it to your specific tag setup. Reminder: for AprilTag, the tag size is the dimension of the black part of the tag.
    • tag_image_source, default is "right". For the Realsense T265, we can use left or right camera. You might find it useful to choose the camera with the least obstruction in its view, for example the landing gear.
    • landing_target_msg_hz_default, default is 20. This is the rate at which the LANDING_TARGET MAVLink message will be sent to the flight controller. As noted in the MAVLink wiki, the rate depends on the landing speed and desired accuracy; start with rates between 10 Hz and 50 Hz and tune performance as needed.

Viewing the AprilTag detection image

If you have a monitor connected to the companion computer:

  • Run the script with visualization option enabled:
    python3 t265_precland_apriltags.py --visualization 1
  • A window will pop up, showing the already processed image. If the tag is in the image’s view, there will be the tag’s id and a bordering rectangle in the image as well.

If you don’t have a monitor connected:

You can view the image through the ssh connection with X11 Forwarding.

  • First, enable desktop sharing option on the companion computer, here’s an example. Depends on your setup, you might need to tinker around a bit.
  • Next, ssh to the companion computer from your host computer. Work best with Ubuntu host computer.
    ssh -Y <username>:<ip>
  • Now you can run the script with visualization option enabled:
    python3 t265_precland_apriltags.py --visualization 1

ArduPilot params

  • The params required to facilitate VIO flight are documented in this wiki.
  • Set the following parameters through Mission Planner (or other GCS) to enable the precision landing feature and then reboot the flight controller.
    PLND_ENABLED = 1
PLND_TYPE = 1
  • Setup the vehicle with one of the flight modes to LAND.

Run the script

  • For serial connection: set udev rules in order to get the USB available and allow permission to serial:

    sudo chmod 666 /dev/ttyUSB0

  • When all params are set correctly:

    python3 t265_precland_apriltags.py

5. Ground and Flight test

Ground test

Hold the vehicle on top of the landing tag and check on your GCS:

Verify that ArduPilot is receiving landing_target data

  • In the ssh terminal: when the tag is in the camera’s view, there will be a stream of messages begin with INFO: Detected landing tag followed by the tag’s position in the camera’s frame.
  • Check that ArduPilot is receiving position data by viewing the topic LANDING_TARGET on GCS. For Mission Planner, press Ctrl+F and click on MAVLink Inspector, you should be able to see data coming in under Vehicle 1.

Verify that ArduPilot is receiving pose data

  • Similarly, check that the message VISION_POSITION_ESTIMATE is coming through to the flight controller.

Verify that ArduPilot is receiving confidence_level data

  • View the topic VISION_POSITION_DELTA on GCS, field confidence should show the remapped value of confidence level. The confidence level should start at Medium and change to High after

Flight test

  • For downfacing configuration of the Realsense T265, there is a problem with inconsistent yaw angle between runs. The temporary fix is to tilt the nose up a little, so that the camera is not facing flat down, when the script starts. Once the FCU starts receiving vision messages, the vehicle can be put flat on the ground.
    Initialization
  • Take-off and hover above the target.
  • When target is detected (see above about verifying that ArduPilot is receiving landing_target data), you can switch the vehicle to LAND.
  • If everything is working properly, the copter should move towards, then land on the tag.
  • As usual, be prepared to retake control if there are sudden unexpected movements (Change mode to Stabilize, AltHold or Loiter).
  • If all is well, some successful tests can be seen below:

6. Next time

We will explore how to achieve the same goal in ROS. Thanks to ROS’s modularity, the software components are really easy to learn and modify. However, not everything is ready-to-go for our use case, and some developments are still required on our part.

7. Useful links

8 Likes
2

It looks great, well done😊

1 Like
3

Yeah, really great! Fantastic detail!

1 Like
4

HI @LuckyBird

Amazing work!
I’ve been trying to implement this but I’m getting an error
TypeError: landing_target_encode() takes 9 positional arguments but 15 were given

Do you have any Idea why am I getting this?
Thanks!

5

okay this was solved by updating the firmware.

I have one more question. Why do I have to set PLND_TYPE = 2 (irLock) instead of 1(Companion Computer) as discussed here How to use Precision Landing feature with companion computer??

6

Hi @Amitesh_Gautam, you are right, it is a typo and should be PLND_TYPE = 1 instead. Thank you for pointing this out!

Edit: the typo has been fixed. For anyone looking for the meaning of different values for PLND_TYPE, see here.

1 Like
7

What’s the range of the t265 implementation? I’m trying to get an accurate non-gps loiter outdoors at around 150 feet.

8

My experiment have demonstrated a good tracking up to 60 feets. (see details in the threads of the non ROS implementation blog from @LuckyBird.
Beyound that, you need bigger and very well defined features with an optimum scene lightning (not too dark and not too bright).

1 Like
9

SharedScreenshot
SharedScreenshot1093×651 114 KB

Here’s an example of my use case. Where you see that black square on the bow would be the landing area (that’s not my boat, just one I plucked off google).

10

Hi @LuckyBird, I’m a little bit confused what is stereo matching here for if detection procedure only use one camera? BTW it would be better to use both

11

Hi @soldierofhell, you are right: to detect the target we only need one camera, thus the stereo matching is redundant. It would only become useful if stereo depth is needed for your application. As a blog post it would be fine to be a bit more general for other use cases, thus I have decided to keep the stereo matching part in the code. You can, of course, remove any parts as you wish.

Regarding the use of the two cameras at the same time: for the T265 has a very narrow stereo baseline (~5cm) the overlapping area between the rectified images is quite large. At landing altitude in my experiments (1m-5m), both of the cameras usually see the target at the same time, so using any of the two cameras would make no difference.

On the other hand, if you can make full use of the field-of-view (~163°) of the cameras, using both will make a lot of sense in that case.

12

Have a look at my use case above. I’m real interested in your precision landing work.

13

Hi @moonman,

If you are only concern about the precision landing task (using camera images and computer vision algorithm), I think landing on a boat is doable since the performance will mainly depend on the image and you can test that out straightforwardly. The detectable range will vary based on the size of the landing target, the illumination of the scene, the relative speed between the target / copter etc. For a static target, you can test the range with different paper sizes and extrapolate.

The precision flight (using VISLAM pose data), on the other hand, might not work well, or at all, because of the challenges that @ppoirier mentioned.

14

@LuckyBird I thought rather about combining two methods od distance calculaton: from known shape as in april tags detector and stereo distance. Especially in application like landing on this boat it might be of benefit

15

For flight I could probably just use gps follow and have a good gps on both the uav and gcs.

16

Because of high reflectivity of water and the lack of fixed features, I would recommend using IR-LOCK:

https://ardupilot.org/copter/docs/precision-landing-with-irlock.html

17

Hi, @LuckyBird, many thanks to your detailed introduction for your work!
I am following these setps to realize the precision landing with Ardupilot and a Jetson Nano as the companion computer, but I met some troubles and need your help.
When I start the script t265_precland_apriltags.py, it works OK in the first few tens of seconds. I can see the messages (landing_target, pose estimation, delta) in MAVLINK inspector, and there is a quadrotor icon in the map.
But few tens of seconds later, when I shake my drone, the quadrotor drone is disappear!, however, I can still watch the messages in MAVLINK inspector.
Do you know what caused this, Looking forward to your reply…
THANK YOU.

Snipaste_2020-06-20_17-34-27
Snipaste_2020-06-20_17-34-271388×833 172 KB

18

@Andrew5c maybe the connection (usb cable to the camera/ telemetry to the ground station) become unstable when you shake your drone. In the picture, the other messages also dropped to 0 Hz, which indicates that the telemetry might be the main problem.

If possible, can you use a usb cable to connect the flight controller and the ground station for telemetry, do the shaking again and see if the problem persists.

19

Yes, thanks, @LuckyBird, that’s probably the problem.
I used a USB cable to connect the pixhawk with my computer GCS just now, and done the same thing, shaked my drone, and the quadrotor icon is showing good. Unfortunately, there were still once time(1/3) the quadrotor disapeared. But overall, things are much better than before.
Thanks for your timely reply!

20

This project is very useful to me，and it works in my plane(F450 Pixhawk2.4.8, APM3.6.11).

But when I ported this part of the code, there was an error. I did not know the MavLink protocol very well. May I ask why?

File “apm_startup.py”, line 260, in
main()
File “apm_startup.py”, line 240, in main
1, # position_valid boolean
TypeError: landing_target_encode() takes 9 positional arguments but 15 were given