Failed to initialize CUVSLAM tracker: 4

ssharma4 · June 13, 2024, 8:38pm

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:

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?

Can someone please explain how does cuVSLAM gets the baseline?

asawareeb · June 14, 2024, 10:18pm

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.

ssharma4 · June 14, 2024, 10:25pm

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
    ])

Raffaello · June 27, 2024, 10:09am

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.

Raffaello · June 27, 2024, 4:13pm

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:

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”)
remove the image encoding nodes and provide the correct topic names for a visual slam node
make sure the stereo camera sees a clear, feature-rich scene with both eyes (no exposure\light issues)
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

ssharma4 · July 18, 2024, 1:02am

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.

Raffaello · July 18, 2024, 5:25pm

Hi @ssharma4

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

Raffaello