[GSoC 2020] Enhancements of GPS-less Navigation and Obstacle Avoidance with Tracking and Depth Camera


1. Introduction

Following previous blog posts, ArduPilot now has as our disposal:

In this blog post, we will combine and enhance the usage of these two cameras leveraging our previous project as well as recent advancements in the ArduPilot’s codebase.

Due to limted access, the experiments of this blog post are constrained to only a few small areas (indoor room and garage) whereas outdoor scenarios would be the most demanding tests and results would be more valuable. The system will continue to be validated in the future when the situation becomes more appropriate.
All testers are welcome :slight_smile:

2. Prerequisite

Onboard computer

  • While for the T265 camera a low-cost RPi3/4 is sufficient (a simple setup instruction for RPi4 can be found here), for the D415/D435 cameras I highly recommend using an x86-based CPU to ensure compatibility with Intel’s libraries and hardware.

  • The Up Squared SoC board is what I am using. For other boards of other architecture (RPi, Jetson etc.), you will likely encounter troubles with installation and running the code.


  • OS: Ubuntu 18.04 - highly recommended.

  • Python 3.6 and above, which is also the standard for Ubuntu 18.04. Check with $ python3 -V, you should see the version, something like Python 3.6.9.


  • Installation of supporting packages:
# Only necessary if you installed the minimal version of Ubuntu
sudo apt install python3-opencv

# For T265 and D4xx script
pip3 install pyrealsense2
pip3 install transformations
pip3 install dronekit
pip3 install apscheduler
pip3 install pyserial # For serial connection
pip3 install opencv-python

# For using both scripts at the same time 
pip3 install dronekit-sitl -UI
pip3 install MAVProxy
  • Download the following scripts or clone the ROS package to keep track of all the updates:
# Clone the package in the ROS workspace or any location if you don't use ROS
cd ~/catkin_ws/src
git clone https://github.com/hoangthien94/vision_to_mavros.git
cd vision_to_mavros/script
chmod +x t265_to_mavlink.py
chmod +x d4xx_to_mavlink.py
chmod +x mavlink_control.py
chmod +x rs_to_mavlink.py
chmod +x opencv_depth_filtering.py

Camera-specific instructions

To avoid repetition, please refer to the following resources for the usage of each camera.

  • T265 tracking camera:

    • Wiki page for the most up-to-date instructions.
    • Blog post to post questions specifically related to the code. For queries about modifications and changes to the code, I suggest creating a new thread to discuss your idea and not dilute the discussion in the main posts.
  • D435/D415 depth camera:

    • Blog post for the current instruction and discussion. Wiki page to come.

3. Enhancements for tracking and depth camera

The following sections will assume the basics are established, meaning you can successfully use each camera and we will only focus on how to improve the cameras’ performance. It is important to keep in mind that each camera has its own set of problems and the troubleshooting would be easier if you get familiar with them separately.

Make sure you are using the latest firmware (at least 4.0) and scripts, otherwise the below features might not be available.

Tracking camera

Using the script t265_to_mavlink.py, the recent enhancements include:

  • Various types of information are now available from the T265’s raw data, most notably: VISION_POSITION_ESTIMATE (default), VISION_POSITION_DELTA, VISION_SPEED_ESTIMATE and more. While the basic VISION_POSITION_ESTIMATE message is sufficient for precise non-GPS navigation, the new delta and speed messages allow more flexibility for the users.

    • To use, first enable the messages in the script here, then set VISO_TYPE = 1 and use EKF3 on AP’s side.
    • Combining T265 with GPS for GPS-non-GPS transition is being actively developed on with this PR, so stay tuned.
  • The maximum data rate for the messages can now be up to 50Hz (as opposed to 15Hz previously):

    • Set the max data rate in the script here. At first use the USB cable to connect the FCU and GSC to make sure the data is being received at the intended frequency.
    • Check again using your system’s telemetry, which would likely show lower/unstable data rate than the setting because of limited bandwidth. The other messages might also get affected (no data appears on Mission Planner).
    • You can use SERIALx_OPTIONS bitmask to disable forwarding all MAVLink messages from the T265 to the GCS to preserve the bandwidth for the other messages (supported with this PR).
  • Using VISO_ advance parameters could improve the performance if your system has:

    • Large position offsets between the T265 camera and the FCU (with VISO_POS_X,Y,Z). Instructions for these params can be found in this wiki. The values do not have to be super precise, but if you would like to methodologically calibrate the offsets I recommend using the Kalibr toolbox.
    • Noticeable time delay with the T265’s data received by the FCU (with VISO_DELAY_MS). Here’s an example of the delay (see the peaks of the T265’s roll-VISP.Roll and the internal IMU’s roll-AHRS2.Roll) of about 10ms:

