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