RTAB-Map launch file for zed2i

[Parastou Bakhtiari]

Hi,

I have a rosbag recorded using a zed2i camera. I launched this lunch file https://github.com/stereolabs/zed-ros-examples/blob/master/examples/zed_rtabmap_example/launch/zed_rtabmap.launch, but I only can see the odomety in localization mode. I want to get the 3D pointcloud map and 2D occupancy grid map. Would you please tell me where can I find the correct launch file? I have shared the rosbag in this shared google drive and I have attached the tf frames of this rosbag for your consideration.

Thank you.
https://drive.google.com/drive/folders/1u5mcmOMRJbasoJY9JD-E07zpUv2Dp5T5?usp=sharing

[matlabbe]

Hi,

There are a couple of issues make it difficult to get accurate visual odometry.
 * The camera gets occluded, which can cause the algorithm to lose track of the features

 * The depth image has sometime very wrong disparity

Other system issues:
 * The TF looks wrong, so the base_link frame is tilted 90 degs up by visual odometry

Here is an example running rtabmap on that bag. Note that to make it work, you have to remove "vio/map" frame from the bag (to do so, you can use this script):

roslaunch rtabmap_launch rtabmap.launch \
   args:="-d" \
   visual_odometry:=false \
   odom_topic:=/zed2_front/zed_node/odom \
   rgb_topic:=/zed2_front/zed_node/left/image_rect_color \
   camera_info_topic:=/zed2_front/zed_node/left/camera_info \
   depth_topic:=/zed2_front/zed_node/depth/depth_registered \
   frame_id:=base_link \
   approx_sync:=false rgbd_sync:=true \
   approx_rgbd_sync:=false \
   rgb_image_transport:=compressed \
   compressed:=true \
   depth_image_transport:=raw \
   use_sim_time:=true

rosbag play --clock output.bag

There are some jumps caused by camera occlusions and very bad depth:



Here two other examples not using zed odometry, one using RGB-D mode and the other stereo mode:

roslaunch rtabmap_launch rtabmap.launch \
   args:="-d" \
   visual_odometry:=true \
   rgb_topic:=/zed2_front/zed_node/left/image_rect_color \
   camera_info_topic:=/zed2_front/zed_node/left/camera_info \
   depth_topic:=/zed2_front/zed_node/depth/depth_registered \
   frame_id:=zed2_front_base_link \
   approx_sync:=false \
   rgbd_sync:=true approx_rgbd_sync:=false \
   rgb_image_transport:=compressed \
   compressed:=true \
   depth_image_transport:=raw \
   use_sim_time:=true \
   imu_topic:=/zed2_front/zed_node/imu/data \
   wait_imu_to_init:=true

rosbag play --clock tf:=tf_not_used ts_2023_06_23_21h04m38s_one_random_row.bag

roslaunch rtabmap_launch rtabmap.launch \
   args:="-d" \
   stereo:=true \
   visual_odometry:=true \
   left_image_topic:=/zed2_front/zed_node/left/image_rect_color  \
   left_camera_info_topic:=/zed2_front/zed_node/left/camera_info \
   right_image_topic:=/zed2_front/zed_node/right/image_rect_color \
   right_camera_info_topic:=/zed2_front/zed_node/right/camera_info \
   frame_id:=zed2_front_base_link \
   approx_sync:=false \
   rgbd_sync:=true \
   approx_rgbd_sync:=false \
   compressed:=true  \
   use_sim_time:=true \
   imu_topic:=/zed2_front/zed_node/imu/data \
   wait_imu_to_init:=true

rosbag play --clock tf:=tf_not_used ts_2023_06_23_21h04m38s_one_random_row.bag

In both cases, VO gets lost when there is a camera occlusion or when the depth image is very wrong.

[Parastou Bakhtiari]

Hi Mattlabee, Thank you for your reply.

I assume I did the exact same thing you mentioned here.
I used this python code to remove vio/map:
import rosbag
from tf.msg import tfMessage
import argparse

# Parse command-line arguments
parser = argparse.ArgumentParser(description='Remove vio/map frame from a ROS bag.')
parser.add_argument('--input', required=True, help='Input ROS bag file.')
parser.add_argument('--output', required=True, help='Output ROS bag file.')
args = parser.parse_args()

# Process the bag file
with rosbag.Bag(args.output, 'w') as outbag:
    for topic, msg, t in rosbag.Bag(args.input).read_messages():
        if topic == "/tf" and msg.transforms:
            filteredTransforms = []
            for m in msg.transforms:
                if m.header.frame_id != "vio/map":  # Check for vio/map instead of map
                    filteredTransforms.append(m)
                else:
                    print('vio/map frame removed!')
            if len(filteredTransforms) > 0:
                msg.transforms = filteredTransforms
                outbag.write(topic, msg, t)
        else:
            outbag.write(topic, msg, t)

print(f"Processing complete. Output saved to {args.output}")<quote author="matlabbe">

