Using ZED2i odom, Rtabmap_odom and Rtabmap_slam.

[vivan]

Hello Mathieu, I am using a ZED2i and trying to integrate it properly with Rtabmap. This is my setup: Rtabmap_slam for map, using zed/zed_node/odom for odometry, and running an EKF node from robot_localization to create the transform from odom to base_link. This is working alright, the odometry that I get from the zed seems to be accurate. I am having trouble on the map side as rtabmap_slam is making the map -> odom transform weirdly. Also the /cloud_map that rtabmap is generating seems wrong. For my zed_wrapper, I am running it with pos_tracking:=true and publish_tf:=false.

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    rgbd_sync = Node(
        package="rtabmap_sync",
        executable="rgbd_sync",
        name="rgbd_sync",
        output="screen",
        parameters=[{"approx_sync": True, "queue_size": 3}],
        remappings=[
            ("rgb/image",        "/zed/zed_node/rgb/color/rect/image"),
            ("rgb/camera_info",  "/zed/zed_node/rgb/color/rect/camera_info"),
            ("depth/image",      "/zed/zed_node/depth/depth_registered"),
            ("depth/camera_info","/zed/zed_node/depth/depth_registered/camera_info"),
            ("rgbd_image",       "/zed/rgbd_image"),
        ],
    )


    rtabmap_slam = Node(
        package="rtabmap_slam",
        executable="rtabmap",
        name="rtabmap",
        output="screen",
        parameters=[{
            "frame_id": "base_link",
            "map_frame_id": "map",
            "odom_frame_id": "odom",
            "subscribe_rgbd": True,
            "subscribe_odom_info": False,
            "subscribe_odom": False,
            "publish_tf": True,

            "RGBD/NeighborLinkRefining": "false",
            "RGBD/ProximityBySpace": "true",
            "Kp/MaxFeatures": "800",
            "Vis/MinInliers": "12",
            "Mem/ImageKept": "true",
            "Grid/RangeMax": "8.0",
        }],
        remappings=[
            ("rgbd_image", "/zed/rgbd_image"),
            ("odom", "/zed/zed_node/odom"),
        ]
    )

    return LaunchDescription([rgbd_sync, rtabmap_slam])

ekf_odom:
  ros__parameters:
    frequency: 100.0
    two_d_mode: false
    publish_tf: true
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom
    
    odom0: /zed/zed_node/odom
    odom0_differential: false
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_pose_rejection_threshold: 5.0
    odom0_twist_rejection_threshold: 5.0
    odom0_config: [true,  true,  true,   true,  true,  true,
                   true,  true,  true,   true,  true,  true]
    
    imu0: /zed/zed_node/imu/data
    imu0_remove_gravitational_acceleration: true
    imu0_queue_size: 50
    imu0_nodelay: true
    imu0_config: [false, false, false,  true,  true,  true,
                  false, false, false,  true,  true,  true]
    
    use_control: false

[rtabmap-2] [INFO] [1761867367.117155745] [rtabmap]: rtabmap (14772): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0441s, Maps update=0.0002s pub=0.0000s delay=0.0720s (local map=214, WM=492) Could I get any suggestions for how I could make rtabmap_slam run better? Best, Vivan

[vivan]

Just wanted to update, fixed up a lot of stuff and things are working better now. There is still room for improvement so I would appreciate any suggestions to the config that could make mapping and/or the odometry more accurate.

Here is my setup now:

Rtabmap_slam is working well, rtabmap_odom now instead of using the ZED odometry, rgbd_sync for rtabmap_slam.

Tuned some settings for rtabmap_odom and that greatly increased the odom quality stability. Also set approx_sync in rgbd_sync to false and that helped too (not sure why?).

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    rgbd_sync = Node(
        package="rtabmap_sync",
        executable="rgbd_sync",
        name="rgbd_sync",
        output="screen",
        parameters=[{"approx_sync": False, "queue_size": 5}],
        remappings=[
            ("rgb/image",        "/zed/zed_node/rgb/color/rect/image"),
            ("rgb/camera_info",  "/zed/zed_node/rgb/color/rect/camera_info"),
            ("depth/image",      "/zed/zed_node/depth/depth_registered"),
            ("depth/camera_info","/zed/zed_node/depth/depth_registered/camera_info"),
            ("rgbd_image",       "/zed/rgbd_image"),
        ],
    )

    rtabmap_odom = Node(
        package="rtabmap_odom",
        executable="rgbd_odometry",
        name="rtabmap_odom",
        output="screen",
        parameters=[{
            "frame_id": "base_link",
            "odom_frame_id": "odom",
            "wait_for_transform": 0.2,

            "Vis/EstimationType": "0",
            "Vis/MinInliers": "12",
            "Vis/MaxFeatures": "1500",         # Increased from 1000
            "Vis/PnPReprojError": "5.0",       # Increased from 3.0
            "Reg/Strategy": "0",
            "Odom/Strategy": "0",
            "GFTT/MinDistance": "5",            # If using GFTT detector
            "GFTT/QualityLevel": "0.00001",
        }],
        remappings=[
            ("rgb/image", "/zed/zed_node/rgb/color/rect/image"),
            ("depth/image", "/zed/zed_node/depth/depth_registered"),
            ("rgb/camera_info", "/zed/zed_node/rgb/color/rect/camera_info"),
        ],
    )

    rtabmap_slam = Node(
        package="rtabmap_slam",
        executable="rtabmap",
        name="rtabmap",
        output="screen",
        parameters=[{
            "frame_id": "base_link",
            "map_frame_id": "map",
            "odom_frame_id": "odom",
            "subscribe_rgbd": True,
            "subscribe_odom_info": True,
            "subscribe_odom": False,

            "RGBD/NeighborLinkRefining": "false",
            "RGBD/ProximityBySpace": "true",
            "Kp/MaxFeatures": "800",
            "Vis/MinInliers": "12",
            "Mem/ImageKept": "true",
            "Grid/RangeMax": "8.0",
        }],
        remappings=[
            ("rgbd_image", "/zed/rgbd_image"),

        ]
    )

    return LaunchDescription([rgbd_sync, rtabmap_odom, rtabmap_slam])

[matlabbe]

Hi,

you should definitely set approx_sync=false when the camera topics have all exactly the same stamp, otherwise with approximate sync, it could synchronize the depth image with the wrong color image (which is bad when the camera is moving).

ZED odometry should be fairly good too, it looks like the odometry output was not in the right frame. Did you try zed odometry alone (without EKF) with zed driver publishing the odom TF frame? I think also that th zed odometry may be including IMU already in their estimation, so probably no need for EKF.