Global Optimizing with Loop Closure and Navigation After Mapping.

[imran05]

Hello,
I'm working on mapping and navigation in an apple orchard using the rtabmap SLAM with stereo odometry from an OAK-D-PR-W POE camera. I'm following the ROS tutorial for stereo handheld mapping (https://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping).
I have two questions:

Mapping issue: When moving from the 2nd to 3rd row in the orchard, my map sometimes creates errors. These errors occasionally get fixed when loop closure is detected and optimizes the overall map. However, sometimes loop closure doesn't optimize the map well, resulting in duplicated rows and poor mapping quality. Is there a way to delete or refine specific poses that are causing problems in the mapping? I've attached an image of my map showing this issue.
database link : https://drive.google.com/file/d/1Y5tr5iF25V79tsQ_Gwst_YG-Ni_uftZf/view?usp=sharing
Navigation issue: I'm using the teb local planner with move_base. During navigation, my robot initially localizes itself within the map correctly, but when I provide waypoint goals, it sometimes loses localization in the middle of the rows. This causes incorrect positioning on the map and consequently navigation problems. Is there a method to improve localization using only visual information, without adding additional sensors?

Thank you for your help!

[matlabbe]

Hi,

It looks like there are some stereo issues that is causing the VO to drift more.

1) The stereo rectification doesn't look perfect, there is a 1 pixel vertical shift between the left and right cameras:


2) Bad time sync between left and right cameras causing very large covariance on some links like this:


Looking at the images, we clearly see that the left image is synced with the wrong right image:


I've shown on the right the resulting point clouds for two consecutive frames. The red one is generated from the top image, where we see that disparity is way larger than the one below for similar point of view. I think this issue appears to be worst when the robot is rotating at the end of each row. I would try to get a good map before trying to navigate in it.

Is there a method to improve localization using only visual information, without adding additional sensors?

Here would be the steps to improve VSLAM:
1) Improve stereo calibration,
2) Fix stereo sync,
3) If VO looks drifting too much even after fixing 1 and 2, you may check to integrate a VIO approach instead,
4) For visual localization, using SIFT/SURF/SuperPoint could help to localize over time. I see you are outdoor, classic features like ORB/BRIEF/SIFT/SURF are quite sensible to illumination changes / shadows, features like SuperPoint may be more robust in those cases.

cheers,
Mathieu

[imran05]

Hello,
Thank you for your previous suggestions regarding camera calibration. After several attempts, I was able to achieve the following reprojection errors:

rgb Reprojection Error: 1.588567
Reprojection error threshold -> 1.1111111111111112
left Reprojection Error: 0.157160
Reprojection error threshold -> 1.1111111111111112
right Reprojection Error: 0.174798
Flashing Calibration data into

For your 2nd suggestion can you please let me know that how i fix the stereo_sync issue as I am using the rtabmap_launch file.
For the 4th suggestion i tried to install the Superpoint but i am facing the similar issues like "https://github.com/introlab/rtabmap/issues/1221" even i followed the instruction also in this post but i am still getting the same. and when i installed the tensorflow for only CPU and build rtabmap with that them, the rtabmap is closing by saying the below error:

rminate called after throwing an instance of 'c10::Error'
  what():  PytorchStreamReader failed reading zip archive: failed finding central directory
