ros2 run orbslam3 rgbd crashes after Camera 0 is pinhole

Hi there,

I have trouble running the ROS 2 nodes.

Issue

The rgbd and the stereo node both crash after I run them.

Setup

I have set up a Dockerfile to create an environment with all the necessary dependencies for ORB-SLAM3. After compiling ORB-SLAM3, I proceed to clone the ORB_SLAM3_ROS2 repo from Github and then I switch to the humble branch since my Docker environment is running ROS 2 Humble.

I configure the path to my ORB-SLAM3 installation and I use colcon build to create the orbslam3 package. I then source the workspace.

Expected behaviour

When I run ros2 run orbslam3 rgbd <PATH_TO_VOC_FILE> <PATH_TO_CAMERA_CONFIG> --ros-args <topic remappings> I expect that the node should initiate the parameters from the indicated config file and then start applying SLAM.

Actual behaviour

When I run the following command, I can see that it is loading the config file. I put everything in a launch file, but the result is the same when I run it with ros2 run and all the parameters.

The node shuts down with the following terminal output:

root@adminuser-Precision-5560:/home/orb/ros2_ws# ros2 launch leorover_orbslam3 orbslam3_rgbd.launch.py 
[INFO] [launch]: All log files can be found below /root/.ros/log/2023-08-04-12-52-17-895346-adminuser-Precision-5560-537
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rgbd-1]: process started with pid [538]
[rgbd-1] 
[rgbd-1] ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
[rgbd-1] ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
[rgbd-1] This program comes with ABSOLUTELY NO WARRANTY;
[rgbd-1] This is free software, and you are welcome to redistribute it
[rgbd-1] under certain conditions. See LICENSE.txt.
[rgbd-1] 
[rgbd-1] Input sensor was set to: RGB-D
[rgbd-1] 
[rgbd-1] Loading ORB Vocabulary. This could take a while...
[rgbd-1] Vocabulary loaded!
[rgbd-1] 
[rgbd-1] Initialization of Atlas from scratch 
[rgbd-1] Creation of new map with id: 0
[rgbd-1] Creation of new map with last KF id: 0
[rgbd-1] Seq. Name: 
[rgbd-1] 
[rgbd-1] Camera Parameters: 
[rgbd-1] - Camera: Pinhole
[rgbd-1] - Image scale: 0.5
[rgbd-1] - fx: 308.6
[rgbd-1] - fy: 308.681
[rgbd-1] - cx: 162.318
[rgbd-1] - cy: 121.231
[rgbd-1] - k1: 0
[rgbd-1] - k2: 0
[rgbd-1] - p1: 0
[rgbd-1] - p2: 0
[rgbd-1] - fps: 30
[rgbd-1] - color order: RGB (ignored if grayscale)
[rgbd-1] 
[rgbd-1] Depth Threshold (Close/Far Points): 2.98185
[rgbd-1] 
[rgbd-1] ORB Extractor Parameters: 
[rgbd-1] - Number of Features: 1250
[rgbd-1] - Scale Levels: 8
[rgbd-1] - Scale Factor: 1.2
[rgbd-1] - Initial Fast Threshold: 20
[rgbd-1] - Minimum Fast Threshold: 7
[rgbd-1] There are 1 cameras in the atlas
[rgbd-1] Camera 0 is pinhole
[rgbd-1] Shutdown
[rgbd-1] 
[rgbd-1] Saving keyframe trajectory to KeyFrameTrajectory.txt ...
[rgbd-1] double free or corruption (out)
[ERROR] [rgbd-1]: process has died [pid 538, exit code -6, cmd '/home/orb/ros2_ws/install/orbslam3/lib/orbslam3/rgbd /home/orb/ros2_ws/install/leorover_orbslam3/share/leorover_orbslam3/vocabulary/ORBvoc.txt /home/orb/ros2_ws/install/leorover_orbslam3/share/leorover_orbslam3/config/RealSense_D455.yaml --ros-args -r __node:=leorover_orbslam3_rgbd -r camera/rgb:=/leo02/realsense_camera_leo02/color/image_raw -r camera/depth:=/leo02/realsense_camera_leo02/aligned_depth_to_color/image_raw'].

Question

Can you help me figure out why the node shuts down after launching it? Am I missing something? Is there something wrong in how I run the node? Am I missing a configuration?

With kind regards,
Dave

same question, have you solved it?

oh, I have solved it.
just change these 2 lines

ORB_SLAM3_ROS2/src/rgbd/rgbd-slam-node.cpp

Lines 11 to 12 in 00c5433

    
           rgb_sub = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/rgb"); 
        
           depth_sub = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/depth");

to

    rgb_sub = std::make_shared< message_filters::Subscriber<ImageMsg> >(this, "camera/rgb");
    depth_sub = std::make_shared< message_filters::Subscriber<ImageMsg> >(this, "camera/depth");

I had the same issue, and the solution from @HUANG-Haolun helped me. Thanks!

	rgb_sub = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/rgb");
	depth_sub = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/depth");