Failed to initialize CUVSLAM tracker: 4

Robotics & Edge Computing Robotics - Isaac Isaac ROS
1

Hi,

I am using the latest Isaac ROS 3.0 cuVSLAM. I run the following command:
ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_core.launch.py
I modified the launch file to :

# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# SPDX-License-Identifier: Apache-2.0

from typing import Any, Dict

from isaac_ros_examples import IsaacROSLaunchFragment
import launch
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


class IsaacROSVisualSlamLaunchFragment(IsaacROSLaunchFragment):

    @staticmethod
    def get_interface_specs() -> Dict[str, Any]:
        return {
            'camera_resolution': {'width': 1280, 'height': 800}
        }

    @staticmethod
    def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]:

        enable_image_denoising = LaunchConfiguration('enable_image_denoising')
        rectified_images = LaunchConfiguration('rectified_images')
        enable_slam_visualization = LaunchConfiguration(
            'enable_slam_visualization')
        enable_landmarks_view = LaunchConfiguration('enable_landmarks_view')
        enable_observations_view = LaunchConfiguration(
            'enable_observations_view')
        camera_optical_frames = LaunchConfiguration('camera_optical_frames')
        base_frame = LaunchConfiguration('base_frame')
        num_cameras = LaunchConfiguration('num_cameras')
        enable_imu_fusion = LaunchConfiguration('enable_imu_fusion')
        imu_frame = LaunchConfiguration('imu_frame')
        gyro_noise_density = LaunchConfiguration('gyro_noise_density')
        gyro_random_walk = LaunchConfiguration('gyro_random_walk')
        accel_noise_density = LaunchConfiguration('accel_noise_density')
        accel_random_walk = LaunchConfiguration('accel_random_walk')
        calibration_frequency = LaunchConfiguration('calibration_frequency')
        enable_debug_mode = LaunchConfiguration('enable_debug_mode')
        image_jitter_threshold_ms = LaunchConfiguration(
            'image_jitter_threshold_ms')

        return {
            'image_format_converter_node_left': ComposableNode(
                package='isaac_ros_image_proc',
                plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
                name='image_format_node_left',
                parameters=[{
                        'encoding_desired': 'mono8',
                        'image_width': interface_specs['camera_resolution']['width'],
                        'image_height': interface_specs['camera_resolution']['height'],
                }],
                remappings=[
                    ('image_raw', 'left/image_rect'),
                    ('image', 'left/image_rect_mono')]
            ),
            'image_format_converter_node_right': ComposableNode(
                package='isaac_ros_image_proc',
                plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
                name='image_format_node_right',
                parameters=[{
                        'encoding_desired': 'mono8',
                        'image_width': interface_specs['camera_resolution']['width'],
                        'image_height': interface_specs['camera_resolution']['height'],
                }],
                remappings=[
                    ('image_raw', 'right/image_rect'),
                    ('image', 'right/image_rect_mono')]
            ),
            'visual_slam_node': ComposableNode(
                name='visual_slam_node',
                package='isaac_ros_visual_slam',
                plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
                parameters=[{
                    'enable_image_denoising': enable_image_denoising,
                    'enable_debug_mode': enable_debug_mode,
                    'rectified_images': rectified_images,
                    'enable_slam_visualization': enable_slam_visualization,
                    'enable_landmarks_view': enable_landmarks_view,
                    'enable_observations_view': enable_observations_view,
                    'camera_optical_frames': camera_optical_frames,
                    'base_frame': base_frame,
                    'num_cameras': num_cameras,
                    'enable_imu_fusion': enable_imu_fusion,
                    'imu_frame': imu_frame,
                    'gyro_noise_density': gyro_noise_density,
                    'gyro_random_walk': gyro_random_walk,
                    'accel_noise_density': accel_noise_density,
                    'accel_random_walk': accel_random_walk,
                    'calibration_frequency': calibration_frequency,
                    'image_jitter_threshold_ms': image_jitter_threshold_ms,
                }],
                remappings=[
                    ('/visual_slam/image_0', 'left/image_rect_mono'),
                    ('/visual_slam/camera_info_0', 'left/camera_info_rect'),
                    ('/visual_slam/image_1', 'right/image_rect_mono'),
                    ('/visual_slam/camera_info_1', 'right/camera_info_rect'),

                ]
            )
        }

    @staticmethod
    def get_launch_actions(interface_specs: Dict[str, Any]) -> \
            Dict[str, launch.actions.OpaqueFunction]:
        return {
            'enable_image_denoising': DeclareLaunchArgument(
                'enable_image_denoising',
                default_value='False'
            ),
            'rectified_images': DeclareLaunchArgument(
                'rectified_images',
                default_value='True'
            ),
            'enable_slam_visualization': DeclareLaunchArgument(
                'enable_slam_visualization',
                default_value='True'
            ),
            'enable_landmarks_view': DeclareLaunchArgument(
                'enable_landmarks_view',
                default_value='True'
            ),
            'enable_observations_view': DeclareLaunchArgument(
                'enable_observations_view',
                default_value='True'
            ),
            'camera_optical_frames': DeclareLaunchArgument(
                'camera_optical_frames',
                default_value='[oak_left_camera_optical_frame, \
                                oak_right_camera_optical_frame, \
                                ]'
            ),
            'base_frame': DeclareLaunchArgument(
                'base_frame',
                default_value='oak-d-base-frame'
            ),
            'num_cameras': DeclareLaunchArgument(
                'num_cameras',
                default_value='2'
            ),
            'enable_debug_mode': DeclareLaunchArgument(
                'enable_debug_mode',
                default_value='True'
            ),
            'enable_imu_fusion': DeclareLaunchArgument(
                'enable_imu_fusion',
                default_value='False'
            ),
            'imu_frame': DeclareLaunchArgument(
                'imu_frame',
                default_value='camera_gyro_optical_frame'
            ),
            'gyro_noise_density': DeclareLaunchArgument(
                'gyro_noise_density',
                default_value='0.000244'
            ),
            'gyro_random_walk': DeclareLaunchArgument(
                'gyro_random_walk',
                default_value='0.000019393'
            ),
            'accel_noise_density': DeclareLaunchArgument(
                'accel_noise_density',
                default_value='0.001862'
            ),
            'accel_random_walk': DeclareLaunchArgument(
                'accel_random_walk',
                default_value='0.003'
            ),
            'calibration_frequency': DeclareLaunchArgument(
                'calibration_frequency',
                default_value='200.0'
            ),
            'image_jitter_threshold_ms': DeclareLaunchArgument(
                'image_jitter_threshold_ms',
                default_value='34.0'
            )
        }


