EKF with icp and stereo odometry

[Marcelomaf]

Hello there I'm trying to use the robot_localization pkg to fuse stereo odometry from a zed2i and icp odometry from a VLP32C LiDAR, but no success. The 3d pointcloud and odometry image in the rtabmap_visualizer just freeze at the beggining, and if i echo the icp_odom topic it's correct, but the odometry/filtered topic from EKF is with high covariances in x,y and z poses, i dont know why this is happening, i havent tweaked with covariances in the ekf yaml file. Any help would be appreciated! My launch file:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time')
    deskewing = LaunchConfiguration('deskewing')

    velodyne_pointcloud_dir = get_package_share_directory('velodyne_pointcloud')
    velodyne_pointcloud_launch_dir = os.path.join(velodyne_pointcloud_dir, 'launch')

    velodyne_pointcloud = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(velodyne_pointcloud_launch_dir, 'velodyne_transform_node-VLP32C-launch.py')))
    
    robot_localization_dir = get_package_share_directory('robot_localization')
    robot_localization_launch_dir = os.path.join(robot_localization_dir, 'launch')

    robot_localization = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(robot_localization_launch_dir, 'ekf_rtabmap.launch.py')))
    
    return LaunchDescription([
        
        velodyne_pointcloud,

        # Launch arguments
        DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation (Gazebo) clock if true'),
        
        DeclareLaunchArgument(
            'deskewing', default_value='true',
            description='Enable lidar deskewing'), 
        
        # TF's HB20
        # Node(
        #     package='tf2_ros', executable='static_transform_publisher', output='screen',
        #     arguments=['0', '0', '1.85', '-0.23', '0', '0', 'base_link', 'velodyne']),

        # Node(
        #     package='tf2_ros', executable='static_transform_publisher', output='screen',
        #     arguments=['2.13', '0', '0.99', '-1.57', '0', '-1.57', 'base_link', 'zed2_front_left_camera_optical_frame']),

        # Node(
        #     package='tf2_ros', executable='static_transform_publisher', output='screen',
        #     arguments=['0.30', '0', '0', '0', '0', '0', 'base_link', 'F2005523']),

        # TF's Hilux
        Node(
            package='tf2_ros', executable='static_transform_publisher', output='screen',
            arguments=['0', '0', '2.20', '0', '0', '0', 'base_link', 'velodyne']),

        Node(
            package='tf2_ros', executable='static_transform_publisher', output='screen',
            arguments=['1.70', '0', '1.38', '-1.57', '0', '-1.57', 'base_link', 'zed2_front_left_camera_optical_frame']),
        
        Node(
            package='tf2_ros', executable='static_transform_publisher', output='screen',
            arguments=['1.70', '0', '1.38', '-1.57', '0', '-1.57', 'base_link', 'zed2_front_imu_link']),

        Node(
            package='tf2_ros', executable='static_transform_publisher', output='screen',
            arguments=['-0.65', '0.54', '0.77', '0', '0', '0', 'base_link', 'F2005523']),

        # Nodes to launch

        # Node(
        #     package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen",
        #     parameters=[{
        #         "frame_id": 'base_link',
        #         "approx_sync": False,
        #         'odom_frame_id':'odom',
        #         'publish_tf':True,
        #         'wait_imu_to_init':True,
        #         'GFTT/MinDistance':'5',
        #         'Vis/MinInliers':'10',
        #         'Vis/CorGuessWinSize':'20',
        #         }],
        #     remappings=[
        #         ('left/image_rect', '/zed2_front/zed_node/left/image_rect_color'),
        #         ('left/camera_info', '/zed2_front/zed_node/left/camera_info'),
        #         ('right/image_rect', '/zed2_front/zed_node/right/image_rect_color'),
        #         ('right/camera_info', '/zed2_front/zed_node/right/camera_info'),
        #         ('odom','/odom')
        #         ],
        # ),

        Node(
            package='rtabmap_sync', executable='stereo_sync', name="stereo_sync", output="screen",
            parameters=[],
            remappings=[
                ("left/image_rect", '/zed2_front/zed_node/left/image_rect_color'),
                ("right/image_rect", '/zed2_front/zed_node/right/image_rect_color'),
                ("left/camera_info", '/zed2_front/zed_node/left/camera_info'),
                ("right/camera_info", '/zed2_front/zed_node/right/camera_info'),
                ("rgbd_image", 'rgbd_image')]),

        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{
              'frame_id':'base_link',
              'odom_frame_id':'icp_odom',
              'publish_tf':False,
              'wait_imu_to_init':True,
              'wait_for_transform':0.2,
              'expected_update_rate':10.0,
              'deskewing':deskewing,
              'use_sim_time':use_sim_time,
              # RTAB-Map's internal parameters are strings:
              'Icp/PointToPlane': 'true',
              'Icp/Iterations': '10',
              'Icp/VoxelSize': '0.5',
              'Icp/Epsilon': '0.001',
              'Icp/PointToPlaneK': '20',
              'Icp/PointToPlaneRadius': '0',
              'Icp/MaxTranslation': '2',
              'Icp/MaxCorrespondenceDistance': '1',
              'Icp/Strategy': '1',
              'Icp/OutlierRatio': '0.7',
              'Icp/CorrespondenceRatio': '0.01',
              'Odom/ScanKeyFrameThr': '0.4',
              'OdomF2M/ScanSubtractRadius': '0.1',
              'OdomF2M/ScanMaxSize': '15000',
              'OdomF2M/BundleAdjustment': 'false'
            }],
            remappings=[
              ('scan_cloud', '/velodyne_points'),
              ('odom','/icp_odom')
            ]),
            
        Node(
            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
            parameters=[{
              'max_clouds':10,
              'fixed_frame_id':'',
              'use_sim_time':use_sim_time,
            }],
            remappings=[
              ('cloud', 'odom_filtered_input_scan'),
              ('odom', 'odometry/filtered')
            ]),
            
        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[{
              'frame_id':'base_link',
            #   'odom_frame_id':'odom',
              'subscribe_depth':False,
              'subscribe_rgb':False,
              'subscribe_scan_cloud':True,
              'approx_sync':True,
              'wait_for_transform':0.2,
              'use_sim_time':use_sim_time,
              'subscribe_stereo':False,
              'subscribe_odom_info':True,
              "subscribe_rgbd": True,
              # RTAB-Map's internal parameters are strings:
              'Grid/RayTracing': 'true',
              'Grid/3D': 'true',
              'Grid/MaxGroundHeight': '0.10',
              'Grid/MinClusterSize': '5',
              'Grid/NoiseFilteringMinNeighbors': '2',
              'Grid/NoiseFilteringRadius': '0.5',
              'Grid/NormalK': '50',
              'Grid/NormalsSegmentation': 'true',
              'Grid/CellSize': '0.1',
              'RGBD/ProximityMaxGraphDepth': '0',
              'RGBD/ProximityPathMaxNeighbors': '1',
              'RGBD/AngularUpdate': '0.05',
              'RGBD/LinearUpdate': '0.05',
              'RGBD/CreateOccupancyGrid': 'false',
              'Mem/NotLinkedNodesKept': 'false',
              'Mem/STMSize': '30',
              'Mem/LaserScanNormalK': '20',
              'Mem/IncrementalMemory': 'true',  # SLAM Mode, if false it's Localization Mode.
              'Reg/Strategy': '1',
              'Icp/VoxelSize': '0.5',
              'Icp/PointToPlaneK': '20',
              'Icp/PointToPlaneRadius': '0',
              'Icp/PointToPlane': 'true',
              'Icp/Iterations': '10',
              'Icp/Epsilon': '0.001',
              'Icp/MaxTranslation': '3',
              'Icp/MaxCorrespondenceDistance': '1',
              'Icp/Strategy': '1',
              'Icp/OutlierRatio': '0.7',
              'Icp/CorrespondenceRatio': '0.2'
            }],
            remappings=[
              ('depth/image', '/zed2_front/zed_node/depth/depth_registered'),
              ('left/image_rect', '/zed2_front/zed_node/left/image_rect_color'),
              ('left/camera_info', '/zed2_front/zed_node/left/camera_info'),
              ('right/image_rect', '/zed2_front/zed_node/right/image_rect_color'),
              ('right/camera_info', '/zed2_front/zed_node/right/camera_info'),
              ('scan_cloud', 'assembled_cloud'),
              ('rgbd_image', 'rgbd_image'),
              ('odom','odometry/filtered')
            ],
            arguments=[
              '-d' # This will delete the previous database (~/.ros/rtabmap.db)
            ]), 
     
        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=[{
              'frame_id':'base_link',
            #   'odom_frame_id':'odom',
              'subscribe_stereo': False,
              'subscribe_depth': False,
              'subscribe_odom_info':True,
              'subscribe_rgbd':True,
              'subscribe_scan_cloud':True,
              'approx_sync':True,
              'use_sim_time':use_sim_time,
            #   'wait_for_transform':0.5,
            }],
            remappings=[
              ('depth/image', '/zed2_front/zed_node/depth/depth_registered'),
              ('left/image_rect', '/zed2_front/zed_node/left/image_rect_color'),
              ('left/camera_info', '/zed2_front/zed_node/left/camera_info'),
              ('right/image_rect', '/zed2_front/zed_node/right/image_rect_color'),
              ('right/camera_info', '/zed2_front/zed_node/right/camera_info'),
              ('scan_cloud', 'odom_filtered_input_scan'),
              ('rgbd_image', 'rgbd_image'),
              ('odom','odometry/filtered')
            ]),

        Node(
            package='pitts_rtabmap_bridge', executable='ins_to_imu', output='screen'),

        Node(
            package='pitts_rtabmap_bridge', executable='ins_to_gps', output='screen'),

        robot_localization,

    ])

[matlabbe]

Marcelomaf wrote

The 3d pointcloud and odometry image in the rtabmap_visualizer just freeze at the beggining

Can you try launching rtabmap with lidar alone? by setting "subscribe_rgbd": False (for both rtabmap and rtabmap_viz nodes)? Then see if the odometry point cloud is refreshed correctly in rtabmap_viz. If the icp_odometry topic is published and valid (quaternion is not null), I would expect rtabmap_viz to correctly show it. If it does in that config, it means there is a sync issue between the stereo camera and lidar point cloud, assuming they are published at different rates. You can try playing with topic_queue_size and sync_queue_size parameters to improve sync and avoid deadlock. For ROS2, there is a MR to fix some sync issues related to topics having different rates and different delays (see this post for more in-depth description of the sync issue, which seems appearing more on rtabmap's ROS2 implementation).

For the covariance values, if icp_odometry and zed2i odometry have both small covariances, then the issue may be related to EKF config.

cheers,
Mathieu