rtabmap icon indicating copy to clipboard operation
rtabmap copied to clipboard

Published 20 hours ago • verified publisher icon introlab

ORB_SLAM3 Support?

Open Hlwy opened this issue 4 years ago • 11 comments

I am using rtabmap/rtabmap_ros for a homemade 3D printed skid-steer rover that I am using at home intended to be used as the mobile base for a variety of future projects for both indoor and outdoor projects. Right now I am working on getting the robot to reliably localize itself in my house by using a Pixhawk+CubeOrange+mavros combo for performing the robot localization (via EKF3) and motor control. Everything else (the robot brains), is done on a Jetson TX2 (and an optional RPi4 which I predict will be needed later on for supporting future projects like beer retrieval :P). My current setup uses your rgbd_odometry ROS node as the front end visual odometer (either F2M or ORB_SLAM2) which outputs a 3D estimated pose (via mavros) to be fused with the wheel encoders and IMU, by the EKF running on the Pixhawk. The Pixhawk's EKF in turn outputs the final estimated pose of the robot (via mavros) to your rtabmap node, which I use for running the backend SLAM and pose (via map->odom tf) corrections.

All of this, in combination with move_base, is what I have been using to send the robot to various locations in my office, and I have been very happy with all the work you are doing with rtabmap and the performance of it with my setup so far. The only thing that I have been trying to work out recently is better handling of the visual odometer (VO) losing track during zero-point and/or fast turns. Among many other things, my current solution has been to make move_base send relatively conservative (i.e. slower) velocity commands to the Pixhawk for controlling the motors. Although, the VO will still lose track sometimes, depending on the visual scenario, the final EKF-fused pose of the robot has been relatively reasonable when I manually control the robot in a 2 x 2 meter box. One thing I recently stumbled upon (today), was the ORB_SLAM3 algorithm which, judging from the Youtube video I saw, seems to handle fairly well fast rotational camera motions. Since I am a strong supporting on the use of rtabmap for robot localization, I was wondering if supporting the use of ORB_SLAM3 is something you might consider? If not, how might I go about integrating this new VO method (or any other VO method for that matter) into rtabmap?

control the robot slowly and I have to say that I am a strong advocate for rtabmap, since (so far) has worked the best at helping my robot to localize itself.

Hlwy avatar Dec 04 '20 03:12 Hlwy

I'm very open to integrate ORB_SLAM3 to rtabmap like I did with ORB_SLAM2. I just didn't take time yet to look at ORB_SLAM3 code (which is relatively new, sept 2020). For what I can read on their page:

This software is based on ORB-SLAM2 developed by Raul Mur-Artal ...

I think the integration could be similar to what I did for ORB_SLAM2: https://github.com/introlab/rtabmap/blob/da8e76ffed836fad3319716e5e15112a59e4e4f5/corelib/src/odometry/OdometryORBSLAM2.cpp#L880-L896 with probably a function to feed IMU data. Fast rotations can be then indeed estimated more robustly when including IMU with ORB_SLAM3 (similar than other VIO approaches). During the Christmas vacation, I may have time to play with it (also checking OpenVSLAM and OpenVINS). So if you want to try integrating, look at my ORB_SLAM2 code.

matlabbe avatar Dec 10 '20 18:12 matlabbe

Terrific! I am excited to see how this new ORB_SLAM performs under fast rotations. On the downside, it is very new so there are bound to be bugs-galore. I wouldn't mind working to integrate it with Rtabmap, but before I do, I am trying to add an additional mechanism for RGBD Inertial tracking into the ORB_SLAM3 (as of now only RGBD is supported). I saw in one of their reported issues that they have the ability to easily support RGBD inertial, however it just hasn't been done yet. Once, I get that working then I will probably start to help integrate ORB_SLAM3 into Rtabmap.

Hlwy avatar Dec 16 '20 09:12 Hlwy

Hey, I saw that you committed your changes for using either orbslam 2 or 3. Super awesome turn around BTW. So a couple of questions, but first, the commits that you merged did you test to see if your changes result in successfully using orbslam 3 when using rgbd_odometry node? Unfortunately I didn't hey around to testing your changes when orbslam3 v0.03 was out (had a baby and haven't been able to do much for the first couple of months) but I started testing out your changes earlier last week however I had also pulled in the latest from orbslam3's latest release (v0.04) when I rebuilt rtabmap(_ros). When I tried it out using the same roslaunch file I've always used to run the rgbd_odometry with orbslam2 as the backend, I kept getting a segfault after the vocabulary gets loaded. So my initial thought was that obviously it was the fault of the new orbslam3 (since the last release still had some issues), when I went and ran the rgbd node built by orbslam3 ros it ran fine. So I started messing around with your code (putting a crap ton of printouts in all the major sections of code for both your code and pieces of the orbslam3 code you can in your code) to see where the problem originates from. After playing around with some stuff, I think I've finally found where the problem comes from, which occurs during the call to Transform OdometryORBSLAM::computeTransform(). For some reason, this piece of code

localTransform = data.cameraModels()[0].localTransform();
cv::Mat depth;
if(data.depthRaw().type() == CV_32FC1)
{
	depth = data.depthRaw();
}
else if(data.depthRaw().type() == CV_16UC1)
{
	depth = util2d::cvtDepthToFloat(data.depthRaw());
}
Tcw = ((Tracker*)orbslam_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());

