- 
                Notifications
    
You must be signed in to change notification settings  - Fork 168
 
Description
Problem Description
The graph displays visual odometry keypoints number over time (topic /visual_slam/vis/observations_cloud)
I'm getting frequent periodic keypoint drops (every ~1.7s), doesn't matter if I use 1 or 4 realsense cameras, indoors or outdoors, standing or moving. I'm woundering if it's expected VO behaviour or maybe It can be fixed or improved somehow?
When keypoints drop below certain threshold the odometry data is unreliable. Here is a graph, comparing odometry_speed VS gps_speed when the number of keypoints drops below ~80 - odom speed is completely off, and odom twist variance spikes only for a short period of time. Is there a reliable way to know if odometry is unreliable?
Points num is scaled (x/100).
Launch Parameters
        name        = 'visual_slam_node',
        namespace   = '',
        package     = 'isaac_ros_visual_slam',
        plugin      = 'nvidia::isaac_ros::visual_slam::VisualSlamNode',
        parameters  = [{
            'enable_image_denoising'        : False,
            'rectified_images'              : True,
            'map_frame'                     : 'odom_map',
            'odom_frame'                    : 'odom',
            'base_frame'                    : 'base_footprint',
            'enable_localization_n_mapping' : False,
            'enable_ground_constraint_in_odometry': False,
            'publish_odom_to_base_tf'       : True,
            'publish_map_to_odom_tf'        : True,
            'override_publishing_stamp'     : True,
            'num_cameras'                   : 8,
            'sync_matching_threshold_ms'    : 30.0, 
            'image_jitter_threshold_ms'     : 35.0,
            'multicam_mode'                 : 1,
            'min_num_images'                : 6,
            'enable_slam_visualization'     : True,
            'enable_observations_view'      : True,
            'enable_landmarks_view'         : True,
            'camera_optical_frames'         : [
                'front_camera_infra1_optical_frame',
                'front_camera_infra2_optical_frame',
                'back_camera_infra1_optical_frame',
                'back_camera_infra2_optical_frame',
                'right_camera_infra1_optical_frame',
                'right_camera_infra2_optical_frame',
                'left_camera_infra1_optical_frame',
                'left_camera_infra2_optical_frame',
            ],
        }],
        remappings  = [
            ('visual_slam/image_0'      , 'front_camera/infra1/image_rect_raw'),
            ('visual_slam/camera_info_0', 'front_camera/infra1/camera_info'),
            ('visual_slam/image_1'      , 'front_camera/infra2/image_rect_raw'),
            ('visual_slam/camera_info_1', 'front_camera/infra2/camera_info'),
            ('visual_slam/image_2'      , 'back_camera/infra1/image_rect_raw'),
            ('visual_slam/camera_info_2', 'back_camera/infra1/camera_info'),
            ('visual_slam/image_3'      , 'back_camera/infra2/image_rect_raw'),
            ('visual_slam/camera_info_3', 'back_camera/infra2/camera_info'),
            ('visual_slam/image_4'      , 'right_camera/infra1/image_rect_raw'),
            ('visual_slam/camera_info_4', 'right_camera/infra1/camera_info'),
            ('visual_slam/image_5'      , 'right_camera/infra2/image_rect_raw'),
            ('visual_slam/camera_info_5', 'right_camera/infra2/camera_info'),
            ('visual_slam/image_6'      , 'left_camera/infra1/image_rect_raw'),
            ('visual_slam/camera_info_6', 'left_camera/infra1/camera_info'),
            ('visual_slam/image_7'      , 'left_camera/infra2/image_rect_raw'),
            ('visual_slam/camera_info_7', 'left_camera/infra2/camera_info'),
            ('visual_slam/imu', 'front_camera/imu')
        ]
    )Hardware
Jetson Orin NX
Realsense Cameras D455 (4x)
Jetpack 6.0, L4T 36.3.0
Nvidia Isaac Ros Visual Slam v3.0.0