However, there are still multiple known issues reported from users regarding the T265. More developments are still underway but at the moment it’s best to keep them in mind before/while doing any tests:

  • The position data sometimes does not have the correct scale (i.e. move 1m, reported 0.8m). The temporary solution is to use VISO_SCALE on AP’s side (or scale_factor on the script side). Unfortunately, the issue is not consistent so you need to check this with every flight.
  • The position data can be unreliable sometimes (divergence - go to infinitive, or jump - instantly switch between values) which seem to happen most often at the takeoff and landing phase. Using multiple data types, adding optical flow and range finder, increasing noise level, etc. can help, but nonetheless always be ready to take back control of the vehicle.
  • General issues with the T265 (hardware: USB cable connection, USB port power; software: position drift, velocity unstable etc.). You might want take a quick look at the librealsense issue list.

Depth camera

Tuning for better outdoor performance:

The default settings work well indoor. However, for outdoor experiments I recommended tuning the system until you get reliable detection of obstacle before any actual flight tests.

  • Using the opencv_depth_filtering.py script, you can experiment different settings for the filters.

    • Run the script: python3 opencv_depth_filtering.py
    • Once satisfied with what you see, close this script and change the default filtering values in the main d4xx_to_mavlink.py script.
  • Next, in d4xx_to_mavlink.py script, enable debugging option (debug_enable_default = 1) to see both the input (left) and output (right) images. On the upper right corner is the current processing speed (fps).

    • Run the script: python3 d4xx_to_mavlink.py

    • In Mission Planner, open the Proximity View (Ctrl + F > Proximity). You should see three arcs indicating the distance to the obstacles in front of the cameras. Use the keys (+/-/[/]) and change the sizes to improve the visualization if necessary.

    • Update: In MP version 1.3.73 onwards, the Proximity View has been updated and would look like this:

Note: the proximity view shows the distance to the nearest individual point within each 45 degree arc but only on the obstacle detection line as shown in the output image. As such, sometimes it might seem incorrect what is appearing on the Proximity view but it is simply because we don’t see what ArduPilot is actually perceiving.

Improving the obstacle detection line

As stated above, the horizontal line on the output image (right) indicates the line on which we find the distances to the obstacles in front of the camera. Below are some improvements based on actual usage of the system:

  • When the vehicle is on the ground, it is possible that a large portion of the depth image will see the ground. In such cases, within the d4xx_to_mavlink.py script, reduce the obstacle_line_height_ratio parameter (closer to zero) to move the obstacle detection line up.

  • Additionally, you can increase the thickness of the obstacle line with obstacle_line_thickness_pixel. At the time of this writing, the idea is to process a group of pixels within a certain boundary (defined by this parameter) and find the lowest value to use as indicator to the object. This can change in the future if a better scheme is found.

  • Lastly, the obstacle line will now be kept “fixed” when the vehicle pitches up and down, by compensating for the current pitch angle of the vehicle which is provided by the ATTITUDE MAVLink message.

4. Combining the tracking and depth camera

We provide several scripts to simplify the process of using the tracking and depth cameras simultaneously, be it with ArduPilot or any other systems (SLAM/navigation). The following scripts can be found in the same vision_to_mavros repo.

  • rs_list_info.py: automatically show serial number and available data streams of all connected realsense cameras.
  • calibrate_extrinsics.py: find the extrinsics of the two cameras given a calibrating target (chessboard grid) is present in both cameras’ FOV. The details of the calibration process is outside the scope of this blog, but for anyone interested all the main steps can be found within the script.
  • rs_to_mavlink.py: using MAVProxy to create a software “bridge” between the scripts and the actual hardware serial connection. This way, the T265 and D435 scripts can be used simultaneously as well as seperately without any modifications in either.
    • Within rs_to_mavlink.py script, change connection_in_port and connection_in_baud to the actual hardware connection.
    • Run with: python3 rs_to_mavlink.py. The behaviour (as well as problems) should be the same as if each camera was being used normally.

5. Example of autonomous flight in GUIDED mode

Finally, here is a simple example of autonomous flight with accurate GPS-less navigation (T265 tracking camera) and obstacle avoidance (D4xx camera).

  • In the rs_to_mavlink.py script, find and uncomment the thread4, which will add mavlink_control.py script into the software stack.

The purpose of mavlink_control.py script is to control the vehicle to follow a pre-defined pattern at the flick of an RC switch (activation switch).

  • Within the mavlink_control.py script, change rc_control_channel to any channel that you want to use to activate the autonomous flight (default is 6). The activation threshold as well as the logic might need to be changed depends on how your RC is configured.
  • The autonomous flight will start when the flight mode is switched to LOITER or GUIDED AND the activation switch is on. The procedure is as follows: take off and flight normally; switch to LOITER or GUIDED; then flick the activation switch on.
  • The autonomous flight is as follows:
    1. The vehicle will first attempt to yaw to the zero heading. If all compasses are disabled/not used, then zero heading is relative to the starting orientation. If compass is used, then zero heading is the magnetic north.
    2. The vehicle will flight in a 2m x 2m square pattern, changing the heading as it goes.
    3. Depends on the parameters, you will see the avoidance behaviour if there are any obstacles along the path.
  • If your testing environment does not permit a full square pattern, simply comment out/modify the script as needed to accomodate your conditions.

