publishMapPointCloud() still in progress?

NilsRublein commented 2 weeks ago

Hey @suchetanrs, thank you for your great work!

I would like to use your version of ORB SLAM3 to publish the map points however I noticed that the following lines in rgbd-slam-node.cpp are commented:

publishMapPointCloud();
std::thread(&RgbdSlamNode::publishMapPointCloud, this).detach();

I uncommented the lines and made 2 observations:

publishing the map data seriously slows down the tracking, which seems to result in loss of tracking. Without the publishing of the map points, the tracking runs at approx. 30Hz.
'The KF was not found in allKFs_. This should not happen'. This bug was also reported in issue #13.

I am therefore wondering if your map point publisher is still work in progress and whether you have made any attempts to solve issue #13.

Below you can find a log from my experiment as well as some details about my system setup.

[rgbd-1] First KF:0; Map init KF:0
[rgbd-1] New Map created with 843 points
[rgbd-1] [INFO] [1724769923.188128070] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 28.002 frames / sec
[rgbd-1] [INFO] [1724769923.188399570] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769924.188168177] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 23.0007 frames / sec
[rgbd-1] [INFO] [1724769924.189291284] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769925.188172923] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 18.0015 frames / sec
[rgbd-1] [INFO] [1724769925.188514149] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769926.188155301] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 18.0012 frames / sec
[rgbd-1] [INFO] [1724769926.188562151] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769927.188231701] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 13.9997 frames / sec
[rgbd-1] [INFO] [1724769927.189519829] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769928.188283028] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 15.0007 frames / sec
[rgbd-1] [INFO] [1724769928.188907571] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769929.188328212] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 12 frames / sec
[rgbd-1] [INFO] [1724769929.189379042] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] [INFO] [1724769930.188232490] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 11.0017 frames / sec
[rgbd-1] [INFO] [1724769930.190822590] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()
[rgbd-1] virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()
[rgbd-1] virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] [INFO] [1724768942.634185556] [robot_0.ORB_SLAM3_RGBD_ROS2]: Current ORB-SLAM3 tracking frequency: 0 frames / sec
[rgbd-1] [INFO] [1724768942.635951838] [robot_0.ORB_SLAM3_RGBD_ROS2]: *************************
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Fail to track local map!
[rgbd-1] ORB-SLAM failed: Tracking LOST.
[rgbd-1] Creation of new map with id: 1
[rgbd-1] Stored map with ID: 0
[rgbd-1] Creation of new map with last KF id: 31
[rgbd-1] ORB-SLAM failed: No images yet.
[rgbd-1] First KF:31; Map init KF:31
[rgbd-1] New Map created with 1048 points
[rgbd-1] Fail to track local map!
[rgbd-1] LM: Active map reset recieved
[rgbd-1] LM: Active map reset, waiting...
[rgbd-1] LM: Reseting current map in Local Mapping...
[rgbd-1] LM: End reseting Local Mapping...
[rgbd-1] LM: Reset free the mutex
[rgbd-1] LM: Active map reset, Done!!!
[rgbd-1] mnFirstFrameId = 0
[rgbd-1] mnInitialFrameId = 254
[rgbd-1] 0 Frames set to lost
[rgbd-1] First KF:32; Map init KF:31
[rgbd-1] New Map created with 1068 points
[rgbd-1] terminate called after throwing an instance of 'std::runtime_error'
[rgbd-1]   what():  The KF was not found in allKFs_. This should not happen
[ERROR] [rgbd-1]: process has died [pid 1225, exit code -6, cmd '/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd /home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt /root/colcon_ws/src/orb_slam3_ros2_wrapper/params/RealSense_D435i.yaml --ros-args -r __ns:=/robot_0 --params-file /tmp/tmpwqxzsal_'].

About my setup:

no_odom_mode set to true
I am using the inertial mode via trackRGBDi()
I am using VINS-RGBD's Normal.bag (converted to a ROS2 bag) and the corresponding camera.yaml file (RealSense_d435i).

suchetanrs commented 2 weeks ago

Hi @NilsRublein ! Thanks a lot for your ticket. In short I can tell you that I have not yet made attempts to sort out #13 . This has been a persistent issue and I will take out some time today / tomorrow to fix this on priority. To address the issue with the decrease in frequency, it is because as the number of keyframes increases, the pointcloud size becomes excessively huge to create and publish. I have a small fix for this. It is to publish everytime only the latest keyframe's pointcloud and not the whole map's pointcloud everytime. I will fix this along with #13 as well.

Can you please tell me where I can find the VIND-RGBD's bag converted to ROS 2? This would help me a lot in recreating what you show :) Thanks!

NilsRublein commented 2 weeks ago

Thanks for the quick response!

Of course! You can check out my fork, there you can find instructions for downloading, converting and playing the rosbag, along with a launchfile for the RealSense_D435i. Make sure to checkout the navigation_topics branch, which has the most up-to-date instructions.

suchetanrs / ORB-SLAM3-ROS2-Docker

publishMapPointCloud() still in progress? #18