When RGBD odometry lost, loop closure is not being detected

[yuan-0816]

I am using the Realsense D435i to build a map (utilizing the infrared and depth cameras, RGBD odometry).

my ENV: ubuntu 22.04 ros2 humble

To prevent odometry lost, I have configured some parameters.

1. In rtabmap_odom package, I use rgbd_odometry, I set

   "Odom/ResetCountdown": "True",
   "publish_null_when_lost": True,

2. In rtabmap_slam package, I set

   "Rtabmap/StartNewMapOnLoopClosure": "True"

This is my rtabmap launch file

import os
from launch import LaunchDescription, Substitution, LaunchContext
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from launch_ros.actions import SetParameter
from typing import Text
from ament_index_python.packages import get_package_share_directory

class ConditionalText(Substitution):
    def __init__(self, text_if, text_else, condition):
        self.text_if = text_if
        self.text_else = text_else
        self.condition = condition

    def perform(self, context: 'LaunchContext') -> Text:
        if self.condition == True or self.condition == 'true' or self.condition == 'True':
            return self.text_if
        else:
            return self.text_else

def launch_setup(context, *args, **kwargs):     

    parameters={
    'frame_id':'camera_link',
    'subscribe_depth':True,
    'subscribe_odom_info':True,
    'approx_sync':False,
    'wait_imu_to_init':True
    }

    remappings=[
          ('imu', '/imu/data'),
          ('rgb/image', '/camera/infra2/image_rect_raw'),
          ('rgb/camera_info', '/camera/infra2/camera_info'),
          ('depth/image', '/camera/depth/image_rect_raw')] 

    return [

        DeclareLaunchArgument('args',  default_value=LaunchConfiguration('rtabmap_args'), 
                              description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),

        Node(
            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
            parameters=[
                parameters,
                {
                    "Odom/ResetCountdown": "True",
                    "publish_null_when_lost": True,
                }],
            remappings=remappings),

        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[{   
                'frame_id':'camera_link',
                'subscribe_depth':True,
                'subscribe_odom_info':True,
                'approx_sync':False,
                'wait_imu_to_init':True,
                "database_path": LaunchConfiguration('database_path'),
                "Mem/IncrementalMemory": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' != 'true'"]))._predicate_func(context)).perform(context),
                "Mem/InitWMWithAllNodes": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' == 'true'"]))._predicate_func(context)).perform(context),
                "Rtabmap/StartNewMapOnLoopClosure": "True",
                }],
            remappings=remappings,
            arguments=[LaunchConfiguration("args")],
        ),

        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=[parameters],
            condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
            remappings=remappings),

        Node(
            package='rviz2', executable='rviz2', output='screen',
            condition=IfCondition(LaunchConfiguration("rviz")),
            arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]
            ),

        Node(
            package='rtabmap_util', executable='point_cloud_xyzrgb', output='screen',
            condition=IfCondition(LaunchConfiguration("rviz")),
            parameters=[{
                "decimation": 4,
                "voxel_size": 0.0,
                "approx_sync": LaunchConfiguration('approx_sync'),
                "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval')
            }],
            remappings=remappings),

        # Compute quaternion of the IMU
        Node(
            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
            parameters=[{'use_mag': False, 
                         'world_frame':'enu', 
                         'publish_tf':False}],
            remappings=[('imu/data_raw', '/camera/imu')]),

        # The IMU frame is missing in TF tree, add it:
        Node(
            package='tf2_ros', executable='static_transform_publisher', output='screen',
            arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
    ]

def generate_launch_description():

    config_rviz = os.path.join(
        get_package_share_directory('drone_vslam'), 'config', 'rtab.rviz')   

    return LaunchDescription([

        # Declare Launch Argument
        DeclareLaunchArgument('approx_sync', default_value='false', 
                              description='If timestamps of the input topics should be synchronized using approximate or exact time policy.'),

        DeclareLaunchArgument('approx_sync_max_interval', default_value='0.0', 
                              description='(sec) 0 means infinite interval duration (used with approx_sync=true)'),

        DeclareLaunchArgument('database_path', default_value='~/ros2_ws/src/drone_vslam/map_database/rtabmap.db',  
                              description='Where is the map saved/loaded.'),

        DeclareLaunchArgument('localization', default_value='false', 
                              description='Launch in localization mode.'),

        DeclareLaunchArgument('rtabmap_viz', default_value='True',  
                              description='Launch RTAB-Map UI (optional).'),

        DeclareLaunchArgument('rviz', default_value='true', 
                              description='Launch RVIZ (optional).'),

        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, 
                              description='Configuration path of rviz2.'),

        DeclareLaunchArgument('initial_pose', default_value='', 
                              description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),

        DeclareLaunchArgument('rtabmap_args',   default_value='',                   
                              description='Backward compatibility, use "args" instead.'),

        OpaqueFunction(function=launch_setup)

    ])

I am encountering an issue during the map creation where the same location is being built twice, and loop closure detection is not being triggered.

https://github.com/introlab/rtabmap_ros/assets/83689217/4008bb3d-0ca5-436c-bc44-2f2f7faca9aa

The areas circled in red and blue represent the same space.

How should I configure the parameters for loop closure detection?

I have set wait_imu_to_init to true, but when the odometry is lost and reset, it seems that IMU and gravity alignment are not utilized. Is there a way to configure Odom/ResetCountdown to trigger the use of IMU data for gravity alignment during the reset?

Additionally, I would like to ask about the RTABMAP_PARAM

RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D"); 

Is there any difference between them? OpenVINS seems to use the VIO method (I understand that RTAB-Map only aligns gravity at the beginning).

[matlabbe]

For the missing loop closures, it is difficult to see, maybe they were rejected for some reason (look at the terminal for possible warnings about loop closures rejected). You can share the database if you want, it will be easier to see it.

On odometry automatic reset, if guess_frame_id doesn't exist, it is reset to last pose. If last pose has not Identity rotation (which is likely the case), the imu orientation initialization will be skipped. To force reset with latest IMU orientation, you may change https://github.com/introlab/rtabmap_ros/blob/74d611c46553f19ea0dc553abb67e7eaadab0e15/rtabmap_odom/src/OdometryROS.cpp#L872-L873 to

Transform newPose(odometry_->getPose().x(), odometry_->getPose().y(), odometry_->getPose().z(), 0,0,0);
this->reset(newPose);

Note that we need to also change odometry_->reset to this->reset.

Another approach without modifying code is to provide another "odometry" frame that could be simply be the IMU orientation and use guess_frame_id. For convenience, we provide a node to publish that kind of TF, see rtabmap_util/imu_to_tf node.

For all odometry approaches that are not VIO, if IMU is provided, the first pose will be initialized with gravity. Other specifications:

• F2M (default) is loosely coupled VIO approach (when IMU is provided)
• OKVIS: third party tightly coupled VIO approach
• MSCKF_VIO: third party tightly coupled VIO approach
• VINS-Fusion: third party tightly coupled VIO approach (when IMU is provided)
• OpenVINS: third party tightly coupled VIO approach

For third party approaches, I suggest to try their official repo first (if they provide a ros2 node already). It is possible to use their visual odometry node directly as input to rtabmap node, however, to correctly get TF odometry estimation of the base frame of the robot and not the camera, using the integration inside rtabmap can be more convenient. If you are using ROS2, if they didn't provide a non-ros library, it may be difficult to compile them inside rtabmap though.