rtabmap ros2 zed2i

[loleklel]

Hello,

I just want to get running my zed2i camera (only this device for now) and slam rtabmap. Are there maybe any other options to get SLAM running for this setup?

my setup:
ubuntu 22.04, ros2 humble, zed2i
ZED SDK 4.1.3, CUDA 12.6, Nvidia Driver 560.28.03
last branch rtabmap
last branch zed_wrapper

I tried to launch:
ros2 launch rtabmap_launch rtabmap.launch.py and
ros2 run rtabmap_slam rtabmap --ros-args -r odom:=/zed/zed_node/odom -r rgb/image:=/zed/zed_node/rgb_raw/image_raw_color -r depth/image:=/zed/zed_node/depth/depth_registered -r rgb/camera_info:=/zed/zed_node/rgb/camera_info

and still get errors:
[ WARN] (2024-08-08 14:35:40.222) MsgConversion.cpp:1994::getTransform() (can transform odom -> base_link?) Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist. canTransform returned after 0.20125 timeout was 0.2. (wait_for_transform=0.200000)
[WARN] [1723120540.222579218] [rtabmap]: We received odometry message, but we cannot get the corresponding TF odom->base_link at data stamp 1723120539.975974s (odom msg stamp is 1723120539.975974s). Make sure TF of odometry is also published to get more accurate pose estimation. This warning is only printed once.
[ WARN] (2024-08-08 14:35:40.424) MsgConversion.cpp:1994::getTransform() (can transform base_link -> zed_left_camera_optical_frame?) Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist. canTransform returned after 0.20191 timeout was 0.2. (wait_for_transform=0.200000)
[ERROR] (2024-08-08 14:35:40.424) MsgConversion.cpp:2177::convertRGBDMsgs() TF of received image 0 at time 1723120539.975974s is not set!
[ERROR] [1723120540.424660542] [rtabmap]: Could not convert rgb/depth msgs! Aborting rtabmap update...
[ WARN] (2024-08-08 14:35:40.630) MsgConversion.cpp:1994::getTransform() (can transform odom -> base_link?) Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist. canTransform returned after 0.201538 timeout was 0.2. (wait_for_transform=0.200000)
[ WARN] (2024-08-08 14:35:40.835) MsgConversion.cpp:1994::getTransform() (can transform odom -> base_link?) Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist. canTransform returned after 0.2013 timeout was 0.2. (wait_for_transform=0.200000)
[ WARN] (2024-08-08 14:35:41.040) MsgConversion.cpp:1994::getTransform() (can transform odom -> base_link?) Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist. canTransform returned after 0.201575 timeout was 0.2. (wait_for_transform=0.200000)
[ WARN] (2024-08-08 14:35:41.246) MsgConversion.cpp:1994::getTransform() (can transform odom -> base_link?) Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist. canTransform returned after 0.201538 timeout was 0.2. (wait_for_transform=0.200000)
[ WARN] (2024-08-08 14:35:41.448) MsgConversion.cpp:1994::getTransform() (can transform base_link -> zed_left_camera_optical_frame?) Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist. canTransform returned after 0.201519 timeout was 0.2. (wait_for_transform=0.200000)
[ERROR] (2024-08-08 14:35:41.448) MsgConversion.cpp:2177::convertRGBDMsgs() TF of received image 0 at time 1723120540.976142s is not set!
[ERROR] [1723120541.448482887] [rtabmap]: Could not convert rgb/depth msgs! Aborting rtabmap update...

I am not sure how to change frame_id and what should I change? I tried to edit "rtabmap.launch.py" from "base_link" to "camera_link" without success. If I run "rtabmap_slam" I don't know how can I change the frame_id at all.

Any help would be appreciated.

Cheers,
Robin

[matlabbe]

Following example conversion from here with the ros1 examples with zed odometry from here:

ros2 launch zed_wrapper zed_camera.launch.py \
   publish_map_tf:=false \
   camera_model:=zed2i

ros2 launch rtabmap_launch rtabmap.launch.py \
   args:="--delete_db_on_start" \
   frame_id:=zed_camera_link \
   odom_topic:=/zed/zed_node/odom \
   visual_odometry:=false \
   rgb_topic:=/zed/zed_node/rgb/image_rect_color \
   depth_topic:=/zed/zed_node/depth/depth_registered \
   camera_info_topic:=/zed/zed_node/rgb/camera_info \
   wait_imu_to_init:=true \
   imu_topic:=/zed/zed_node/imu/data \
   rviz:=true \
   approx_sync:=false \
   rgbd_sync:=true \
   approx_rgbd_sync:=false \
   topic_queue_size:=2

[hoffer]

