Map update stopped in between

[alexr]

Hi,

I am using rtabmap latest pull on ROS kinetic.

With default settings and demo_robot_mapping.launch on a pre recorded rosbag, the mapping progresses nicely for 80-85 percent of the bag play. After that it just stops with no warning or error messages. However the visual odometry and laser scans are published and can be visualized on rviz.

 I thought this might be due to GTSAM,  so I changed Optimizer/Strategy to 0 and tried the optimization with TORO. With this the mapping progresses but the loop closure could not be detected and the following Warnings are continuously published.

[ INFO] [1542862881.032622088, 1535796433.983701935]: rtabmap (670): Rate=1.00s, Limit=0.000s, RTAB-Map=0.1392s, Maps update=0.0148s pub=0.0102s (local map=612, WM=612)
[ WARN] (2018-11-22 14:01:21.974) Rtabmap.cpp:2547::process() Rejecting all added loop closures (1, first is 671 <-> 125) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 1473.318115 (edge 412->413, type=0, abs error=6.125689 m, stddev=0.004158). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 2.000000 of std deviation.
[ WARN] (2018-11-22 14:01:21.974) Rtabmap.cpp:2571::process() Rejecting all added loop closures (1, first is 671 <-> 125) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 48.344860 (edge 412->413, type=0, abs error=11.516787 deg, stddev=0.004158). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 2.000000 of std deviation.
[ WARN] (2018-11-22 14:01:21.974) Rtabmap.cpp:2581::process() Loop closure 671->125 rejected!
[ INFO] [1542862881.998658637, 1535796434.950497694]: rtabmap (671): Rate=1.00s, Limit=0.000s, RTAB-Map=0.1277s, Maps update=0.0112s pub=0.0090s (local map=612, WM=612)
[ WARN] (2018-11-22 14:01:23.247) Rtabmap.cpp:2547::process() Rejecting all added loop closures (1, first is 672 <-> 125) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 1477.281738 (edge 412->413, type=0, abs error=6.142168 m, stddev=0.004158). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 2.000000 of std deviation.
[ WARN] (2018-11-22 14:01:23.247) Rtabmap.cpp:2571::process() Rejecting all added loop closures (1, first is 672 <-> 125) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 48.413525 (edge 412->413, type=0, abs error=11.533143 deg, stddev=0.004158). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 2.000000 of std deviation.
[ WARN] (2018-11-22 14:01:23.247) Rtabmap.cpp:2581::process() Loop closure 672->125 rejected!

The same bag file worked previously on ROS Indigo but since then I have upgraded to Kinetic and cannot revert back.
Can you suggest something.

Thanks

[matlabbe]

Hi Alex,

I did run demo_robot_mapping.launch multiple times with the sample demo_mapping.bag and the mapping is always fine.

Can you share the rosbag? Make sure you updated both rtabmap and rtabmap_ros projects.

The question is why "After that it just stops with no warning or error messages." is happening while odometry is still working. The only reason I see is that input topics of rtabmap node get not synchronized. You could add "--udebug" to args of rtabmap node to get more info. Copy/paste the latest debug log when the mapping stops at 80-85 percent.

cheers,
Mathieu

[alexr]

Hi Mathieu,

It was the problem of a bad dataset. I updated to the latest source and rebuild the package.

Everything working fine now.

Do you feel adding the following parameters will most likely improve the results ??

<param name ="Icp/PM" type="string" value="true"/>                       <!--Use libpointmatcher for ICP registration instead of PCL's implementation. -->             
          <param name ="Icp/PMConfig" type="string" value=""/>                      <!--Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., Icp/VoxelSize, Icp/DownsamplingStep), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters Icp/Iterations, Icp/Epsilon and Icp/MaxCorrespondenceDistance are also ignored if configuration file is set. -->
 	  <param name ="Icp/PMMatcherEpsilon" type="string" value="0.0"/>         <!-- [KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.]-->
          <param name ="Icp/PMMatcherKnn" type="string" value="1"/>               <!-- [KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.]-->
          <param name ="Icp/PMOutlierRatio" type="string" value="0.95"/>           <!-- [TrimmedDistOutlierFilter/ratio: For convenience when configuration file is not set. For kinect-like point cloud, use 0.65.]-->  

[matlabbe]

Hi,

Using Icp/PM=true gives generally better ICP optimizations. When using libpointmatcher for ICP, you can increase also Icp/MaxCorrespondenceDistance (like 0.5 or even 1 m) to be more robust to odometry drift.

Are you using 2D lidar? If so, with Icp/PM=true, you can use Icp/PointToPlane=true with 2d scans. For indoor environments, this gives better accuracy.

Note that to be able to enable Icp/PM, rtabmap should be built with libpointmatcher dependency (e.g., install libpointmatcher and rebuild/install rtabmap), otherwise you will have a warning/error on terminal.

cheers,
Mathieu

[alexr]

Hi,

Yes I am using 2d Lidar with libpointmatcher library previously build.

Another thing I noticed is that sometimes with GTSAM as the optimizer strategy the loop does not close while with g2o or TORO it closes nicely. Any reason for that and how to decide for what type of environment which optimizer is the best. I mostly plan to use indoors in a planar environment but would also like to try outdoor with 2D Lidar + IMU + stereo setup.

Thanks

[matlabbe]

Hi,

GTSAM as the optimizer strategy the loop does not close while with g2o or TORO it closes nicely

I would need the warning messages on terminal when loop closures are rejected to comment on this.

To determinate which optimizer is the best, after mapping, we can switch the optimizer to see how the graph is optimized. Most of the time we can see visually which gives the better optimizations. For more accurate metrics, we would have to record a ground truth and compare the error against it between all optimizers.

For "outdoor with 2D Lidar + IMU + stereo setup", the lidar may not be fed to rtabmap node, only maybe for obstacle detection (e.g., obstacle layer of move_base), unless the ground is a perfect plane.

cheers,
Mathieu