To achieve better odometry fusion？

[sun-under-bird]

Below is my EKF configuration file. Is this accurate? When using pure visual odometry, there won't be any ghosting, but when there is no texture, it will be lost. After the loss, the mapping process will be interrupted. I don't want this to happen, so I want to fuse the odometry information as an auxiliary. However, after the fusion, there is ghosting and the same object appears multiple times. Why is that? Is it because the timestamps of the odometry and the image are not the same? I measured it and the time difference between them is over 100 ms. Could it be because of this?

ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: true
        publish_tf: true
        map_frame: map
        odom_frame: odom
        base_link_frame: base_footprint
        world_frame: odom

        odom0: /odom
        odom0_config: [false, false, false,
                       false, false, false,
                       true,  true,  false,
                       false, false, true,
                       false, false, false]
        odom0_differential: false
        odom0_relative: false
        odom0_queue_size: 10

        odom1: /vo
        odom1_config: [true,  true,  false,
                       false, false, true,
                       false, false, false,
                       false, false, false,
                       false, false, false]
        odom1_differential: false
        odom1_relative: false
        odom1_queue_size: 10

        print_diagnostics: true
        debug: false

        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02, 0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]

        initial_estimate_covariance: [1.0, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    1.0, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    1.0, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1.0, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    1.0, 0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9]

Below is my rtabmap startup file

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():

    base_frame = LaunchConfiguration('base_frame')
    use_viz = LaunchConfiguration('use_viz')
    ekf_config_file = LaunchConfiguration('ekf_config')

    rtabmap_odom_params = {
        'frame_id': base_frame,
        'subscribe_rgbd': False,
        'subscribe_stereo': True,
        'subscribe_odom_info': True,
        'use_sim_time': True,
        'approx_sync': True,
        'approx_sync_max_interval': 0.1,
        'sync_queue_size': 30,
        'topic_queue_size': 30,
        'wait_for_transform': 0.5,
        'Rtabmap/ImagesAlreadyRectified': 'true',
        'publish_tf': False,
        'Vis/FeatureType': '8',
        'Vis/EstimationType': '1',
        'Vis/MinInliers': '20',
        'Vis/MaxFeatures': '1500',
        'Vis/CorType': '0',
        'Odom/ResetCountdown': '1',
        'Odom/Strategy': '0',
        'OdomF2M/MaxSize': '1000',
        'GFTT/MinDistance': '5',
        'GFTT/QualityLevel': '0.00001',
        'Stereo/MaxDisparity': '128',
    }

    rtabmap_slam_params = {
        'frame_id': base_frame,
        'subscribe_rgbd': False,
        'subscribe_stereo': True,
        'subscribe_odom_info': False,
        'subscribe_odom': False,
        'odom_frame_id':'odom',
        'use_sim_time': True,
        'approx_sync': True,
        'approx_sync_max_interval': 0.1,
        'sync_queue_size': 30,
        'topic_queue_size': 30,
        'wait_for_transform': 0.5,
        'tf_delay': 0.05,
        'Rtabmap/ImagesAlreadyRectified': 'true',
        'Rtabmap/DetectionRate': '3',
        'Reg/Force3DoF': 'true',
        'Kp/MaxFeatures': '1000',
        'GFTT/MinDistance': '5',
        'GFTT/QualityLevel': '0.00001',
        'Stereo/MaxDisparity': '128',
        'Grid/RayTracing': 'true',
        'Grid/RangeMax': '3',
        'odom_sensor_sync': True,
    }

    odom_remaps = [
        ('left/image_rect', '/stereo/left/camera/image_rect'),
        ('right/image_rect', '/stereo/right/camera/image_rect'),
        ('left/camera_info', '/stereo/left/camera/camera_info'),
        ('right/camera_info', '/stereo/right/camera/camera_info'),
        ('odom', '/vo'),
    ]

    slam_remaps = [
        ('left/image_rect', '/stereo/left/camera/image_rect'),
        ('right/image_rect', '/stereo/right/camera/image_rect'),
        ('left/camera_info', '/stereo/left/camera/camera_info'),
        ('right/camera_info', '/stereo/right/camera/camera_info'),
        ('odom', '/odometry/filtered'),
    ]

    return LaunchDescription([

        DeclareLaunchArgument('base_frame', default_value='base_footprint'),
        DeclareLaunchArgument('use_viz', default_value='true'),
        DeclareLaunchArgument(
            'ekf_config',
            default_value='/home/yahboom/camera_ws/src/stereo_camera_pkg/config/ekf_wheel_imu_vo.yaml'
        ),


        Node(
            package='image_proc',
            executable='rectify_node',
            name='rectify_left',
            namespace='/stereo/left/camera',
            parameters=[{'use_sim_time': True}],
            remappings=[
                ('image', '/stereo/left/camera/image_mono'),
                ('camera_info', '/stereo/left/camera/camera_info'),
                ('image_rect', '/stereo/left/camera/image_rect')
            ]
        ),

        Node(
            package='image_proc',
            executable='rectify_node',
            name='rectify_right',
            namespace='/stereo/right/camera',
            parameters=[{'use_sim_time': True}],
            remappings=[
                ('image', '/stereo/right/camera/image_mono'),
                ('camera_info', '/stereo/right/camera/camera_info'),
                ('image_rect', '/stereo/right/camera/image_rect')
            ]
        ),

        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[ekf_config_file, {'use_sim_time': True}],
            ),

        Node(
            package='rtabmap_odom',
            executable='stereo_odometry',
            name='stereo_odometry',
            output='screen',
            parameters=[rtabmap_odom_params],
            remappings=odom_remaps
        ),

        Node(
            package='rtabmap_slam',
            executable='rtabmap',
            name='rtabmap',
            output='screen',
            parameters=[rtabmap_slam_params],
            remappings=slam_remaps,
            arguments=['-d']
        ),

        Node(
            package='rtabmap_viz',
            executable='rtabmap_viz',
            name='rtabmap_viz',
            output='screen',
            condition=IfCondition(use_viz),
            parameters=[rtabmap_slam_params],
            remappings=slam_remaps
        ),
    ])

