Proper way to get stereo odometry working on custom dataset

[TakShimoda]

Hello, thank you for this library.

I looked through the documentation on this page, and I'm still confused on how to properly get this system running on ROS2. Here's what I have done so far to set up the workspace:

My attempt to run it

• I used the docker implementation (with zenoh bridge DDS) because for some reason, the packages couldn't find gtsam on my pc, similar to the issue here
• I manually downloaded resnet101_512.pth to /Swarm-SLAM/install/cslam/share/cslam/models/ as some of the launch files were throwing errors not finding these specific resnet weights there.
• I recorded my own rosbags in .db3 format with stereo and imu data (although I'm not using IMU for now).

From here, I run a few commands in the docker container: ros2 launch cslam_experiments rtabmap_kitti_stereo_odometry.launch.py frame_id:=D435i_link args:=--delete_db_on_start (terminal 1)

• Run rtabmap for stereo odometry front-end. I'm using realsense2 d435i for ROS2 foxy and renamed my camera node as D435i, so the frame_id I place there is the appropriate frame where odom will be its parent.
• Even though the file name said "kitti" I didn't see any lines related to config files so I assume it's for any stereo dataset. I had rtabmap working fine by itself and there was no need to provide intrinsics/extrinsics.

ros2 launch cslam_experiments cslam_stereo.launch.py (terminal 2)

• Run the map manager, loop detection, and pose graph manager nodes

ros2 bag play <my bag file>.db3 --remap <old:=new> --qos-profile-overrides-path qos_overrides.yaml (terminal 3)

• I specifically remap my old topics to match the new topics: /r0/stereo_camera/left/camera_info, /r0/stereo_camera/right/camera_info, /r0/stereo_camera/left/image_rect_color, and /r0/stereo_camera/right/image_rect_color
• I also override the QoS so its reliability policy is best_effort and durability policy is volatile to match the subscriber QoS which was only an issue on docker. I also override the durability policy of /tf_static to transient_local as it only publishes once and rtabmap needs it.

I'm assuming this is the correct setup based on the nodes and what topics they subscribe to: the stereo frontend (rtabmap) would subscribe to the image topics mentioned above and then publish to /r0/odom (from the paper, I assume the loop closure detection reads the odometry output) and I assumed the pose graph manager has enough data from this.

Outputs

In terminal 1, I see the output of rtabmap stereo and it seems to work (although I can't verify as there's no visualization in docker). I'll consistently see lines like: [stereo_odometry-1] [INFO] [1718118649.944846719] [r0.stereo_odometry]: Odom: quality=437, std dev=0.005433m|0.016851rad, update time=0.000000s

• I'll see lines like this when I run rtabmap itself on my host pc and it works fine with proper outputs.
• The issue is there's no output from the launch file I used in docker in terminal 1 above. Usually, rtabmap would output a database file to ~/.ros/rtabmap.db but there's no output in the docker.

In terminal 2, I constantly get output such as the following: [map_manager-2] [INFO] [1718118087.844839116] [r0.cslam_map_manager]: Could not get transform from camera_link to D435i_infra1_optical_frame after 0.1 s!

• This seemed like strange behavior as all my frames start with 'D435i' but this map manager still seems to be looking for 'camera_link'.

In terms of the tf frames (output of ros2 run tf2_tools view_frames.py), it seems there is a disconnection in the tf frames. Here is how tf frames looks like when running just rtabmap (frames.pdf) and when running cslam on docker (frames_2024-06-11_16.46.41.pdf)

frames.pdf frames_2024-06-11_16.46.41.pdf

I was just wondering if I was running it correctly and had questions about this package: 1) What types of outputs should it give? I know to assess the ATE/RPE I should be getting poses in EUROC/TUM/KITTI format but I can't seem to find it. I was also wondering if for rtabmap (or any front-end), should I expect outputs for single-robots or are these not useful after inter-robot loop closures?

2) What needs to be done to get a proper front-end running as according to the paper, it seems to run with any RGB-D/Stereo/Vision+IMU/Lidar front-end. From the launch file I used, it seemed to only run the rtabmap_odom stereo_odometry node, but for the launch file for just rtabmap here, it seems to launch many more nodes, such as rtabmap(from package rtabmap_slam). I was wondering if files, such as rtabmap_kitti_stereo_odometry.launch.py are only for the front-end (and there's no need to setup communication with the back-end) so they only communicate via topics/services/actions? For example, /r0/odom is properly published by stereo_odometry and subscribed to by cslam_map_manager, and I can topic echo and see it fine. I was thinking of just using the default rtabmap launch file to see if it worked.

[lajoiepy]

Hi! I am very sorry for the delayed response.

To clarify a few things, RTABMap is simply used to compute the odometry and the odometry is considered an external input of Swarm-SLAM, so you could use any other odometry software if need be.

For the tf frame issue, you need to modify this parameter if your camera frame ID is not camera_link: https://github.com/lajoiepy/cslam/blob/32527430ef271064d468f442a3ab754917abb011/config/cslam/example.yaml#L9 In your case, it should be D435i_infra1_optical_frame.

As for your other questions:

1. There are two types of outputs: live ones, and estimates files. To visualize the live ones during the run you need to use the cslam_visualization package (https://github.com/lajoiepy/cslam_visualization) . To have access to the estimates files, you need to enable the logs with this parameter https://github.com/lajoiepy/cslam/blob/32527430ef271064d468f442a3ab754917abb011/config/cslam/example.yaml#L43 and specify the output folder.
2. Again, for Swarm-SLAM, we only need an odometry input, so we do not need all the other rtabmap modules, you could even replace rtabmap with any other odometry software. If you want single robot maps and databases or other rtabmap features, you could run all the other nodes in parallel, but they are not integrated with Swarm-SLAM, so the results will be independent.

[TakShimoda]

Hello, thanks for the reply.

For some reason, I couldn't get it to see changes in the yaml files, so I just manually changed the source code to get the correct frame id and output log folder, and it now works and I can also visualize it.

I'm having the exact same issue as #19 where I have ground-truth 6DoF poses but not GPS data. I know the numbers in the g2o file are related to gtsam labeled symbols but I can't seem to relate them to actual unix time from the bag files which I need for correspondence to perform Umeyama's method for calculating ATE and also plotting the aligned trajectories. The author there fixed the issue but they don't mention how they did it.

There are pose timestamp files, e.g. pose_timestamps0.csv, but they're empty.

[TakShimoda]

Edit: I got it working by changing enable_pose_timestamps_recording_ inside decentralized_pgo.cpp to true then just made a csv and aligned the timestamps, converted it tum, then got it working with evo. I'll leave this comment here in case anyone in the future runs into the same issue.

Thanks for your help @lajoiepy !