[Realsense D455 Visual SLAM] Pose jumps after initialization

[ishakbhatt-h3d]

I am trying to get isaac ros visual slam to give me an accurate pose using the realsense D455 but have run into a problem where the pose is normal for some time initially and then shoots off / jumps to some random pose that is definitely incorrect. For example, if I am moving in +/- x I should expect to see that /visual_slam/tracking/vo_pose is increasing/decreasing in x accordingly. It does that for a bit and then the pose is completely wrong and it shoots off even though I am for example not moving or still just moving in +/- x. In the source code I have disabled localization and mapping (though yes I could just do it in the launch). I also have the ground constraint enabled for odometry for the current physical setup I need it for. I found that when the constraint is enabled it is shooting off to a much smaller value like what is shown below compared to x y and z being in the 100s range and sometimes 1000s.

Here's an example of the pose it would shoot off to:
x: -6.530872821807861
y: -42.238014221191406
z: 41.82627487182617

This is what my isaac_ros_visual_slam_realsense.launch.py looks like:

import launch
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    """Launch file which brings up visual slam node configured for RealSense."""
    realsense_camera_node = ComposableNode(
        name='camera',
        namespace='camera',
        package='realsense2_camera',
        plugin='realsense2_camera::RealSenseNodeFactory',
        parameters=[{
                'enable_infra1': True,
                'enable_infra2': True,
                'enable_color': True,
                'enable_depth': False,
                'align_depth.enable': True,
                'depth_module.emitter_enabled': 0,
                'depth_module.profile': '640x360x30',
                'rgb_camera.profile': '848x480x5',
                'rgb_camera.enable_auto_exposure': False,
                'rgb_camera.exposure': 50,
                'clip_distance': 5.0,
                'enable_gyro': True,
                'enable_accel': True,
                'gyro_fps': 200,
                'accel_fps': 200,
                'unite_imu_method': 2,
                'intra_process_comms': True
        }]
    )

    visual_slam_node = ComposableNode(
        name='visual_slam_node',
        package='isaac_ros_visual_slam',
        plugin='isaac_ros::visual_slam::VisualSlamNode',
        parameters=[{
                    'denoise_input_images': False,
                    'rectified_images': True,
                    'enable_debug_mode': False,
                    'debug_dump_path': '/tmp/cuvslam',
                    'enable_slam_visualization': False,
                    'enable_landmarks_view': False,
                    'enable_observations_view': False,
                    'map_frame': 'map',
                    'odom_frame': 'odom',
                    'base_frame': 'camera_link',
                    'input_imu_frame': 'camera_gyro_optical_frame',
                    'enable_imu_fusion': True,
                    'gyro_noise_density': 0.000244,
                    'gyro_random_walk': 0.000019393,
                    'accel_noise_density': 0.001862,
                    'accel_random_walk': 0.003,
                    'calibration_frequency': 200.0,
                    'img_jitter_threshold_ms': 60.0,
                   'enable_ground_constraint_in_odometry': True
                    }],
        remappings=[('stereo_camera/left/image', 'camera/infra1/image_rect_raw'),
                    ('stereo_camera/left/camera_info', 'camera/infra1/camera_info'),
                    ('stereo_camera/right/image', 'camera/infra2/image_rect_raw'),
                    ('stereo_camera/right/camera_info', 'camera/infra2/camera_info'),
                    ('visual_slam/imu', 'camera/imu')]
    )

    visual_slam_launch_container = ComposableNodeContainer(
        name='visual_slam_launch_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            visual_slam_node,
            realsense_camera_node
        ],
        output='screen'
    )

    return launch.LaunchDescription([visual_slam_launch_container])

I do also notice that right at the beginning I get the delta is above threshold warning, but it's only initially and then it's fine and that warning goes away.

Any help here would be appreciated!

[ishakbhatt-h3d]

@jaiveersinghNV @swapnesh-wani-nvidia @hemalshahNV @hguillen

pinging anyone it mentions when I use "@" as I see many of the previous issues have not been responded to yet.