And I noticed that if "subscribe_odom=true" is set and the "odom_frame_id" is deleted, the loop closure will not be triggered. Why is that?
Hope to receive your reply. Thank you.

[matlabbe]

To fuse visual odometry with wheel odometry, you have to setup visual odometry node with:

 rtabmap_odom_params = {
        'odom_frame_id': 'odom',
        'publish_tf': False,
        'publish_null_when_lost': False,
        'Odom/ResetCountdown': '1',

This will make visual odometry resets automatically after 1 frame being lost. It will also not publish null twist odometry to the EKF when lost. With EKF publishing odom TF, it will use that TF to reinitialize visual odometry after being lost. Note however that if twists are fused, that doesn't really matter at which pose visual odometry is reset. Visual odometry should also not publish tf, because EKF is publishing it.

For robot_localization, you may want to fuse velocities of both odometry inputs. The tricky part to get good fusion is to have representative twist covariance of the error of each odometry. For example, if visual odometry is more accurate than wheel odometry, it should have smaller covariance than wheel odometry, otherwise the filter could trust more wheel odometry even if visual odometry is fine. If you have control on your wheel odometry covariance, you may try to make it larger than visual odometry, so that when visual odometry is available, the filter will trust more visual odometry than wheel odometry.

For rtabmap node, not sure why it won't update as approx_sync is true, which means it should be able to synchronize the required input topics. Are the timestamps largely off between odom topic and the camera topics? Do you have warnings that it could not sync topics for the past 5 sec?

With stereo input, I don't recommend using approx_sync: true and subscribe_stereo at the same time, use rtabmap_sync/stereo_sync node to pre-sync stereo data with exact synchronization, then set subscribe_rgbd: true to true with approx_sync: true.

[sun-under-bird]

2026-04-14.111331.mp4

I tested the mapping using pure wheel-based odometers. The point cloud shown above is the point cloud generated by the pure wheel-based odometer. You can see a lot of double shadows. Therefore, the double shadows caused during fusion are due to the wheel-based odometer. Why is that? Is it because of the timestamp issue? My wheel-based odometer uses this->now() as the timestamp, while the image uses the original timestamp of usb_cam. The time difference between the latest image data and the latest wheel-based odometer is between 100 and 800 ms. This time difference changes every time the camera is restarted. Why? If I set a cache queue of size 50 for the odometer, allowing the image to find the most matching timestamp within it, then the time difference will become around 20 ms. Is this the problem? How to solve it? I tried to also add this->now() timestamp to the image, but there will still be double shadows, and the situation might even be worse.
My mileage counter frame rate is 20 fps, and the image frame rate is 15 fps. However, it doesn't reach 15 fps consistently. After mapping, it usually drops to 12-13 fps.

2026-04-14.111729.mp4

This is the effect of pure visual odometry mapping. It is normal because the visual odometry is calculated based on images, so the timestamps between them are perfectly matched. Will the point cloud accurately be placed at the position calculated by the visual odometry?

I hope to receive your solution to this problem. It has been bothering me for a long time and I haven't been able to solve it. Thank you very much!

[matlabbe]

In the first video, it looks there is a mismatch between camera stamps and the wheel odometry stamps. Are they synchronized on the same system's clock? We can see the image changing while the pose is not rotating yet. Based on the delay difference when you restart the camera, could it be that the camera is using stamps from the internal clock in the camera? Check if the camera driver has an option to convert internal clock to system clock.

When computing visual odometry, it will use timestamps from the image topic (not system's clock), so the relative time interval between images should follow the same clock. You can do ros2 topic delay /vo to see how bad is the delay in comparison to your system's clock.

[sun-under-bird]

The wheel odometer readings and image data of mine are all timestamped with this->now() when being sent. However, this approach seems not to work. It causes point cloud duplication. If not this way, how can I achieve time synchronization? When using the USB_cam driver to control the camera, each time the camera is started, the difference between the camera's timestamp and the system time is different. For example, the average time difference this time is between 350ms and 370ms. After closing the startup node, the next time the camera is started, the time difference will become between 500ms and 520ms. Therefore, I can only use the timestamp of this->now to overwrite the timestamp of msg->header.stamp. I don't have a better solution. Do you have any good ideas? Thank you for your every response.

This is my camera cropping and publishing node.

[matlabbe]

The timestamp in the topic should be the capture timestamp (synchronized with system's clock), not the time that node is receiving the image. It is unlikely that your camera provides images with 0 ms delay. The problem here is that if the images have really 500 ms delay and you restamp to now, then all wheel odometry poses will match image frames 500 ms in the past. RTAB-Map will sync the wrong pose with an image taken in reality 500 ms in the past.

For the wheel odometry, assuming a 0 ms delay may be close to reality, but I don't know what hardware you are using or where the actual odometry topic is generated with which stamp.