Map distortion/bending in straight hallway during loop closure (RTAB-Map RGB-D + IMU on Unitree Go2)

[CGV_lee]

Hi,

I am working on 3D Visual-Inertial SLAM using RTAB-Map on a Unitree Go2 quadruped robot equipped with an Intel RealSense D435i camera and NVIDIA Jetson Orin (16GB RAM).

--System Environment
- OS: Ubuntu 20.04 (Jetson Linux)
- ROS Version: ROS 2 Foxy
- RTAB-Map: `rtabmap_ros` (foxy-devel) compiled from source with OpenCV CUDA support
- Camera: Intel RealSense D435i (RGB-D + IMU at 640x480 @ 30fps)
- IMU Filter: `imu_filter_madgwick` (`gain:=0.02`, `zeta:=0.001`, orientation aligned to ROS body frame `base_link` / `camera_link`)
- Mode: Pure Visual-Inertial Odometry (Pure VIO without wheel/leg odometry guess)

---

Problem Description
When mapping a straight indoor corridor (walking back and forth), the resulting map and trajectory split into a V-shape instead of aligning into a single straight corridor:

1. V-Shape Trajectory Drift: On the return path, the trajectory drifts laterally and forms a V-shape split, creating a duplicated "double corridor".
2. Graph Optimization Behavior: As shown in the attached screenshot, yellow loop closure links ARE detected between the outward and return paths. However, graph optimization does not pull or align the return path onto the forward path, leaving the corridor bent and split at the end.

---


The Graph View shows yellow loop closure links connecting the forward and return paths, but the hallway splits into two angled corridors on the left.

---
Current Launch Command

```bash
ros2 launch rtabmap_launch rtabmap.launch.py \
    rtabmap_args:='--delete_db_on_start \
                  --Vis/FeatureType 10 \
                  --Kp/DetectorStrategy 10 \
                  --ORB/Gpu true \
                  --FAST/Gpu true \
                  --Kp/MaxFeatures 1000 \
                  --Vis/MinInliers 15 \
                  --Optimizer/GravitySigma 0.3 \
                  --Grid/MaxObstacleHeight 1.5 \
                  --Grid/CellSize 0.1 \
                  --Grid/RayTracing true' \
    frame_id:=base_link \
    visual_odometry:=true \
    publish_tf_odom:=true \
    odom_topic:=/odom \
    subscribe_imu:=true \
    imu_topic:=/imu/data \
    approx_sync:=true \
    odom_approx_sync:=true \
    qos:=1 \
    rviz:=true