results in some kind of memory corruption during either the instantiation of a new Mat object or modifying the depth values stored in the Mat's memory in the call to depth = util2d::cvtDepthToFloat(data.depthRaw());. Just for kicks I commenting that line out and tried out different methods for achieving the same effect as your util2d function in that else if block. In the place of the depth = util2d::cvtDepthToFloat(data.depthRaw()); line, I use the following chunk of code instead:

UWARN("Depth data available -- ");
int nrows = data.depthRaw().rows;
int ncols = data.depthRaw().cols;
UWARN("Copying stored data to local container \'tmpCopy\'...");
cv::Mat tmpCopy(nrows, ncols, CV_16UC1, (void*) data.depthRaw().data);
UWARN("Mat \'tmpCopy\' objects has this:");
// This properly prints out showing same values as what data.depthRaw() has 
for(int i=0; i<tmpCopy.rows; ++i){
     for(int j=0; j<tmpCopy.cols; ++j){
          cout << tmpCopy.at<float>(i,j)/1000.0f << ", ";
     }
}
cout << "\r\n ------------ ";
UWARN("Constructing empty \'depth\' CV_32FC1 mat of the same size to fill in ...");
depth = cv::Mat(nrows, ncols, CV_32FC1);     // Segfaults here
UWARN("Filling in \'depth\' CV_32FC1 pixel values...");
for(int i=0; i<nrows; ++i){
     for(int j=0; j<ncols; ++j){
          float tmpVal = tmpCopy.at<float>(i,j)/1000.0f;
          depth.at<float>(i,j) = tmpVal;
     }
}
UWARN("Finished CV_32FC1 Conversion w/ result...");

When I run the rgbd_odometry node even if I wanted to simply instantiate (for the cv::Mat depth; object) an empty Mat container of some size, it fails. This leads me to my second question of, am I simply missing extra stuff required for new orbslam3 stuff that is causing this or is there some other reason this is occurring? I will upload the launch file I use and the results of running a bt when attaching the launched ROS nodes using gdb later tomorrow when I get the chance.

Hlwy avatar May 19 '21 05:05 Hlwy

I've attached the launch file I used (just remove the txt extension I had to do to allow github to upload the file) here. rtabmap_vo.launch.txt

Here is the gdb outputs leading up to the malloc memory corruption

[ INFO] [1621509106.791697943]: Initializing nodelet with 8 worker threads.
[New Thread 0x7fff27ff7700 (LWP 27290)]
[tcsetpgrp failed in terminal_inferior: Invalid argument]
[New Thread 0x7fff24b06700 (LWP 27297)]
[ INFO] [1621509108.643814606]: Odometry: frame_id               = base_link
[ INFO] [1621509108.643869265]: Odometry: odom_frame_id          = odom
[ INFO] [1621509108.643892665]: Odometry: publish_tf             = false
[ INFO] [1621509108.643914254]: Odometry: wait_for_transform     = true
[ INFO] [1621509108.643950938]: Odometry: wait_for_transform_duration  = 0.100000
[ INFO] [1621509108.644022498]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1621509108.644053027]: Odometry: ground_truth_frame_id  = 
[ INFO] [1621509108.644081456]: Odometry: ground_truth_base_frame_id = base_link
[ INFO] [1621509108.644109680]: Odometry: config_path            = 
[ INFO] [1621509108.644138933]: Odometry: publish_null_when_lost = true
[ INFO] [1621509108.644165116]: Odometry: guess_frame_id         = 
[ INFO] [1621509108.644194437]: Odometry: guess_min_translation  = 0.000000
[ INFO] [1621509108.644223397]: Odometry: guess_min_rotation     = 0.000000
[ INFO] [1621509108.644257050]: Odometry: guess_min_time         = 0.000000
[ INFO] [1621509108.644299777]: Odometry: expected_update_rate   = 0.000000 Hz
[ INFO] [1621509108.644320033]: Odometry: max_update_rate        = 0.000000 Hz
[ INFO] [1621509108.644351606]: Odometry: wait_imu_to_init       = false
[ INFO] [1621509108.644402440]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0
[ INFO] [1621509108.772831616]: Setting odometry parameter "GFTT/BlockSize"="3"
[ INFO] [1621509108.784478061]: Setting odometry parameter "GFTT/MinDistance"="8"
[ INFO] [1621509108.786373636]: Setting odometry parameter "GFTT/QualityLevel"="0.001"
[ INFO] [1621509108.962786606]: Setting odometry parameter "Odom/FilteringStrategy"="0"
[ INFO] [1621509108.979874366]: Setting odometry parameter "Odom/Holonomic"="true"
[ INFO] [1621509108.985043218]: Setting odometry parameter "Odom/ImageBufferSize"="1"
[ INFO] [1621509109.059042132]: Setting odometry parameter "Odom/ResetCountdown"="10"
[ INFO] [1621509109.067956686]: Setting odometry parameter "Odom/Strategy"="5"
[ INFO] [1621509109.082842592]: Setting odometry parameter "OdomF2M/BundleAdjustment"="1"
[ INFO] [1621509109.087424788]: Setting odometry parameter "OdomF2M/BundleAdjustmentMaxFrames"="10"
[ INFO] [1621509109.095326105]: Setting odometry parameter "OdomF2M/MaxSize"="1500"
[ INFO] [1621509109.616110386]: Setting odometry parameter "OdomORBSLAM/MaxFeatures"="1500"
[ INFO] [1621509109.625743103]: Setting odometry parameter "OdomORBSLAM/VocPath"="/home/hunter/libraries/ORB_SLAM3/Vocabulary/ORBvoc.bin"
[ INFO] [1621509109.782853098]: Setting odometry parameter "Optimizer/Iterations"="20"
[ INFO] [1621509109.808116179]: Setting odometry parameter "Optimizer/Strategy"="2"
[ INFO] [1621509109.873918333]: Setting odometry parameter "Reg/Force3DoF"="false"
[ INFO] [1621509109.889176602]: Setting odometry parameter "Reg/Strategy"="0"
[ INFO] [1621509110.054295015]: Setting odometry parameter "Vis/BundleAdjustment"="1"
[ INFO] [1621509110.111645588]: Setting odometry parameter "Vis/CorNNType"="7"
[ INFO] [1621509110.116217204]: Setting odometry parameter "Vis/CorType"="0"
[ INFO] [1621509110.138221365]: Setting odometry parameter "Vis/FeatureType"="6"
[ INFO] [1621509110.140953972]: Setting odometry parameter "Vis/ForwardEstOnly"="true"
[ INFO] [1621509110.164067726]: Setting odometry parameter "Vis/Iterations"="150"
[ INFO] [1621509110.172664471]: Setting odometry parameter "Vis/MaxFeatures"="1500"
[ INFO] [1621509110.185047544]: Setting odometry parameter "Vis/MinDepth"="0"
[ INFO] [1621509110.231761943]: Setting odometry parameter "Vis/RoiRatios"="0.1 0.0 0.0 0.15"
[ WARN] (2021-05-20 07:11:50.553) OdometryORBSLAM.cpp:962::ORBSLAMSystem() Loading ORB Vocabulary: "/home/hunter/libraries/ORB_SLAM3/Vocabulary/ORBvoc.bin". This could take a while...
[ WARN] (2021-05-20 07:11:51.107) OdometryORBSLAM.cpp:978::ORBSLAMSystem() Vocabulary loaded in 0.56s

