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