Hi all,
I was using an Intel T265 tracking camera to provide VISION_POSITION_ESTIMATE to ArduPilot (via mavros / vision_to_mavros) so the autopilot could run POSHOLD in GPS-denied areas. The T265 is discontinued for me now and I have an Intel RealSense D435i. I’m looking for the best way to get the D435i to provide the same pose input so ArduPilot can hold position (POSHOLD). Below is what I’ve tried/planned and some questions.
Key facts / constraints
D435i is an RGB-D depth camera with an IMU, but it does NOT output a ready-made pose stream (unlike the T265). You must run a VIO/VO/SLAM node that fuses the D435i images ± IMU to produce a pose/odometry topic.
ArduPilot expects external position via MAVLink VISION_POSITION_ESTIMATE (or GLOBAL_VISION_POSITION_ESTIMATE) — mavros will take a ROS PoseStamped/PoseWithCovarianceStamped on /mavros/vision_pose/pose, convert frames if necessary, and forward the MAVLink message. You should also publish the global origin/home via MAVROS so ArduPilot can map local coordinates → lat/lon.
Which VIO method gives stable POSHOLD after T265?
I was using RTAB-Map odometry but it drifts and does not give stable position hold.
I want suggestions for a stable VIO pipeline with a D435i that outputs pose to ArduPilot via MAVROS (/mavros/vision_pose/pose).
Should I switch to OpenVINS or VINS-Fusion? Or use robot_localization to fuse RTAB-Map odometry with IMU?
Looking for the most stable solution for GPS-denied indoor flight using ArduPilot. @rmackay9
Hi @Manoj_Kashyap ! So for the D435i and VIO, take a look at ORB-SLAM3. It has a good example setup for it and works well. You can also use VINS-Fusion and it can be more robust if properly calibrated. The main hurdle overall with VIO will be the IMU and camera calibrationI(use kalibr). That is a pain (the onboard IMU simplifes a bit) but you’ll find tutorials for it in both projects. Also, since these are odometry sources, they will inevitably drift over time unless corrected.
Rtab-map’s odometry is not that good to be used directly. RTAB-Map or ORB-SLAM3 (stereo-only mode) are good if you are looking for SLAM solutions that are much simpler (avoiding the inertial calibration hassle) and are straightforward for ROS.
And for sending data to AP, after you have your pose / odom, you can use MAVROS 2 or using the AP’s native DDS interface. ROS 1 has reached EOL.
If looking for alternative to T265, you can look for ZED cameras / OAK-d as their SDK likely supports pose estimation)
To use Kalibr , I have to use Docker since Kalibr is not present in ROS2 , If u have any resource to setup Kalibr in a Ubuntu22.04 system .
Let me know. @snktshrma
For a unified calibration, docker is the only solution as far as I can think of with Kalibr. There are other open source extrinsic calibration packages, inspired from kalibr, but not as good, as far as I know.
What you can use is packages like Allan_ros2 and camera_calibration toolkit for independently calibrating IMU Allan params and camera intrinsics respectively. But for extrinsic, either use Kalibr in docker or can rely on VINS-Fusion’s internal estimator but I never had good outputs from those unless there is a close initial guess of extrinsics.
Though you can also find Realsense documentation or resources available to get extrinsic of camera and IMU (should be available already) and can calibrate each sensor independently.. So probably won’t need kalibr.
If you mean reusing a map that you got using rtab map for localising from subsequent flight, then yes! It will localise itself though matching. Look for ros parameter sets associated with localisation in rtabmap
I have done the calibration and the drift is less using handheld right now but when i am doing a little bit yaw in handheld its getting error compass variance in ek3srcyaw
is it due to the imu of D435if camera or i have to tuned more ardupilot ek3 to accept covariance?
Any suggestions @snktshrma
Hi @snktshrma,
I’ve attached my log file in this reply.
I’m also facing one more issue related to AltHold.
I’m using a downward-facing RealSense D435i (IR stereo) to compute depth and publish it as a rangefinder to the flight controller. Most of the time AltHold works well and the rangefinder data quality looks good in the logs. But occasionally the rangefinder quality drops suddenly and the EKF switches back to barometer for altitude.
Could you guide me on which parameters I should tune so that the EKF trusts the rangefinder more consistently, and avoids falling back to baro unless necessary?
For the rangefinder part, try increasing EK3_RNG_I_GATE and also reduce EK3_RNG_M_NSE (secondary, first increase measurement gate ; this affects weighting, not the quality threshold that most likely is causes switching.)
Hi @Tristan_Redish You can absolutely get it working and is being used a lot these days. Go through this:
And follow the same procedure as described above in this post using the mavlink/ros interface to send positional data to Ardupilot. You might need to do some frame conversions, but overall, it’s feasible.
You can use it along side NAV2 if planning over ROS and doing off board path planning but ya, follow the link.
Hello @snktshrma I used External Vision as a EK3 SRC POSZ i am getting a deviation of 0.5m deviation when i am doing guided takeoff .
when i used rangefinder it was perfect .
Is this an issue like i have to set any thing to make it correct or External Vision behaves like this only
@Manoj_Kashyap Is the deviation static? Most likely just the lateral transformation error between camera frame and body frame or wrong estimation when landed(stereo too close to ground). If it’s variable, most likely the fused state being noisy, due to IMU and Stereo fusion(maybe check process noise / measurement noise matrix). Can you elaborate more on your setup? And I am yet to look at your logs. Will check it asap.