Exception raised from valid at ../caffe2/serialize/inline_container.cc:228 (most recent call first):
frame #0: c10::Error::Error(c10::SourceLocation, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) + 0x6c (0x7f16a857ba0c in /home/darlab/libtorch/lib/libc10.so)
frame #1: c10::detail::torchCheckFail(char const*, char const*, unsigned int, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) + 0xfa (0x7f16a85258bc in /home/darlab/libtorch/lib/libc10.so)
frame #2: caffe2::serialize::PyTorchStreamReader::valid(char const*, char const*) + 0x35b (0x7f16903a1b9b in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #3: caffe2::serialize::PyTorchStreamReader::init() + 0xb1 (0x7f16903a60f1 in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #4: caffe2::serialize::PyTorchStreamReader::PyTorchStreamReader(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) + 0x185 (0x7f16903a8e15 in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #5: torch::jit::import_ir_module(std::shared_ptr<torch::jit::CompilationUnit>, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::optional<c10::Device>, std::unordered_map<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::hash<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::equal_to<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >&, bool, bool) + 0x2c8 (0x7f16917a9908 in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #6: torch::jit::import_ir_module(std::shared_ptr<torch::jit::CompilationUnit>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::optional<c10::Device>, bool) + 0x92 (0x7f16917a9cd2 in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #7: torch::jit::load(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::optional<c10::Device>, bool) + 0xc0 (0x7f16917a9de0 in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #8: torch::serialize::InputArchive::load_from(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::optional<c10::Device>) + 0x3c (0x7f1691eb219c in /home/darlab/libtorch/lib/libtorch_cpu.so)
frame #9: void torch::load<std::shared_ptr<rtabmap::SuperPoint>, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::shared_ptr<rtabmap::SuperPoint>&, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&&) + 0x47 (0x7f16b07ec2e7 in /usr/local/lib/librtabmap_core.so.0.21)
frame #10: rtabmap::SPDetector::SPDetector(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, float, bool, int, bool) + 0x1d0 (0x7f16b07e8330 in /usr/local/lib/librtabmap_core.so.0.21)
frame #11: rtabmap::SuperPointTorch::parseParameters(std::map<std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) + 0x210 (0x7f16b0419620 in /usr/local/lib/librtabmap_core.so.0.21)
frame #12: rtabmap::SuperPointTorch::SuperPointTorch(std::map<std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) + 0xcb (0x7f16b041983b in /usr/local/lib/librtabmap_core.so.0.21)
frame #13: rtabmap::Feature2D::create(rtabmap::Feature2D::Type, std::map<std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) + 0xff (0x7f16b041997f in /usr/local/lib/librtabmap_core.so.0.21)
frame #14: rtabmap::Feature2D::create(std::map<std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) + 0xb3 (0x7f16b0419ba3 in /usr/local/lib/librtabmap_core.so.0.21)
frame #15: rtabmap::Memory::Memory(std::map<std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) + 0x398 (0x7f16b02bb838 in /usr/local/lib/librtabmap_core.so.0.21)
frame #16: rtabmap::Rtabmap::init(std::map<std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&, std::_cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) + 0xe47 (0x7f16b02435f7 in /usr/local/lib/librtabmap_core.so.0.21)
frame #17: rtabmap::RtabmapThread::mainLoop() + 0x5b3 (0x7f16b028cfd3 in /usr/local/lib/librtabmap_core.so.0.21)
frame #18: UThread::ThreadMain() + 0x51 (0x7f16afe808b1 in /usr/local/lib/librtabmap_utilite.so.0.21)
frame #19: UThreadC<void>::ThreadMainHandler(UThreadC<void>::Instance*) + 0xa9 (0x7f16afe80bd9 in /usr/local/lib/librtabmap_utilite.so.0.21)
frame #20: <unknown function> + 0x8609 (0x7f16aaa24609 in /lib/x86_64-linux-gnu/libpthread.so.0)
frame #21: clone + 0x43 (0x7f16aefb0353 in /lib/x86_64-linux-gnu/libc.so.6)

Aborted (core dumped)
darlab@darlab-Inspiron-7501:~/rtabmap/build$

[matlabbe]

For the stereo calibration, the reprojection errors look okay, but do a quick visual check by comparing the two rectified images in Gimp for example (with two layers, top one 50% transparent) to see really that there is no more vertical shift. The reprojection error could be good for each individual camera, but you may also check the reprojection error after stereo rectification. From experience, the hardest part to get right in stereo calibration are the stereo extrinsics (rotation and translation between the cameras), not the individual camera intrinsics.

For the stereo sync issue:
a. If the camera driver is publishing left/right images with exact same stamp, make sure you use approx_sync:=false in rtabmap. If it is already your setup and there is still bad sync like above, then it is a camera driver issue. You should ask them why their images with exact same stamps are not synchronized.
b. If the camera driver is publishing left/right images with slightly different stamps but very close (<1 ms), assuming on hardware side they all shutter at the same time, you can set approx_sync:=true and set approx_sync_max_interval:=0.001 (very small accepted interval). If even with that there are bad synchronizations, you would have to ask on your camera driver's support.

For SuperPoint, I don't actively test with latest pytorch/cuda versions, last time I tried I created a doc here: https://github.com/introlab/rtabmap/tree/master/archive/2022-IlluminationInvariant#how-reproduce-results-shown-in-the-paper to reproduce the results of the last paper (comparing SuperPoint). There is also instructions to build a Docker version with SuperPoint support.

I'll suggest to improve stereo calibration and stereo sync in first phase, then jump to SuperPoint later.

cheers,
Mathieu

[imran05]

Hello,

I've been following your recommendations from last week, but I'm still uncertain if stereo rectification is the root cause of my issue. Upon inspecting both the left and right images, I didn't observe any vertical shift:
GIMP_VErticalShiftChecking_150.mp4

Additionally, I've checked the timestamps from my stereo camera, and both the left and right topics appear to be synchronized. I used "approx_sync:=true" and set "approx_sync_max_interval:=0.001" in my code:

Timestamps:
Left CameraInfo:  1747807021.4514253
Right CameraInfo: 1747807021.4514253
Left Image:       1747807021.4514253
Right Image:      1747807021.4514253
IMU:              1747807021.4497094

I've recently updated my map using the OAK D PRO W POE Camera, and I didn't notice any stereo synchronization issues. Here's the link to the database link Could you also please advise on how to generate the GIF showing both left and right image disparities?

Furthermore, I encountered similar localization issues when using the RealSense D435i camera. Here's the Realsense database link. Could you kindly assist me in achieving accurate localization? If I need to adopt the VIO approach with RTAB-Map, how should I proceed? I attempted to install Superpoint, but I encountered issues related to PyTorch, even when using Docker.

I look forward to your response.

Best regards,