From 81d99d2c1c7877cca8dfc93de908ce8a1e7db137 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 1 Mar 2025 17:43:22 -0800
Subject: [PATCH] Improved rgbdslam_datasets.launch.py to re-use vo features in
 back-end (#1277), also added ground truth recording.

---
 .../launch/rgbdslam_datasets.launch.py        | 59 +++++++++++++------
 1 file changed, 42 insertions(+), 17 deletions(-)

diff --git a/rtabmap_examples/launch/rgbdslam_datasets.launch.py b/rtabmap_examples/launch/rgbdslam_datasets.launch.py
index 9aab2edd..92e8e030 100644
--- a/rtabmap_examples/launch/rgbdslam_datasets.launch.py
+++ b/rtabmap_examples/launch/rgbdslam_datasets.launch.py
@@ -1,47 +1,72 @@
 
 # Example to run rgbd datasets:
+#
 # [ROS1] Prepare ROS1 rosbag for conversion to ROS2
 #   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
 #   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
+#   $ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/45e6ac01541973a17505000fc8c7ca399ae275eb/tum_rename_world_kinect_frame.py
+#   $ python3 tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag
 #   $ wget https://raw.githubusercontent.com/srv/srv_tools/kinetic/bag_tools/scripts/change_frame_id.py
+#
 #   Edit change_frame_id.py, remove/comment lines beginning with "PKG" and "import roslib", change line "Exception, e" to "Exception"
 #   $ roscore
 #   $ python3 change_frame_id.py -o rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag -i rgbd_dataset_freiburg3_long_office_household.bag -f openni_rgb_optical_frame -t /camera/rgb/image_color
+#
 # [ROS2]
 #   $ sudo pip install rosbags     # See https://docs.openvins.com/dev-ros1-to-ros2.html
-#   $ rosbags-convert rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag
-
+#   $ rosbags-convert --src rgbd_dataset_freiburg3_long_office_household_frameid_fixed.bag --dst rgbd_dataset_freiburg3_long_office_household_frameid_fixed
+#
 #   $ ros2 launch rtabmap_examples rgbdslam_datasets.launch.py
 #   $ cd rgbd_dataset_freiburg3_long_office_household_frameid_fixed
 #   $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 --clock
-
+#
+# To get RMSE after the run:
+#   $ rtabmap-report ~/.ros/rtabmap.db
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.actions import SetParameter
 
 def generate_launch_description():
 
-    parameters=[{
+    odom_parameters=[{
+        'frame_id':'kinect',
+        # ground truth here is just used to align odometry with ground truth's first pose
+        'ground_truth_frame_id':'world',
+        'ground_truth_base_frame_id':'kinect_gt',
+        'keep_color': True,
+        'wait_for_transform': 0.5,
+        # RTAB-Map's parameters should all be string type:
+        'Odom/Strategy':'0',
+        'Odom/ResetCountdown':'15',
+        'Odom/GuessSmoothingDelay':'0',
+    }]
+    slam_parameters=[{
           'frame_id':'kinect',
-          'subscribe_depth':True,
+          # Record ground truth to compute RMSE
+          'ground_truth_frame_id':'world',
+          'ground_truth_base_frame_id':'kinect_gt',
+          'subscribe_rgb':False,
+          'subscribe_depth':False,
+          'subscribe_rgbd':True,
           'subscribe_odom_info':True,
           # RTAB-Map's parameters should all be string type:
-          'Odom/Strategy':'0',
-          'Odom/ResetCountdown':'15',
-          'Odom/GuessSmoothingDelay':'0',
+          'Mem/UseOdomFeatures': 'true',
           'Rtabmap/StartNewMapOnLoopClosure':'true',
           'RGBD/CreateOccupancyGrid':'false',
           'Rtabmap/CreateIntermediateNodes':'true',
           'RGBD/LinearUpdate':'0',
           'RGBD/AngularUpdate':'0'}]
           
-    remappings=[
+    odom_remappings=[
           ('rgb/image', '/camera/rgb/image_color'),
           ('rgb/camera_info', '/camera/rgb/camera_info'),
           ('depth/image', '/camera/depth/image')]
+    
+    # We will use the output of odometry to avoid re-extracting 
+    # the same features on slam side.
+    slam_remappings=[
+        ("rgbd_image", "odom_rgbd_image")]
           
 
     return LaunchDescription([
@@ -52,19 +77,19 @@ def generate_launch_description():
         # Nodes to launch
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', output='screen',
-            parameters=parameters,
-            remappings=remappings),
+            parameters=odom_parameters,
+            remappings=odom_remappings),
 
         Node(
             package='rtabmap_slam', executable='rtabmap', output='screen',
-            parameters=parameters,
-            remappings=remappings,
+            parameters=slam_parameters,
+            remappings=slam_remappings,
             arguments=['-d']),
 
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', output='screen',
-            parameters=parameters,
-            remappings=remappings),
+            parameters=slam_parameters,
+            remappings=slam_remappings),
        
         # /tf topic is missing in the converted ROS2 bag, create a fake tf
         Node(