[ INFO] [1621509111.144599276]: RGBDOdometry: approx_sync    = true
[ INFO] [1621509111.144668696]: RGBDOdometry: queue_size     = 1000
[ INFO] [1621509111.144706343]: RGBDOdometry: subscribe_rgbd = false
[ INFO] [1621509111.144738782]: RGBDOdometry: rgbd_cameras   = 1
[ INFO] [1621509111.144769512]: RGBDOdometry: keep_color     = false
[tcsetpgrp failed in terminal_inferior: Invalid argument]
[New Thread 0x7fff096d7700 (LWP 27309)]
[ INFO] [1621509111.260835997]: 
/rtabmap/rgbd_odometry3 subscribed to (approx sync):
   /camera/color/image_raw \
   /camera/depth/image_rect_raw \
   /camera/color/camera_info
[New Thread 0x7fff08bb9700 (LWP 27310)]
[New Thread 0x7fff03fff700 (LWP 27311)]
[New Thread 0x7fff037fe700 (LWP 27312)]
[New Thread 0x7fff02ffd700 (LWP 27313)]
[New Thread 0x7fff027fc700 (LWP 27314)]
[New Thread 0x7fff01ffb700 (LWP 27315)]
[New Thread 0x7fff017fa700 (LWP 27316)]
[ WARN] [1621509111.506606579]: OdometryROS::processData() -- Started
[ WARN] [1621509111.507623442]: OdometryROS::processData() -- Starting to process all imu data...
[ WARN] [1621509111.508509207]: OdometryROS::processData() -- if(!guessFrameId_.empty())
[ WARN] [1621509111.508732127]: OdometryROS::processData() -- Setting ground truth data
[ WARN] [1621509111.508899489]: OdometryROS::processData() -- Calling rtabmap::Transform pose = odometry_->process(data, guess_, &info);
rtabmap::Odometry::process() -- !data.imu().empty() = 0
rtabmap::Odometry::process() -- !this->canProcessAsyncIMU() = 1
[ WARN] (2021-05-20 07:11:51.509) OdometryORBSLAM.cpp:1339::computeTransform() computeTransform() -- 
[ WARN] (2021-05-20 07:11:51.509) OdometryORBSLAM.cpp:1408::computeTransform() computeTransform() -- ORB_SLAM3 initializing...
 -- CameraModel model = Name: ros
Size: 848x480
K= [614.15625, 0, 426.5084228515625;
 0, 613.42626953125, 234.4129791259766;
 0, 0, 0]
D= [0, 0, 0, 0, 0]
R= [0, 0, 0;
 0, 0, 0;
 0, 0, 0]
P= [614.15625, 0, 426.5084228515625, 0;
 0, 613.42626953125, 234.4129791259766, 0;
 0, 0, 0, 0]