Below is a short video summarizes the above steps and how a working system should behave:

6. Conclusion and next steps

With both the tracking camera and depth camera at our disposal, the next step would be to use them in a more elaborate manner. Specifically, there are a number of recent open-source systems utilizing both cameras for various high-level tasks such as mapping, path planning and 3D avoidance. Our next target is to adapt a simple, easy-to-replicate system to work with ArduPilot, enabling the pursuit of even more interesting projects in the future.

Useful resources


Hi @LuckyBird, great to see this blog post! One small thing I noticed is MP’s radar view doesn’t seem to be moving the red arcs around properly. I assume this is a Mission Planner issue but normally when I’ve used it the red arcs have moved to indicate the distances to obstacles. Anyway, I’m sure we will get to the bottom of this and we have the new detailed radar view coming along soon anyway.


@rmackay9 yes, that is also my typical experience with Mission Planner. Perhaps in that particular test the specific combination of the vehicle size and window size was an edge case.

Great work as usual!!!
Just one little question i have, if in future (when EKF3 will be default) i add a T265 to my system will it increase precision flight if used together with RTK GPS?
We do a precision photo work and it is crucial for us to have the aircraft be as precise as possible on the route, RTK does a good job, i was just wondering if a T265 could help.


For large scale applications, the T265’s data might not be as reliable as GPS (drift and scale issues will be much more obvious in the long run, example can be seen here and here). I would suggest only recording the T265’s data for several flights, compare it to GPS to see any potential issues before fusing.

Ok, thank you very much. Maybe fusing a flow at the moment is safer.

Hi, great work. Also cool lab! Im testing my drone in my room and it’s dangerous… must rent a better lab :slight_smile: I have ordered the intel real-sense tracking cameras the same as you and want to use the same setup but with Nvidia jetson nano. It’s not an x86 processor. Do you think it’s possible?

As you can read at beginning of blog:

Problem with ARM architecture and GPU /CUDA accelerated is that you have to build most of the stuff and deal with CMAKE and inconsistency with libraires

1 Like

How much power is the Up board using compared to the Jetson Nano?

I was able to install all the libraries on the nano. The only problem was with librealsene but this fixes it https://github.com/jetsonhacksnano/installLibrealsense I Will test the cameras next week and posts my results. Even Apple is going ARM :wink: so I think it will be better if it works.

1 Like

Hi @LuckyBird. Thanks for putting all of this info together! I am interested in reproducing what you have done. Would you mind sharing which specific Up Squared board (and any daughter board) you are using? Also, do you have any recommendations between the two flight controllers below?

  • PX4 Pixhawk 4 FMUv5
  • Pixhawk 2 Orange Cube

Hi @arizonadad, thank you for your interest in the project. The specs of the Up Squared board can be found here. I am using the 4GB RAM + 32GB eMMC storage but the base version should work as well.

Regarding the flight controller, I believe they both suffice to replicate this project. The rest should be up to your vehicle and preference.

Good to know. Thanks!

I would like to install the APsync on Latte Panda with similar specs.What would be my route.Could i use the image for UP2 or i have to install from scratch ?Thanks.

Hi @LuckyBird ,
Great article. Can you share the OS image file for the both cameras working simultaneously so that I can load the os on the up board and get going without going through all the installation steps ? That would make life very easy for a beginner like me.

Raj Patel

Hi @Raj_Patel,

You can follow the ArduPilot wiki to find the image as well as up-to-date instructions on how to replicate this project. Note that as things continue to be developed, some details might be different between the newest image and what is written in this blog.

Hope this helps.

Hi @LuckyBird ,
Thank you for the prompt reply. So, if I understand correctly , the latest image for up board (https://firmware.ardupilot.org/Companion/apsync/beta/apsync-up2-d435i-20201020.tar.xz) should have suppport for both the depth cameras working together ? So it will have these scripts
Pre installed in the os image ?

Also, can we use multiple d4xx cameras with up board ? Like one forward and the other backward . Is the processor capable of handing multiple cameras ?

Raj Patel

Hi @Raj_Patel ,

  • The OS image should support the T265 (tracking camera) and D4xx (depth camera). The other scripts that you listed are not included as they are not essential but you can install them quickly.
  • You can use multiple D4xx cameras with the Up board. UP Board has only 1 USB3 port so you will need a powered UWB hub. Even then, depending on the number of cameras, resolution and framerate, there will likely be a decrease in performance and stability. But it is possible.

You might also be interested in this blog post: GSOC 2020 - Improvements to Object Avoidance, Conclusion

Hi @LuckyBird ,
Thank you. So if I understand correctly , if I want to use 2 d4xx cameras, I’ll have to run 2 separate instances of d4xx_to_mavlink.py, one for each camera ? How can we assign the specific camera to this script(does every camera have a unique id) ?Also, what would be the orientation parameter for a backward facing camera in the script ? The script rs_to_mavlink.py will also need to be modified for additional cameras ?

Raj Patel