but launching gives me this error:
[ WARN] [1742683066.303884525, 1685653489.214015925]: Could not get transform from base_link to zed2_front_left_camera_optical_frame after 0.200000 seconds (for stamp=1685653488.912728)! Error="canTransform: source_frame zed2_front_left_camera_optical_frame does not exist.. canTransform returned after 0.202284 timeout was 0.2.".
[ERROR] [1742683066.303978734, 1685653489.214015925]: TF of received image 0 at time 1685653488.912728s is not set!
[ERROR] [1742683066.304027361, 1685653489.214015925]: Could not convert rgb/depth msgs! Aborting rtabmap_viz update...

Would you please tell me what can I do?

[matlabbe]

In my example I was using zed2_front_base_link instead of base_link, don't remember if because there was a missing link in the TF tree causing your error. You may check how all the TF frames are connected in the TF tree.

[Parastou Bakhtiari]

Thank you, Matlabbe.

Would you please tell me how can I get the trajetories and the 3D map from RTAB-MAP, I can visualize data in GUI now.

[Parastou Bakhtiari]

The export items are not active.

[matlabbe]

With rtabmap-databaseViewer, these options are under the File menu. Some may be grayed out if the Graph View is not shown, so show the View->Graph View and all options should be enabled.

[Parastou Bakhtiari]

Sorry to ask so many questions.
I enabled the graph view, but still the export options under File menu are gray.
Is there something wrong or any other way?

[matlabbe]

Can you see images with the main slider?

Actually I just re-tested and the File menu options should be enabled with or without Graph view shown. If the "Export 3D Map" option is disabled, it is because no database is opened.

[Parastou Bakhtiari]

what I actually did was to use this launch:
roslaunch rtabmap_launch rtabmap.launch \
   args:="-d" \
   stereo:=true \
   visual_odometry:=true \
   left_image_topic:=/zed2_front/zed_node/left/image_rect_color  \
   left_camera_info_topic:=/zed2_front/zed_node/left/camera_info \
   right_image_topic:=/zed2_front/zed_node/right/image_rect_color \
   right_camera_info_topic:=/zed2_front/zed_node/right/camera_info \
   frame_id:=zed2_front_base_link \
   approx_sync:=false \
   rgbd_sync:=true \
   approx_rgbd_sync:=false \
   compressed:=true  \
   use_sim_time:=true \
   imu_topic:=/zed2_front/zed_node/imu/data \
   wait_imu_to_init:=true

and play the bag
rosbag play --clock tf:=tf_not_used ts_2023_06_23_21h04m38s_one_random_row.bag

then I can see the GUI opensfile:///home/asaas/Pictures/Screenshot%20from%202025-03-23%2014-21-13.png
this is the mapping mode. this is the File menu file:///media/asaas/DATA/jose/documents/extract_data/data/Image.jpeg

[Parastou Bakhtiari]

[Parastou Bakhtiari]

Btw, I get this in terminal when launching.
[ INFO] [1742757506.261714983, 1685653568.649166305]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.068524s, delay=0.133632s
[ WARN] (2025-03-23 14:18:26.314) Features2d.cpp:971::generateKeypoints3D() A large number (693/946) of stereo correspondences are rejected! Optical flow may have failed because images are not calibrated, the background is too far (no disparity between the images), maximum disparity may be too small (128.000000) or that exposure between left and right images is too different.
[ WARN] (2025-03-23 14:18:26.332) OdometryF2M.cpp:569::computeTransform() Registration failed: "Not enough inliers 6/20 (matches=80) between -1 and 1062" (guess=xyz=0.000000,0.000000,0.000000 rpy=-0.067908,-0.003609,0.067692)
[ WARN] (2025-03-23 14:18:26.332) OdometryF2M.cpp:317::computeTransform() Failed to find a transformation with the provided guess (xyz=0.000000,0.000000,0.000000 rpy=-0.067908,-0.003609,0.067692), trying again without a guess.
[ WARN] (2025-03-23 14:18:26.370) OdometryF2M.cpp:559::computeTransform() Trial with no guess still fail.
[ WARN] (2025-03-23 14:18:26.370) OdometryF2M.cpp:569::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=48) between -1 and 1062" (guess=xyz=0.000000,0.000000,0.000000 rpy=-0.067908,-0.003609,0.067692)
[ INFO] [1742757506.371335765, 1685653568.749840820]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.075885s, delay=0.134306s
[ERROR] (2025-03-23 14:18:26.373) Rtabmap.cpp:1376::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 42125 is ignored!
[ INFO] [1742757506.373380641, 1685653568.759946023]: rtabmap (8): Rate=1.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.0000s, Maps update=0.0000s pub=0.0000s (local map=7, WM=7)

[Parastou Bakhtiari]

I used this command in terminal:
rtabmap --database ~/.ros/rtabmap.db

I attached the image showing the GUI.
now in this new GUI. I can see export 3D map and export poses option under File menu.

[matlabbe]

In rtabmap_viz, the processing should be paused to enable the File menu options. It is however better to do it offline:

rtabmap-databaseViewer ~/.ros/rtabmap.db