LocalTransform= [2.22045e-16, 2.22045e-16, 1, 0.26;
 -1, 0, 2.22045e-16, 0;
 0, -1, 2.22045e-16, 0.095]
 -- stereo = 0
 -- data.stereoCameraModel().baseline() = 0
 -- imuLocalTransform_ = [0, 0, 0, 0;
 0, 0, 0, 0;
 0, 0, 0, 0]
Creation of new map with id: 0
Creation of new map with last KF id: 0
[ WARN] (2021-05-20 07:11:51.543) OdometryORBSLAM.cpp:1151::init() ORBSLAMSystem::init() -- new Tracker...

Camera Parameters: 
- Camera: Pinhole
- fx: 614.156
- fy: 613.426
- cx: 426.508
- cy: 234.413
- k1: 0
- k2: 0
- p1: 0
- p2: 0
- k3: 0
- fps: 30
- color order: RGB (ignored if grayscale)

Depth Threshold (Close/Far Points): 0

ORB Extractor Parameters: 
- Number of Features: 1500
- Scale Levels: 3
- Scale Factor: 2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1156::init() ORBSLAMSystem::init() -- new LocalMapping...
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1159::init() ORBSLAMSystem::init() -- new thread(&LocalMapping::Run
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1169::init() ORBSLAMSystem::init() -- new LoopCloser
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1171::init() ORBSLAMSystem::init() -- new thread(&LoopCloser::RunNoLoop
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1175::init() ORBSLAMSystem::init() -- mpTracker->SetLocalMapper(mpLocalMapper);
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1177::init() ORBSLAMSystem::init() -- mpTracker->SetLoopClosing(mpLoopCloser);
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1181::init() ORBSLAMSystem::init() -- mpLocalMapper->SetTracker(mpTracker);
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1183::init() ORBSLAMSystem::init() -- mpLocalMapper->SetLoopCloser(mpLoopCloser);
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1186::init() ORBSLAMSystem::init() -- mpLoopCloser->SetTracker(mpTracker);
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1188::init() ORBSLAMSystem::init() -- mpLoopCloser->SetLocalMapper(mpLocalMapper);
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1195::init() ORBSLAMSystem::init() -- returning...
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1419::computeTransform() computeTransform() -- orbslam_->init() SUCCESS!
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1432::computeTransform() if(stereo) --- else...
 -- localTransform = [2.22045e-16, 2.22045e-16, 1, 0.26;
 -1, 0, 2.22045e-16, 0;
 0, -1, 2.22045e-16, 0.095]
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1438::computeTransform() Raw Depth data size (r, c) = (480, 848)
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1441::computeTransform() if(data.depthRaw().type() == CV_32FC1)
[ WARN] (2021-05-20 07:11:51.546) OdometryORBSLAM.cpp:1461::computeTransform() Depth data available -- 
'depth mat' [848, 480, 1, 16UC1] -- Limits = [0.000, 10479.000] w/ mean = 1413.36, std = 1191.31
[ WARN] (2021-05-20 07:11:51.548) OdometryORBSLAM.cpp:1468::computeTransform() Copying stored data to local container 'depth'...
malloc(): memory corruption

Thread 21 "rgbd_odometry" received signal SIGABRT, Aborted.

and here is the output of bt

(gdb) bt
#0  0x00007ffff541dfb7 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1  0x00007ffff541f921 in __GI_abort () at abort.c:79
#2  0x00007ffff5468967 in __libc_message (action=action@entry=do_abort, fmt=fmt@entry=0x7ffff5595b0d "%s\n") at ../sysdeps/posix/libc_fatal.c:181
#3  0x00007ffff546f9da in malloc_printerr (str=str@entry=0x7ffff5593d8e "malloc(): memory corruption") at malloc.c:5342
#4  0x00007ffff5473b24 in _int_malloc (av=av@entry=0x7ffefc000020, bytes=bytes@entry=1628232) at malloc.c:3748
#5  0x00007ffff54763cd in __GI___libc_malloc (bytes=1628232) at malloc.c:3075
#6  0x00007fffd419c539 in cv::fastMalloc(unsigned long) () at /usr/local/lib/libopencv_core.so.4.4
#7  0x00007fffd43325d2 in cv::Mat::create(int, int const*, int) () at /usr/local/lib/libopencv_core.so.4.4
#8  0x00007ffff6d223f4 in rtabmap::OdometryORBSLAM::computeTransform(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) () at /usr/local/lib/librtabmap_core.so.0.20
#9  0x00007ffff6cf474a in rtabmap::Odometry::process(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) () at /usr/local/lib/librtabmap_core.so.0.20
#10 0x00007fff3841dd2c in rtabmap_ros::OdometryROS::processData(rtabmap::SensorData&, std_msgs::Header_<std::allocator<void> > const&) () at /home/hunter/catkin_ws/devel/lib/librtabmap_ros.so
#11 0x00007fff2703adbf in rtabmap_ros::RGBDOdometry::commonCallback(std::vector<boost::shared_ptr<cv_bridge::CvImage const>, std::allocator<boost::shared_ptr<cv_bridge::CvImage const> > > const&, std::vector<boost::shared_ptr<cv_bridge::CvImage const>, std::allocator<boost::shared_ptr<cv_bridge::CvImage const> > > const&, std::vector<sensor_msgs::CameraInfo_<std::allocator<void> >, std::allocator<sensor_msgs::CameraInfo_<std::allocator<void> > > > const&) ()
    at /home/hunter/catkin_ws/devel/lib//librtabmap_plugins.so
#12 0x00007fff2703b3bf in rtabmap_ros::RGBDOdometry::callback(boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::CameraInfo_<std::allocator<void> > const> const&) () at /home/hunter/catkin_ws/devel/lib//librtabmap_plugins.so
#13 0x00007fff270b904b in boost::_mfi::mf3<void, rtabmap_ros::RGBDOdometry, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::CameraInfo_<std::allocator<void> > const> const&>::operator()(rtabmap_ros::RGBDOdometry*, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::CameraInfo_<std::allocator<void> > const> const&) const () at /home/hunter/catkin_ws/devel/lib//librtabmap_plugins.so
#14 0x00007fff2709e339 in void boost::_bi::list4<boost::_bi::value<rtabmap_ros::RGBDOdometry*>, boost::arg<1>, boost::arg<2>, boost::arg<3> >::operator()<boost::_mfi::mf3<void, rtabmap_ros::RGBDOdometry, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::CameraInfo_<std::allocator<void> > const> const&>, boost::_bi::rrlist9<boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::CameraInfo_<std::allocator<void> > const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&> >(boost::_bi::type<void>, boost::_mfi::mf3<void, rtabmap_ros::RGBDOdometry, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&---Type <return> to continue, or q <return> to quit---

Hlwy avatar May 20 '21 11:05 Hlwy

You have a local opencv version and you use rgbd_odometry. There could be a compatibility issue with rtabmap_ros built with system OpenCV binaries (from cv_bridge) and OpenCV built from source for rtabmap libraries.

Ideally, don't use OpenCV built from source, so everything will use same OpenCV binaries. If you really need the OpenCV built from source, try to use the same version than the one installed by the system binaries (the one cv_bridge is depending on). One last solution is to remove system opencv libraries, but this will unsintall a lot of packages depending on it, so you would have to rebuild all ros packages manually depending on opencv (like cv_bridge).

matlabbe avatar May 30 '21 14:05 matlabbe

Note also I updated the orbslam3 v0.4-beta patch for rtabmap integration here (this fixed a problem with RGB-D mode): https://gist.github.com/matlabbe/f5cb281304a1305b2824a6ce19792e13

matlabbe avatar May 30 '21 18:05 matlabbe

Cool! Thanks for that. For my opencv problem, I don't think the cv_bridge dependencies is the issue, because I built a custom spin-off of the cv_bridge package (I call cv4_bridge) that is built using OpenCv 4-something as well as all of the related core ROS packages that depend on cv_bridge such that they all are built and depend on this new cv4_bridge. I basically disregard the linking of the old bridge by sourcing the workspace the new bridge lives before building any other workspace (i.e. rtabmap_ros, etc.). Although it seems I was able to solve my previous OpenCV problem by doing a clean build from the ground up of ORB_SLAM3 -> rtabmap -> rtabmap_ros, but this time making sure my shell environment variables were properly setup. This seemed to fix my issue. Moving forward, although I went ahead used your patches during the rebuild, I was still getting bugs galore with orbslam3. On the bright side I grinded through most of the bugs, and I am able to have run a modified version of the RGBD ros node that orbslam3 provides however this also couples in the IMU data. I will try to attach a video I took of that running and working. In the video (although performance is at a glance not optimal) it shows the rgbd+imu pose estimation working continuously, however the pose will periodically get reset to zero, but I think this is fine in the grand scheme of things, because it seems that behavior is caused by the orbslam code responsible for the local mapping (since RTABMAP will ultimately be doing that instead if I remember correctly).

That being said I updated the OdometryORBSLAM.cpp code to try to hook in the imu related stuff, however when I try running the rgbd_odometry (using the same launch file I uploaded above) rtabmap never generates a non-zero pose estimate. Below is a snippet from my terminal showing the output of running that launch file. This seems to be a result of rtabmap not having a SensorData implementation that has the RGBD + IMU data together. So that being said is there any way for this data to be pushed through the rtabmap processing pipeline via the SensorData object ?

`hunter@TheRedtop:[~]$ roslaunch swanson_algorithms rtabmap_vo.launch ... logging to /home/hunter/.ros/log/79aeb194-d56f-11eb-8aab-00044bc547a5/roslaunch-TheRedtop-7806.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.2.4:33309/

SUMMARY

PARAMETERS

  • /rosdistro: melodic
  • /rosversion: 1.14.11
  • /rtabmap/rgbd_odometry3/GFTT/BlockSize: 3
  • /rtabmap/rgbd_odometry3/GFTT/MinDistance: 8
  • /rtabmap/rgbd_odometry3/GFTT/QualityLevel: 0.001
  • /rtabmap/rgbd_odometry3/Odom/FilteringStrategy: 0
  • /rtabmap/rgbd_odometry3/Odom/Holonomic: True
  • /rtabmap/rgbd_odometry3/Odom/ImageBufferSize: 1
  • /rtabmap/rgbd_odometry3/Odom/ResetCountdown: 10
  • /rtabmap/rgbd_odometry3/Odom/Strategy: 5
  • /rtabmap/rgbd_odometry3/OdomF2M/BundleAdjustment: 1
  • /rtabmap/rgbd_odometry3/OdomF2M/BundleAdjustmentMaxFrames: 10
  • /rtabmap/rgbd_odometry3/OdomF2M/MaxSize: 1500
  • /rtabmap/rgbd_odometry3/OdomORBSLAM/Bf: 8.7860446709
  • /rtabmap/rgbd_odometry3/OdomORBSLAM/Fps: 30.0
  • /rtabmap/rgbd_odometry3/OdomORBSLAM/MapSize: 3000.0
  • /rtabmap/rgbd_odometry3/OdomORBSLAM/MaxFeatures: 1500
  • /rtabmap/rgbd_odometry3/OdomORBSLAM/ThDepth: 35.0
  • /rtabmap/rgbd_odometry3/OdomORBSLAM/VocPath: /home/hunter/libr...
  • /rtabmap/rgbd_odometry3/Optimizer/Iterations: 20
  • /rtabmap/rgbd_odometry3/Optimizer/Strategy: 2
  • /rtabmap/rgbd_odometry3/Reg/Force3DoF: False
  • /rtabmap/rgbd_odometry3/Reg/Strategy: 0
  • /rtabmap/rgbd_odometry3/Vis/BundleAdjustment: 1
  • /rtabmap/rgbd_odometry3/Vis/CorNNType: 7
  • /rtabmap/rgbd_odometry3/Vis/CorType: 0
  • /rtabmap/rgbd_odometry3/Vis/FeatureType: 6
  • /rtabmap/rgbd_odometry3/Vis/ForwardEstOnly: True
  • /rtabmap/rgbd_odometry3/Vis/Iterations: 150
  • /rtabmap/rgbd_odometry3/Vis/MaxFeatures: 1500
  • /rtabmap/rgbd_odometry3/Vis/MinDepth: 0
  • /rtabmap/rgbd_odometry3/Vis/RoiRatios: 0.1 0.0 0.0 0.15
  • /rtabmap/rgbd_odometry3/approx_sync: True
  • /rtabmap/rgbd_odometry3/frame_id: base_link
  • /rtabmap/rgbd_odometry3/keep_color: False
  • /rtabmap/rgbd_odometry3/odom_frame_id: odom
  • /rtabmap/rgbd_odometry3/publish_null_when_lost: True
  • /rtabmap/rgbd_odometry3/publish_tf: False
  • /rtabmap/rgbd_odometry3/queue_size: 1000
  • /rtabmap/rgbd_odometry3/wait_for_transform_duration: 0.1

NODES /rtabmap/ rgbd_odometry3 (rtabmap_ros/rgbd_odometry)

ROS_MASTER_URI=http://192.168.2.3:11311

process[rtabmap/rgbd_odometry3-1]: started with pid [7822] [ INFO] [1624611010.210465970]: Initializing nodelet with 8 worker threads. [ INFO] [1624611010.591886411]: Odometry: frame_id = base_link [ INFO] [1624611010.592015773]: Odometry: odom_frame_id = odom [ INFO] [1624611010.592117995]: Odometry: publish_tf = false [ INFO] [1624611010.592184265]: Odometry: wait_for_transform = true [ INFO] [1624611010.592278510]: Odometry: wait_for_transform_duration = 0.100000 [ INFO] [1624611010.592476811]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 [ INFO] [1624611010.592592292]: Odometry: ground_truth_frame_id = [ INFO] [1624611010.592669274]: Odometry: ground_truth_base_frame_id = base_link [ INFO] [1624611010.592761357]: Odometry: config_path = [ INFO] [1624611010.592857327]: Odometry: publish_null_when_lost = true [ INFO] [1624611010.592961542]: Odometry: guess_frame_id = [ INFO] [1624611010.593036771]: Odometry: guess_min_translation = 0.000000 [ INFO] [1624611010.593115402]: Odometry: guess_min_rotation = 0.000000 [ INFO] [1624611010.593180721]: Odometry: guess_min_time = 0.000000 [ INFO] [1624611010.593257292]: Odometry: expected_update_rate = 0.000000 Hz [ INFO] [1624611010.593355012]: Odometry: max_update_rate = 0.000000 Hz [ INFO] [1624611010.593438536]: Odometry: wait_imu_to_init = false [ INFO] [1624611010.593666259]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0 [ INFO] [1624611010.747370037]: Setting odometry parameter "GFTT/BlockSize"="3" [ INFO] [1624611010.761728217]: Setting odometry parameter "GFTT/MinDistance"="8" [ INFO] [1624611010.765246191]: Setting odometry parameter "GFTT/QualityLevel"="0.001" [ INFO] [1624611010.940851343]: Setting odometry parameter "Odom/FilteringStrategy"="0" [ INFO] [1624611010.960750585]: Setting odometry parameter "Odom/Holonomic"="true" [ INFO] [1624611010.967754113]: Setting odometry parameter "Odom/ImageBufferSize"="1" [ INFO] [1624611011.037142175]: Setting odometry parameter "Odom/ResetCountdown"="10" [ INFO] [1624611011.048808191]: Setting odometry parameter "Odom/Strategy"="5" [ INFO] [1624611011.061757969]: Setting odometry parameter "OdomF2M/BundleAdjustment"="1" [ INFO] [1624611011.072075904]: Setting odometry parameter "OdomF2M/BundleAdjustmentMaxFrames"="10" [ INFO] [1624611011.086559612]: Setting odometry parameter "OdomF2M/MaxSize"="1500" [ INFO] [1624611011.642580158]: Setting odometry parameter "OdomORBSLAM/Bf"="8.78604" [ INFO] [1624611011.650531509]: Setting odometry parameter "OdomORBSLAM/Fps"="30" [ INFO] [1624611011.657446400]: Setting odometry parameter "OdomORBSLAM/MapSize"="3000" [ INFO] [1624611011.665787977]: Setting odometry parameter "OdomORBSLAM/MaxFeatures"="1500" [ INFO] [1624611011.671668777]: Setting odometry parameter "OdomORBSLAM/ThDepth"="35" [ INFO] [1624611011.680414221]: Setting odometry parameter "OdomORBSLAM/VocPath"="/home/hunter/libraries/ORB_SLAM3/Vocabulary/ORBvoc.bin" [ INFO] [1624611011.843377264]: Setting odometry parameter "Optimizer/Iterations"="20" [ INFO] [1624611011.880405685]: Setting odometry parameter "Optimizer/Strategy"="2" [ INFO] [1624611011.952480329]: Setting odometry parameter "Reg/Force3DoF"="false" [ INFO] [1624611011.965615320]: Setting odometry parameter "Reg/Strategy"="0" [ INFO] [1624611012.128692349]: Setting odometry parameter "Vis/BundleAdjustment"="1" [ INFO] [1624611012.186407205]: Setting odometry parameter "Vis/CorNNType"="7" [ INFO] [1624611012.191079256]: Setting odometry parameter "Vis/CorType"="0" [ INFO] [1624611012.216764282]: Setting odometry parameter "Vis/FeatureType"="6" [ INFO] [1624611012.219976791]: Setting odometry parameter "Vis/ForwardEstOnly"="true" [ INFO] [1624611012.258481186]: Setting odometry parameter "Vis/Iterations"="150" [ INFO] [1624611012.270209661]: Setting odometry parameter "Vis/MaxFeatures"="1500" [ INFO] [1624611012.286963799]: Setting odometry parameter "Vis/MinDepth"="0" [ INFO] [1624611012.348005997]: Setting odometry parameter "Vis/RoiRatios"="0.1 0.0 0.0 0.15" [ WARN] (2021-06-25 04:50:12.735) OdometryORBSLAM.cpp:1192::ORBSLAMSystem() Loading ORB Vocabulary: "/home/hunter/libraries/ORB_SLAM3/Vocabulary/ORBvoc.bin". This could take a while... [ WARN] (2021-06-25 04:50:13.277) OdometryORBSLAM.cpp:1208::ORBSLAMSystem() Vocabulary loaded in 0.55s

[ INFO] [1624611013.330674575]: RGBDOdometry: approx_sync = true [ INFO] [1624611013.330739310]: RGBDOdometry: queue_size = 1000 [ INFO] [1624611013.330762204]: RGBDOdometry: subscribe_rgbd = false [ INFO] [1624611013.330788104]: RGBDOdometry: rgbd_cameras = 1 [ INFO] [1624611013.330809319]: RGBDOdometry: keep_color = false [ INFO] [1624611013.410342480]: /rtabmap/rgbd_odometry3 subscribed to (approx sync): /camera/color/image_raw
/camera/depth/image_rect_raw
/camera/color/camera_info [ WARN] [1624611013.644958417]: OdometryROS::processData() -- Started [ WARN] [1624611013.645133560]: OdometryROS::processData() -- Starting to process all imu data... [ WARN] [1624611013.645313926]: OdometryROS::processData() -- if(!guessFrameId_.empty()) [ WARN] [1624611013.645348795]: OdometryROS::processData() -- Setting ground truth data [ WARN] [1624611013.645378356]: OdometryROS::processData() -- Calling rtabmap::Transform pose = odometry_->process(data, guess_, &info); rtabmap::Odometry::process() -- !data.imu().empty() = 0 rtabmap::Odometry::process() -- !this->canProcessAsyncIMU() = 0 [ WARN] (2021-06-25 04:50:13.645) OdometryORBSLAM.cpp:1619::computeTransform() computeTransform() -- return Transform() [ WARN] [1624611013.645494609]: OdometryROS::processData() -- if(!pose.isNull()) = 0 [ WARN] [1624611013.645570559]: Odometry lost! [ INFO] [1624611013.647739470]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.002348s [ WARN] [1624611013.662103269]: OdometryROS::processData() -- Started [ WARN] [1624611013.662191934]: OdometryROS::processData() -- Starting to process all imu data... [ WARN] [1624611013.662223249]: OdometryROS::processData() -- if(!guessFrameId_.empty()) [ WARN] [1624611013.662254606]: OdometryROS::processData() -- Setting ground truth data [ WARN] [1624611013.662279317]: OdometryROS::processData() -- Calling rtabmap::Transform pose = odometry_->process(data, guess_, &info); rtabmap::Odometry::process() -- !data.imu().empty() = 0 rtabmap::Odometry::process() -- !this->canProcessAsyncIMU() = 0 [ WARN] (2021-06-25 04:50:13.662) OdometryORBSLAM.cpp:1619::computeTransform() computeTransform() -- return Transform() [ WARN] [1624611013.662359342]: OdometryROS::processData() -- if(!pose.isNull()) = 0 [ WARN] [1624611013.662388207]: Odometry lost! [ INFO] [1624611013.662447942]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000190s [ WARN] [1624611013.692228643]: OdometryROS::processData() -- Started [ WARN] [1624611013.692351699]: OdometryROS::processData() -- Starting to process all imu data... [ WARN] [1624611013.692421844]: OdometryROS::processData() -- if(!guessFrameId_.empty()) [ WARN] [1624611013.692468372]: OdometryROS::processData() -- Setting ground truth data [ WARN] [1624611013.692492381]: OdometryROS::processData() -- Calling rtabmap::Transform pose = odometry_->process(data, guess_, &info); rtabmap::Odometry::process() -- !data.imu().empty() = 0 rtabmap::Odometry::process() -- !this->canProcessAsyncIMU() = 0 [ WARN] (2021-06-25 04:50:13.692) OdometryORBSLAM.cpp:1619::computeTransform() computeTransform() -- return Transform() [ WARN] [1624611013.692577404]: OdometryROS::processData() -- if(!pose.isNull()) = 0 [ WARN] [1624611013.692600027]: Odometry lost! [ INFO] [1624611013.692681264]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000226s [ WARN] [1624611013.725578669]: OdometryROS::processData() -- Started [ WARN] [1624611013.725704481]: OdometryROS::processData() -- Starting to process all imu data... [ WARN] [1624611013.725767800]: OdometryROS::processData() -- if(!guessFrameId_.empty()) [ WARN] [1624611013.725823920]: OdometryROS::processData() -- Setting ground truth data [ WARN] [1624611013.725889205]: OdometryROS::processData() -- Calling rtabmap::Transform pose = odometry_->process(data, guess_, &info); rtabmap::Odometry::process() -- !data.imu().empty() = 0 rtabmap::Odometry::process() -- !this->canProcessAsyncIMU() = 0 [ WARN] (2021-06-25 04:50:13.725) OdometryORBSLAM.cpp:1619::computeTransform() computeTransform() -- return Transform() [ WARN] [1624611013.726008915]: OdometryROS::processData() -- if(!pose.isNull()) = 0 [ WARN] [1624611013.726060111]: Odometry lost! `

https://user-images.githubusercontent.com/7784782/123398061-1ed18380-d571-11eb-8e86-9d19c495f3e7.mp4

Hlwy avatar Jun 25 '21 08:06 Hlwy

I'll share my fork of the orbslam3 and rtabmap repos once I create them (properly, since all the stuff I've been adding + metric-ton worth of debug prinouts were done on local clones of the original repos and I wasn't really sure I would ever get anything working). Here are the links to my forks of the two repos and the branches I intend to upload all of my local changes into (however right now they are just clones of the latest upstream heads) ORB_SLAM3 and rtabmap

Hlwy avatar Jun 25 '21 09:06 Hlwy

So I've updated and pushed my local changes to my forked repos. I started reading the ORB_SLAM3 paper in detail. I was thinking about how you integrate orb slam by leaving the back end operations to rtabmap. That being said I was wondering how to integrate the inertial bundle adjustments that orb slam does for the inertial side of things?

Hlwy avatar Jun 28 '21 00:06 Hlwy

What does happen when integrating orb_slam3 with rtabmap? Both are visual slam methods right? Can someone explain the overall functionality with this integration?

SahanGura avatar Dec 21 '21 17:12 SahanGura

The ORB_SLAM2 or ORB_SLAM3 integration in RTAB-Map is just for the visual odometry part (Front-End) of ORB_SLAM, the loop closure detection and global graph optimization is done by RTAB-Map. We disabled all mapping stuff from ORB_SLAM, keeping only visual odometry and local mapping (ORB_SLAM's loop closure detection and bundle adjustment are disabled).

ORB_SLAM -> odom -> RTAB-Map -> map (referred by ORB2-RTAB in results of this paper)

In comparison, here is the default RTAB-Map approach for visual SLAM (using Frame-to-Map odometry from RTAB-Map): F2M -> odom -> RTAB-Map -> map (referred by F2M in results of this paper)

So the biggest differences between vanilla ORB_SLAM3 and ORB_RTAB are the loop closure detection approach used (DBoW versus original RTAB-Map) and how the map is optimized after a loop closure (Global Bundle Adjustment versus Graph Optimization). Beside visual SLAM stuff, RTAB-Map will also provide occupancy grids for navigation.

For IMU support from the ORB_SLAM3 integration, it is yet to be implemented.

cheers, Mathieu

matlabbe avatar Dec 21 '21 22:12 matlabbe