Thanks for that reply @matlabbe. I have been trying for the better part of 2 days to get a ZEDx camera working well together with RTABMap unsuccessfully. With the config you suggest above, I do indeed at least get a full map -> odom -> base, however, I can't get any visualizations to update in foxglove and my terminal is full of the following. Feels like something is missing. Any suggestions?

[rtabmap-10] [WARN] [1723775897.914904057] [rtabmap.rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "sync_queue_size" and/or "topic_queue_size" parameters (current=500 and 500 respectively).
[rtabmap-10] rtabmap subscribed to (approx sync):
[rtabmap-10]    /zed_node/odom \
[rtabmap-10]    /rtabmap/rgbd_image
[rgbd_sync-9] [WARN] [1723775897.941365131] [rtabmap.rgbd_sync]: The time difference between rgb and depth frames is high (diff=0.100121s, rgb=1723775897.591064s, depth=1723775897.490943s). You may want to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use approx_sync=false if streams have all the exact same timestamp.
[rgbd_sync-9] [WARN] [1723775898.008707069] [rtabmap.rgbd_sync]: The time difference between rgb and depth frames is high (diff=0.066786s, rgb=1723775897.690808s, depth=1723775897.757594s). You may want to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use approx_sync=false if streams have all the exact same timestamp.

[matlabbe]

"diff=0.100121s" that looks bad. On which machine are you doing this? Are the topic frame rate stable?

Testing on my side I saw that the frame rate output of zed2i default config was only about 10 Hz. I changed resolution to VGA in zed2i.yaml in zed_wrapper to get 30 Hz.

I also forgot to set approx_rgbd_sync, which should be false for zed camera (updated last post). After a doublecheck, approx_sync can also be set to false, as zed outputs all data with exactly same stamp (updated last post).

For rgbd_sync, it seems that even receiving images at 30 Hz, it was outputting at 10 Hz (expected 30 Hz). By setting topic_queue_size:=2 (updated last post) it helped to get >24 fps on /rtabmap/rgbd_image.

cheers,
Mathieu

[hoffer]

Thanks a lot Mathieu. With these settings changes I've gotten past these errors but not stuck not being able to create loop closures. When I do a loop in my office, right about where I come back to where I started, I get loop closure rejections where there should be a loop closure. I am also using depthimage_to_laserscan with subscribe_scan:=true for better occupancy grid generation. Attached my map as it appears before these loop closure errors. Any ideas?

[rtabmap-18] [ WARN] (2024-08-16 10:53:18.067) Rtabmap.cpp:3844::process() Rejecting all added loop closures (1, first is 180 <-> 26) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 11.933435 (edge 26->180, type=1, abs error=53.889996 deg, stddev=0.078817). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation.
[rtabmap-18] [ WARN] (2024-08-16 10:53:18.067) Rtabmap.cpp:3882::process() Loop closure 180->26 rejected!
[rtabmap-18] [ WARN] (2024-08-16 10:53:19.327) Rtabmap.cpp:3807::process() Rejecting all added loop closures (1, first is 181 <-> 23) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 135.697876 (edge 23->181, type=1, abs error=2.591433 m, stddev=0.019097). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation.

[hoffer]

Just an update to the above post and for anyone else trying to get things working with a ZEDx camera providing VIO for rtabmap_slam... I was able to reduce these loop closure rejections and stay localized with the following zed params (as well as Mathieu's suggested RTAB settings above).

Zed
pos_tracking_enabled: true
pos_tracking_mode: "GEN_1" # GEN_2 did not  work well for me
area_memory: false # to prevent loop closure detections
sensors_image_sync: true # Synchronize Sensors messages with latest published video/depth message

[matlabbe]

For the loop closures rejected, there were huge errors " abs error=53.889996 deg" or "abs error=2.591433 m". To debug this, it can be easier to create 2 maps, overlapping where the loop closures should happen. We can then assemble the two maps in same database with:

rtabmap-reprocess "map1.db;map2.db" output.db

rtabmap-databaseViewer output.db

In Database Viewer, open Constraints and Graph Views. Using the main sliders, choose a frame from first map and a second frame on the second map. On Constraints view, click on "Add Constraint". In Core Parmaeters view, it is possible to change "Vis/..." parameters and click again Add Constraint with different parameters. If the loop keeps being rejected by graph deformation rejection like above, set temporary RGBD/OptimizeMaxError to 0 to see if the result makes sense.

You can also share that database here so we can take a look.

Mathieu

[hoffer]

Thanks Mathieu. As mentioned above, I am no longer seeing these loop closure rejections and have things dialed in now. I've found it's pretty important, at least when using a zedx, to tune zed settings correctly especially when feeding the zed's VIO into an EKF.