NVIDIA-ISAAC-ROS / NVIDIA-ISAAC-ROS/isaac_ros_visual_slam

[Realsense D455 Visual SLAM] Pose jumps after initialization

Open
#183 3 comments 0 reactions 0 assignees View on GitHub

Nobody has claimed this yet.

Dominant language
C++
Stars
1.5k
Forks
206
PR merge metrics
No merged PRs in 30d

Description

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!

Contributor guide

Open the contributing guide

First steps

  1. Read the whole issue, then the project's contributing guide.
  2. Comment on the issue to say you are picking it up — it saves two people doing the same work.
  3. Fork the repository and make your change on a branch.
  4. Open a pull request that references the issue number.

Research direction

Start with the isaac_ros_visual_slam_realsense.launch.py configuration and the /visual_slam/tracking/vo_pose output, then reproduce the pose jump with the RealSense D455 settings shown. Check the initial delta-above-threshold warning and ground-constraint behavior; done means identifying the cause and documenting or validating a reliable correction.

Written by the indexing model from the issue text.

Assessment

Tech stack
cpp
Domain
computer-vision, robotics
Issue type
Bug
Difficulty
4/5
Estimated time
3-5 days
Activity status
Stale
Clarity
Needs clarification
Newbie friendliness
25/100

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.