def generate_launch_description():
    """Launch file to bring up visual slam node standalone."""
    interface_specs = IsaacROSVisualSlamLaunchFragment.get_interface_specs()
    visual_slam_launch_container = ComposableNodeContainer(
        name='visual_slam_launch_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=IsaacROSVisualSlamLaunchFragment
        .get_composable_nodes(interface_specs).values(),
        output='screen'
    )

    # Retrieve and include the launch actions that declare launch arguments
    launch_actions = IsaacROSVisualSlamLaunchFragment.get_launch_actions(
        interface_specs).values()

    return launch.LaunchDescription([
        *launch_actions,  # This unpacks all launch actions to be part of the launch description
        visual_slam_launch_container
    ])

When I run my custom stereo camera in ROS2 and cuVSLAM, I get the following output. The slam doesn’t publish anything on any topic:

[INFO] [launch]: All log files can be found below /home/admin/.ros/log/2024-06-13-15-42-26-500096-ubuntu-132364
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [132389]
[component_container-1] [INFO] [1718307747.002534026] [visual_slam_launch_container]: Load Library: /opt/ros/humble/lib/libimage_format_converter_node.so
[component_container-1] [INFO] [1718307747.147382875] [visual_slam_launch_container]: Found class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::ImageFormatConverterNode>
[component_container-1] [INFO] [1718307747.147537108] [visual_slam_launch_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::ImageFormatConverterNode>
[component_container-1] [INFO] [1718307747.158645667] [image_format_node_left]: [NitrosNode] Initializing NitrosNode
[component_container-1] [INFO] [1718307747.159683800] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/std/libgxf_std.so
[component_container-1] [INFO] [1718307747.166837516] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_gxf_helpers.so
[component_container-1] [INFO] [1718307747.177159036] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_sight.so
[component_container-1] [INFO] [1718307747.189514455] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_atlas.so
[component_container-1] e[33m2024-06-13 15:42:27.216 WARN  gxf/std/program.cpp@532: No GXF scheduler specified.e[0m
[component_container-1] [INFO] [1718307747.217990767] [image_format_node_left]: [ImageFormatConverterNode] Set output data format to: "nitros_image_mono8"
[component_container-1] [INFO] [1718307747.218172967] [image_format_node_left]: [NitrosNode] Starting NitrosNode
[component_container-1] [INFO] [1718307747.232755620] [image_format_node_left]: [NitrosNode] Loading extensions
[component_container-1] [INFO] [1718307747.233116309] [image_format_node_left]: [NitrosContext] Loading extension: gxf/lib/multimedia/libgxf_multimedia.so
[component_container-1] [INFO] [1718307747.240606524] [image_format_node_left]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_message_compositor.so
[component_container-1] [INFO] [1718307747.242443151] [image_format_node_left]: [NitrosContext] Loading extension: gxf/lib/cuda/libgxf_cuda.so
[component_container-1] [INFO] [1718307747.250501693] [image_format_node_left]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_tensorops.so
[component_container-1] [INFO] [1718307747.274472594] [image_format_node_left]: [NitrosNode] Loading graph to the optimizer
[component_container-1] [INFO] [1718307747.277938689] [image_format_node_left]: [NitrosNode] Running optimization
[component_container-1] [INFO] [1718307747.381702440] [image_format_node_left]: [NitrosNode] Obtaining graph IO group info from the optimizer
[component_container-1] [INFO] [1718307747.401005696] [image_format_node_left]: [NitrosPublisherSubscriberGroup] Pinning the component "sink/sink" (type="nvidia::isaac_ros::MessageRelay") to use its compatible format only: "nitros_image_mono8"
[component_container-1] [INFO] [1718307747.411914647] [image_format_node_left]: [NitrosNode] Starting negotiation...
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/image_format_node_left' in container '/visual_slam_launch_container'
[component_container-1] [INFO] [1718307747.417255415] [visual_slam_launch_container]: Found class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::ImageFormatConverterNode>
[component_container-1] [INFO] [1718307747.417338644] [visual_slam_launch_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::image_proc::ImageFormatConverterNode>
[component_container-1] [INFO] [1718307747.424699680] [image_format_node_right]: [NitrosNode] Initializing NitrosNode
[component_container-1] [INFO] [1718307747.425750228] [image_format_node_right]: [ImageFormatConverterNode] Set output data format to: "nitros_image_mono8"
[component_container-1] [INFO] [1718307747.425849776] [image_format_node_right]: [NitrosNode] Starting NitrosNode
[component_container-1] [INFO] [1718307747.440124218] [image_format_node_right]: [NitrosNode] Loading extensions
[component_container-1] [INFO] [1718307747.440746304] [image_format_node_right]: [NitrosNode] Loading graph to the optimizer
[component_container-1] [INFO] [1718307747.442870183] [image_format_node_right]: [NitrosNode] Running optimization
[component_container-1] [INFO] [1718307747.546639374] [image_format_node_right]: [NitrosNode] Obtaining graph IO group info from the optimizer
[component_container-1] [INFO] [1718307747.567244079] [image_format_node_right]: [NitrosPublisherSubscriberGroup] Pinning the component "sink/sink" (type="nvidia::isaac_ros::MessageRelay") to use its compatible format only: "nitros_image_mono8"
[component_container-1] [INFO] [1718307747.570844441] [image_format_node_right]: [NitrosNode] Starting negotiation...
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/image_format_node_right' in container '/visual_slam_launch_container'
[component_container-1] [INFO] [1718307747.576730978] [visual_slam_launch_container]: Load Library: /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam/lib/libvisual_slam_node.so
[component_container-1] [INFO] [1718307747.688601622] [visual_slam_launch_container]: Found class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::visual_slam::VisualSlamNode>
[component_container-1] [INFO] [1718307747.688857996] [visual_slam_launch_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::visual_slam::VisualSlamNode>
[component_container-1] [INFO] [1718307747.703870679] [visual_slam_node.ManagedNitrosSubscriber]: Starting Managed Nitros Subscriber
[component_container-1] [INFO] [1718307747.737125351] [visual_slam_node]: cuVSLAM version: 12.2
[component_container-1] [INFO] [1718307747.922972289] [visual_slam_node]: Time taken by CUVSLAM_WarmUpGPU(): 0.185738
[component_container-1] [INFO] [1718307747.935222784] [image_format_node_left]: Negotiating
[component_container-1] [INFO] [1718307747.935319164] [image_format_node_left]: Could not negotiate
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/visual_slam_node' in container '/visual_slam_launch_container'
[component_container-1] [INFO] [1718307748.412900653] [image_format_node_left]: [NitrosNode] Starting post negotiation setup
[component_container-1] [INFO] [1718307748.413079013] [image_format_node_left]: [NitrosNode] Getting data format negotiation results
[component_container-1] [INFO] [1718307748.413136643] [image_format_node_left]: [NitrosPublisher] Negotiation ended with no results
[component_container-1] [INFO] [1718307748.413170273] [image_format_node_left]: [NitrosPublisher] Use only the compatible publisher: topic_name="/left/image_rect_mono", data_format="nitros_image_mono8"
[component_container-1] [INFO] [1718307748.413203520] [image_format_node_left]: [NitrosSubscriber] Negotiation ended with no results
[component_container-1] [INFO] [1718307748.413225823] [image_format_node_left]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/left/image_rect", data_format="nitros_image_rgb8"
[component_container-1] [INFO] [1718307748.414485290] [image_format_node_left]: [NitrosNode] Exporting the final graph based on the negotiation results
[component_container-1] [INFO] [1718307748.445780059] [image_format_node_left]: [NitrosNode] Wrote the final top level YAML graph to "/tmp/isaac_ros_nitros/graphs/LUFPOEFOIZ/LUFPOEFOIZ.yaml"
[component_container-1] [INFO] [1718307748.445943508] [image_format_node_left]: [NitrosNode] Loading application
[component_container-1] [INFO] [1718307748.452582459] [image_format_node_left]: [ImageFormatConverterNode] postLoadGraphCallback().
[component_container-1] [INFO] [1718307748.452831824] [image_format_node_left]: [NitrosNode] Initializing and running GXF graph
[component_container-1] [INFO] [1718307748.563881101] [image_format_node_left]: [NitrosNode] Node was started
[component_container-1] [INFO] [1718307748.571644420] [image_format_node_right]: [NitrosNode] Starting post negotiation setup
[component_container-1] [INFO] [1718307748.571744512] [image_format_node_right]: [NitrosNode] Getting data format negotiation results
[component_container-1] [INFO] [1718307748.571777278] [image_format_node_right]: [NitrosPublisher] Negotiation ended with no results
[component_container-1] [INFO] [1718307748.571802877] [image_format_node_right]: [NitrosPublisher] Use only the compatible publisher: topic_name="/right/image_rect_mono", data_format="nitros_image_mono8"
[component_container-1] [INFO] [1718307748.571822492] [image_format_node_right]: [NitrosSubscriber] Negotiation ended with no results
[component_container-1] [INFO] [1718307748.571839388] [image_format_node_right]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/right/image_rect", data_format="nitros_image_rgb8"
[component_container-1] [INFO] [1718307748.572193933] [image_format_node_right]: [NitrosNode] Exporting the final graph based on the negotiation results
[component_container-1] [INFO] [1718307748.596588802] [image_format_node_right]: [NitrosNode] Wrote the final top level YAML graph to "/tmp/isaac_ros_nitros/graphs/GZHXRSDTGJ/GZHXRSDTGJ.yaml"
[component_container-1] [INFO] [1718307748.596750844] [image_format_node_right]: [NitrosNode] Loading application
[component_container-1] [INFO] [1718307748.599500743] [image_format_node_right]: [ImageFormatConverterNode] postLoadGraphCallback().
[component_container-1] [INFO] [1718307748.599660224] [image_format_node_right]: [NitrosNode] Initializing and running GXF graph
[component_container-1] [INFO] [1718307748.633534212] [image_format_node_right]: [NitrosNode] Node was started
[component_container-1] [INFO] [1718307768.998694955] [visual_slam_node]: Initializing cuVSLAM.
[component_container-1] [INFO] [1718307768.999309676] [visual_slam_node]: Use use_gpu: true
[component_container-1] [INFO] [1718307768.999348618] [visual_slam_node]: Enable IMU Fusion: false
[component_container-1] [INFO] [1718307769.000831518] [visual_slam_node]: Time taken by CUVSLAM_CreateTracker(): 0.001464
[component_container-1] [ERROR] [1718307769.000916665] [visual_slam_node]: Failed to initialize CUVSLAM tracker: 4
[component_container-1] [INFO] [1718307769.047431705] [visual_slam_node]: Initializing cuVSLAM.
[component_container-1] [INFO] [1718307769.047624463] [visual_slam_node]: Use use_gpu: true
[component_container-1] [INFO] [1718307769.048175955] [visual_slam_node]: Enable IMU Fusion: false
[component_container-1] [INFO] [1718307769.048745878] [visual_slam_node]: Time taken by CUVSLAM_CreateTracker(): 0.000084
[component_container-1] [ERROR] [1718307769.048835153] [visual_slam_node]: Failed to initialize CUVSLAM tracker: 4
[component_container-1] [INFO] [1718307769.095387086] [visual_slam_node]: Initializing cuVSLAM.
[component_container-1] [INFO] [1718307769.095651073] [visual_slam_node]: Use use_gpu: true
[component_container-1] [INFO] [1718307769.095696350] [visual_slam_node]: Enable IMU Fusion: false
[component_container-1] [INFO] [1718307769.095759483] [visual_slam_node]: Time taken by CUVSLAM_CreateTracker(): 0.000037
[component_container-1] [ERROR] [1718307769.095791514] [visual_slam_node]: Failed to initialize CUVSLAM tracker: 4
[component_container-1] [INFO] [1718307769.139596197] [visual_slam_node]: Initializing cuVSLAM.
[component_container-1] [INFO] [1718307769.139795195] [visual_slam_node]: Use use_gpu: true
[component_container-1] [INFO] [1718307769.139817593] [visual_slam_node]: Enable IMU Fusion: false
[component_container-1] [INFO] [1718307769.139866487] [visual_slam_node]: Time taken by CUVSLAM_CreateTracker(): 0.000035

When I run my ROS2 camera driver I am getting a message that says left and right camera are not synchronized. Is that the cause of this error?

Tf graph:

Screenshot from 2024-06-13 21-01-28
Screenshot from 2024-06-13 21-01-281604×732 44.4 KB

Edit 1: I was able to get rid of the error Failed to initialize CUVSLAM tracker: 4 by increasing the number of camera to 2 in the launch file and by giving synchronized images. But now I have a warning and there are empty messages being published on the visualization topics. The warning is :
[component_container-1] [WARN] [1718395624.462606101] [visual_slam_node]: Delta between current and previous frame [66.669074 ms] is above threshold [34.000000 ms]

Edit 2 : I got few points but they were very less and when I moved the camera a little bit the slam path was very large and still getting the warning above. This time I gave synchronized frames at around 20fps.
How can I improve performance?

Screenshot from 2024-06-14 16-27-41
Screenshot from 2024-06-14 16-27-41627×583 7.46 KB

Screenshot from 2024-06-14 16-33-50
Screenshot from 2024-06-14 16-33-50716×657 4.19 KB

Can someone please explain how does cuVSLAM gets the baseline?

2

Hi, you could try different values for the sync_matching_threshold_ms parameter which defines the maximum duration between image timestamps to be considered in sync.

The number of cameras by default is set to 2, which represents a single stereo camera.

3

I changed the parameter you suggested to 200 but I am still getting the warning. Do you have idea about why the path and points are moving a lot even when I am not moving the camera? I set the fixed frame in Rviz to Map, odom and base frame but still the map is not static.

warning:


[component_container-1] [WARN] [1718411190.308811967] [visual_slam_node]: Delta between current and previous frame [133.333406 ms] is above threshold [34.000000 ms]

Updated launch file:

# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# SPDX-License-Identifier: Apache-2.0

from typing import Any, Dict

from isaac_ros_examples import IsaacROSLaunchFragment
import launch
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


class IsaacROSVisualSlamLaunchFragment(IsaacROSLaunchFragment):

    @staticmethod
    def get_interface_specs() -> Dict[str, Any]:
        return {
            'camera_resolution': {'width': 1280, 'height': 800}
        }

    @staticmethod
    def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]:

        enable_image_denoising = LaunchConfiguration('enable_image_denoising')
        rectified_images = LaunchConfiguration('rectified_images')
        verbosity = LaunchConfiguration('verbosity')
        enable_slam_visualization = LaunchConfiguration(
            'enable_slam_visualization')
        enable_landmarks_view = LaunchConfiguration('enable_landmarks_view')
        enable_observations_view = LaunchConfiguration(
            'enable_observations_view')
        publish_odom_to_rig_tf = LaunchConfiguration('publish_odom_to_rig_tf')
        camera_optical_frames = LaunchConfiguration('camera_optical_frames')
        base_frame = LaunchConfiguration('base_frame')
        num_cameras = LaunchConfiguration('num_cameras')
        enable_imu_fusion = LaunchConfiguration('enable_imu_fusion')
        imu_frame = LaunchConfiguration('imu_frame')
        publish_map_to_odom_tf = LaunchConfiguration('publish_map_to_odom_tf')
        gyro_noise_density = LaunchConfiguration('gyro_noise_density')
        gyro_random_walk = LaunchConfiguration('gyro_random_walk')
        accel_noise_density = LaunchConfiguration('accel_noise_density')
        accel_random_walk = LaunchConfiguration('accel_random_walk')
        calibration_frequency = LaunchConfiguration('calibration_frequency')
        enable_debug_mode = LaunchConfiguration('enable_debug_mode')
        image_jitter_threshold_ms = LaunchConfiguration(
            'image_jitter_threshold_ms')
        sync_matching_threshold_ms = LaunchConfiguration(
            'sync_matching_threshold_ms')

        return {
            'image_format_converter_node_left': ComposableNode(
                package='isaac_ros_image_proc',
                plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
                name='image_format_node_left',
                parameters=[{
                        'encoding_desired': 'mono8',
                        'image_width': interface_specs['camera_resolution']['width'],
                        'image_height': interface_specs['camera_resolution']['height'],
                }],
                remappings=[
                    ('image_raw', 'left/image_rect'),
                    ('image', 'left/image_rect_mono')]
            ),
            'image_format_converter_node_right': ComposableNode(
                package='isaac_ros_image_proc',
                plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
                name='image_format_node_right',
                parameters=[{
                        'encoding_desired': 'mono8',
                        'image_width': interface_specs['camera_resolution']['width'],
                        'image_height': interface_specs['camera_resolution']['height'],
                }],
                remappings=[
                    ('image_raw', 'right/image_rect'),
                    ('image', 'right/image_rect_mono')]
            ),
            'visual_slam_node': ComposableNode(
                name='visual_slam_node',
                package='isaac_ros_visual_slam',
                plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
                parameters=[{
                    'enable_image_denoising': enable_image_denoising,
                    'enable_debug_mode': enable_debug_mode,
                    'rectified_images': rectified_images,
                    'verbosity': verbosity,
                    'enable_slam_visualization': enable_slam_visualization,
                    'enable_landmarks_view': enable_landmarks_view,
                    'enable_observations_view': enable_observations_view,
                    'camera_optical_frames': camera_optical_frames,
                    'base_frame': base_frame,
                    'num_cameras': num_cameras,
                    'publish_map_to_odom_tf': publish_map_to_odom_tf,
                    'sync_matching_threshold_ms': sync_matching_threshold_ms,
                    'enable_imu_fusion': enable_imu_fusion,
                    'imu_frame': imu_frame,
                    'publish_odom_to_rig_tf': publish_odom_to_rig_tf,
                    'gyro_noise_density': gyro_noise_density,
                    'gyro_random_walk': gyro_random_walk,
                    'accel_noise_density': accel_noise_density,
                    'accel_random_walk': accel_random_walk,
                    'calibration_frequency': calibration_frequency,
                    'image_jitter_threshold_ms': image_jitter_threshold_ms,
                }],
                remappings=[
                    ('/visual_slam/image_0', '/left/image_rect_mono'),
                    ('/visual_slam/camera_info_0', '/left/camera_info_rect'),
                    ('/visual_slam/image_1', '/right/image_rect_mono'),
                    ('/visual_slam/camera_info_1', '/right/camera_info_rect'),

                ]
            )
        }

    @staticmethod
    def get_launch_actions(interface_specs: Dict[str, Any]) -> \
            Dict[str, launch.actions.OpaqueFunction]:
        return {
            'enable_image_denoising': DeclareLaunchArgument(
                'enable_image_denoising',
                default_value='False'
            ),
            'rectified_images': DeclareLaunchArgument(
                'rectified_images',
                default_value='True'
            ),
            'enable_slam_visualization': DeclareLaunchArgument(
                'enable_slam_visualization',
                default_value='True'
            ),
            'enable_landmarks_view': DeclareLaunchArgument(
                'enable_landmarks_view',
                default_value='True'
            ),
            'enable_observations_view': DeclareLaunchArgument(
                'enable_observations_view',
                default_value='True'
            ),
            'verbosity': DeclareLaunchArgument(
                'verbosity', default_value='1'
            ),
            'publish_odom_to_rig_tf': DeclareLaunchArgument(
                'publish_odom_to_rig_tf', default_value='True'
            ),
            'publish_map_to_odom_tf': DeclareLaunchArgument(
                'publish_map_to_odom_tf', default_value='True'
            ),
            'camera_optical_frames': DeclareLaunchArgument(
                'camera_optical_frames',
                default_value='[oak_left_camera_optical_frame, \
                                oak_right_camera_optical_frame, \
                                ]'
            ),
            'base_frame': DeclareLaunchArgument(
                'base_frame',
                default_value='oak-d-base-frame'
            ),
            'num_cameras': DeclareLaunchArgument(
                'num_cameras',
                default_value='2'
            ),
            'enable_debug_mode': DeclareLaunchArgument(
                'enable_debug_mode',
                default_value='False'
            ),
            'enable_imu_fusion': DeclareLaunchArgument(
                'enable_imu_fusion',
                default_value='False'
            ),
            'imu_frame': DeclareLaunchArgument(
                'imu_frame',
                default_value='camera_gyro_optical_frame'
            ),
            'sync_matching_threshold_ms': DeclareLaunchArgument(
                'sync_matching_threshold_ms', default_value='200.0'
            ),
            'gyro_noise_density': DeclareLaunchArgument(
                'gyro_noise_density',
                default_value='0.000244'
            ),
            'gyro_random_walk': DeclareLaunchArgument(
                'gyro_random_walk',
                default_value='0.000019393'
            ),
            'accel_noise_density': DeclareLaunchArgument(
                'accel_noise_density',
                default_value='0.001862'
            ),
            'accel_random_walk': DeclareLaunchArgument(
                'accel_random_walk',
                default_value='0.003'
            ),
            'calibration_frequency': DeclareLaunchArgument(
                'calibration_frequency',
                default_value='200.0'
            ),
            'image_jitter_threshold_ms': DeclareLaunchArgument(
                'image_jitter_threshold_ms',
                default_value='34.0'
            )
        }


def generate_launch_description():
    """Launch file to bring up visual slam node standalone."""
    interface_specs = IsaacROSVisualSlamLaunchFragment.get_interface_specs()
    visual_slam_launch_container = ComposableNodeContainer(
        name='visual_slam_launch_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=IsaacROSVisualSlamLaunchFragment
        .get_composable_nodes(interface_specs).values(),
        output='screen'
    )

    # Retrieve and include the launch actions that declare launch arguments
    launch_actions = IsaacROSVisualSlamLaunchFragment.get_launch_actions(
        interface_specs).values()

    return launch.LaunchDescription([
        *launch_actions,  # This unpacks all launch actions to be part of the launch description
        visual_slam_launch_container
    ])
4

Hi @ssharma4

I will get back to you as soon as possible. I will forward your reply to the engineers for a more detailed response.

5

First suggestion: use a simple startup file without fragments to ensure that all parameters are defined correctly.
This launch file can be an example:

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

def generate_launch_description():

    visual_slam_node = ComposableNode(
        name='visual_slam_node',
        package='isaac_ros_visual_slam',
        plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
        parameters=[{
            
            'rectified_images': True,

            'enable_imu_fusion': False,
            'debug_imu_mode': False,
            'enable_debug_mode': False,

            'enable_slam_visualization': False, # visualization may affects the performance
            'enable_landmarks_view': False, # visualization may affects the performance
            'enable_observations_view': False, # visualization may affects the performance

            'enable_localization_n_mapping': True,

            'image_jitter_threshold_ms': 35.00, # for 30 FPS
            'sync_matching_threshold_ms': 1.0, # desync in ms between different cams

            'num_cameras': 2, # 2 cams within one stereo camera
            'base_frame': 'oak-d-base-frame',
            'verbocity': 3,

            'camera_optical_frames': [
                'oak_left_camera_optical_frame',
                'oak_right_camera_optical_frame',
            ],
        }],
        remappings=[
            ('visual_slam/image_0', 'left/image_rect'),
            ('visual_slam/camera_info_0', 'left/camera_info_rect'),
            ('visual_slam/image_1', 'right/image_rect'),
            ('visual_slam/camera_info_1', 'right/camera_info_rect'),
        ],
    )

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

    return launch.LaunchDescription([visual_slam_launch_container])

About this warning:

[component_container-1] [WARN] [1718411190.308811967] [visual_slam_node]: Delta between current and previous frame [133.333406 ms] is above threshold [34.000000 ms]

It’s okay to see them occasionally, especially at the beginning. However, if they appear frequently, it indicates issues with delivering images to the visual slam ROS node. In this case, you should investigate problems in the ROS node of the camera or hardware, such as issues with the camera cable or PC overload.

Unfortunately, the provided error code means it is impossible to initialize a valid stereo pair due to various reasons.

My suggestions\next steps:

  1. reduce the sync_matching_threshold_ms to small number: 1ms (should be as small as possible, not 200 ms, since this is the timing criterion for images being marked as “synchronized”)
  2. remove the image encoding nodes and provide the correct topic names for a visual slam node
  3. make sure the stereo camera sees a clear, feature-rich scene with both eyes (no exposure\light issues)
  4. check the transform tree provided by the camera’s ros-node

if these suggestions do not work, then we need to request a rosbag (with some smooth camera movement) with the following topics: left\right imgs, left\right camera infos, tf, tf_static

6

Hi @Raffaello ,

Sorry for the late reply. I was busy writing ROS 2 driver for the camera I have.

The given suggestions didn’t work. The following is the link to the Rosbag file:
Link to the folder

Please help me debug and use Isaac cuVSLAM.

7

Hi @ssharma4

I forwarded your rosbag to the engineering to determine why your error occurred.
I will keep you posted.

Raffaello

8

Hi @ssharma4

As a result of the analysis of the runway, I came to the following conclusions:

Some notes about the provided rosbag:

  1. It was not launch the depthai_descriptions package, that’s why no valid tf_tree was published, and cuvslam was unable to operate correctly

    image
    image564×655 52.1 KB

  2. the record has significant problems with image and camera info topics:

    1. all topics have a different number of msgs (we expect the same number of msgs for these topics for a one stereocamera)
    2. There are floating intervals between pictures (both smaller and larger relative to the base interval of 33ms (30fps).
      oak-d_stereocam_time_intervals
      oak-d_stereocam_time_intervals1440×1073 146 KB

The evaluation procedure required to replay the rosbag file and run the following ROS packages:

  1. depthai_description ROS package (default params for ‘OAK-D’ camera model) to get the valid tf-tree
  2. isaac_ros_visual_slam ROS package with the following params:
parameters=[{
           'enable_image_denoising': False,
           'rectified_images': True,
           'enable_imu_fusion': False,
           'image_jitter_threshold_ms': 40.00,
           'base_frame': 'oak-d-base-frame',
           'enable_slam_visualization': False,
           'enable_landmarks_view': False,
           'enable_observations_view': False,
           'camera_optical_frames': [
               'oak_left_camera_optical_frame',
               'oak_right_camera_optical_frame',
           ],
       }],
       remappings=[
           ('visual_slam/image_0', '/left/image_rect'),
           ('visual_slam/camera_info_0', '/left/camera_info'),
           ('visual_slam/image_1', '/right/image_rect'),
           ('visual_slam/camera_info_1', '/right/camera_info'),
       ],
  1. the result is presented in the video (pay attention to frequent messages about the absence of synchronized pairs of pictures from the visual slam node):

[component_container_mt-1] [WARN] [1721400163.878483725] [visual_slam_node]: Delta between current and previous frame [477.324234 ms] is above threshold [40.000000 ms]
[component_container_mt-1] [WARN] [1721400163.904459300] [visual_slam_node]: Delta between current and previous frame [55.178628 ms] is above threshold [40.000000 ms]
[component_container_mt-1] [WARN] [1721400164.403431472] [visual_slam_node]: Delta between current and previous frame [266.966356 ms] is above threshold [40.000000 ms]
[component_container_mt-1] [WARN] [1721400164.420050234] [visual_slam_node]: Delta between current and previous frame [234.416554 ms] is above threshold [40.000000 ms]
[component_container_mt-1] [WARN] [1721400164.921137681] [visual_slam_node]: Delta between current and previous frame [432.413352 ms] is above threshold [40.000000 ms]

Conclusion:

  1. the demo demonstrates that isaac_ros_visual_slam is capable of working on provided rosbag, if the correct tf-tree and image/camera_info topics are provided,

  2. we do not expect a good performance on input data with a synchronization issue between the left and right frames of a stereo pair (as indicated by intervals between frames less than 33ms) and frame drops (intervals between frames significantly greater than 33ms).

9

@Raffaello
Thank you for such a quick reply. As per the documentation here, I am providing three frames that are required at the bare minimum, such as the left and right optical frames and base_link. What frames am I missing? Also, why does CuVSLAM need a TF topic. I can understand that it needs tf_static.

Screenshot 2024-07-19 152133
Screenshot 2024-07-19 152133704×478 33 KB

I also wanted to know how does the cuVSLAM algorithm calculates the baseline. Is it through a projection matrix or tf_static frames?

I am publishing the left and right frames with the same time stamp in a while loop, so it must be synchronized. I will take your word for it that it is unsynchronized. So unsynchronization is different than the time difference between two consecutive frames?

Is the issue 'sync_matching_threshold_ms': 1.0, or this 'image_jitter_threshold_ms': 40.00,? The first is synchronization but the error you talked about seems to indicate jitter problem, which is Acceptable time delta between consecutive synced image messages. If the messages exceed the threshold, it might be an indication of message drop from the source.

10

Hi @ssharma4, I’m back with all replies:

Yes, you should provide these frames via tf_static topic.
Please, provide the valid tf-tree using as the reference the official Oak-d camera repository or the Hawk camera (publishing the same bare minimum of tf-frame for isaac ros visual slam)

Tf frames, please explore the initialization step in the source code of the ROS node.

The plot with a time intervals from our previous answer is based on the analysis of the time stamps of image msgs from the rosbag that you provided. Your responsibility is to provide synchronized stereo pairs of images with a minimum constant FPS of 30 frames. This condition is obviously not met. Please, make a control of the output result of your code.

Best,
Raffaello