Drift and Odometry Issues with RTAB-Map (ROS2)

[rob_slam]

Hi everyone,

I’m working on a project (using an Oak-D-S2 (AF) camera) to create a map with RTAB-Map (ROS2), but I’ve been facing some issues that I’m struggling to resolve:

1. Drift in the z-direction:
The generated map constantly has a slope in the z-direction, even though the real environment is flat. I can bypass this by setting the parameter 'Reg/Force3DoF': 'True', but I would like to record gradients and similar terrain features in the future (i.e., without forcing 3DoF mode). Does anyone have suggestions on how to eliminate the z-drift without restricting to 3DoF?

2. Odometry inaccuracies:
During my ROS Bag recording, I used the same start and end point. However, the resulting map shows discrepancies—e.g., a building with only 90° corners is represented with angles >90°, causing the start and end points to misalign and producing an incorrect map.

I have limited experience with VIO and SLAM algorithms like RTAB-Map, so I’d be grateful for any advice, including parameter adjustments, optimization tips, or general recommendations.

Thank you in advance for your help!
Best regards.

ROS-BAG: https://drive.google.com/file/d/1BQAxkD3iKE98WSCGJPNZdq4SXUEqOLcN/view?usp=sharing

Screenshot_from_2024-12-12_21-51-07.png
Screenshot_from_2024-12-12_21-51-28.png
Screenshot_from_2024-12-12_21-50-22.png

[matlabbe]

Hi,

1. It could be related to some error between the base frame of the camera and the imu frame. I tried your rosbag with this:

ros2 launch rtabmap_launch rtabmap.launch.py \
   rgb_topic:=/left/image_rect \
   depth_topic:=/stereo/depth \
   camera_info_topic:=/left/camera_info \
   imu_topic:=/imu/data args:="-d" \
   use_sim_time:=true \
   frame_id:=oak-d-base-frame \
   approx_sync:=true \
   approx_sync_max_interval:=0.002 \
   wait_imu_to_init:=true

ros2 run imu_filter_madgwick imu_filter_madgwick_node --ros-args -r imu/data_raw:=/imu -p use_mag:=false -p publish_tf:=false -p use_sim_time:=true

ros2 bag play THU_W_01_0.db3 --clock

Without graph optimization (ignoring gravity links):


With graph optimization (with gravity links):


With and without gravity links respectively:


It seems with gravity links, the error along z-axis is almost null, so the error may be coming from camera->imu TF frame. See Kalibr camera-imu for example.

2. This drift is expected. Ideally, when finishing your loop, rotate the camera in same orientation than at the beginning to detect the loop closure and correct the map.

cheers,
Mathieu

[Parastou Bakhtiari]

Hi,

Thank you for your great explanation.

I played this rosbag and launched rtabmap the way you said.
How did you reach these images http://official-rtab-map-forum.206.s1.nabble.com/file/n10764/Peek_2024-12-14_12-48.gif?
I can see the graph view but not the 3D map you sent. is there any specific prefrences or views to get the map?

[matlabbe]

That gif is from the 3D Map view in rtabmap_viz. Normally the 3D point cloud and the graph (cyan) should be shown by default. You can enable the 2D occupancy grid under Preferences->Grid Map Assembling. You can enable gravity lines (yellow) under Preferences->3D Rendering->scroll all way down to enable "Show IMU filtered gravity." under Map column.