mapCorrection rejection based on odometry accuracy

[guoqingzh]

We have encountered situation that when a scene is very similar to another (ex. long similiar corridor), Rtabmap is tended to make a wrong loop closure assumption and update mapCorrection with a huge transformation. This kind of behavior should be preventable by checking the timestamp of previous mapCorrection and current timestamp correction and estimate the likelyhood of the current mapCorrection based on odometry covariance. mapCorrection is associated with odometry drift , if it is apparently a huge drift which is unlikely to happen with the know odometry accuracy, it should be rejected.

Any comments/thoughts ?

[borongyuan]

Have you enabled robust graph optimization?

[guoqingzh]

Have you enabled robust graph optimization?

Yes, tried. All the other configurations kept the same, toggled with the following two lines: "RGBD/OptimizationMaxError 0" "Optimizer/Robust true" vs "RGBD/OptimizationMaxError 1" "Optimizer/Robust false"

The one without Robust optimization gives better mapping result. In our scenario, we have long corridors next to each other, they all look similar

[matlabbe]

If you use default RGBD/OptimizationMaxError, it should be already able to reject those wrong loop closures, unless your odometry covariance is too large. How is odometry computed?

[guoqingzh]

If you use default RGBD/OptimizationMaxError, it should be already able to reject those wrong loop closures, unless your odometry covariance is too large. How is odometry computed?

@matlabbe unfortunately, optimitzationMaxError doesn't always work. Imagine the scenario where you have two very similar long corridors in parallel to each other, the robot explores the first corridors from beginning util the end , then continue with the second one. Based on the visual matching, the maximum likelyhood estimation is very likely exceeded LoopThr threshold due to similarity hence gives a wrong hypothesis. In this case, we have no link restriction to prevent optimization merging the second corridor to the first one.

Odometry based neighbor link may add restriction between neighboring nodes but when you concatenate ten of nodes, the allowed error in orientation can easily accumulate to 10-20 degrees ( covariance 0.001=> ~2 degree error/pair, 10 pairs will give us 20 degree). If the corridor is long enough (our case), rtabmap is quite easy to draw the wrong conclusion on loop closure hence causing wrong map.

Our odometry is calculated based IMU and wheel odometry. orientation covariance 0.001 is a reasonable value according to the sensor datasheet. Another thing I want to ask is that why rtabmap take twist covariance as odometry covariance ? twist is velocity in ROS not really the odometry. Do we do proper covariance propagation with delta t ?

[guoqingzh]

@matlabbe I investigated a bit further the Neighbor link and its covariance, Rtabmap_ros gives maximum velocity covariance as input to Rtabmap::process() when neigbhor refinement is off. The code is here https://github.com/introlab/rtabmap/blob/master/corelib/src/Memory.cpp#L1027. This is wrong, right ? The correct covariance should be propagated from the velocity covariance.

[matlabbe]

The pose covariance would increase out of bound, and twist covariance would be ~constant. The propagated covariance is indirectly done during graph optimization, where each link contains twist covariance.

For the two close parallel corridors, RGBD/OptimizationMaxError may indeed not detect it wrong, unless setting it very low, though if it is too low it may reject all (including good) loop closures.

Rtabmap_ros gives maximum velocity covariance as input to Rtabmap::process()

The link you shown is just forwarding the covariance: https://github.com/introlab/rtabmap/blob/64f79813cd2c3a7e3f916a7ef2637e93f1f13a5f/corelib/src/Memory.cpp#L1026-L1027

From ros the covariance is taken from the Odometry msg like this: https://github.com/introlab/rtabmap_ros/blob/c1d1c01d5d732784ceb6b225ebf6eebdb700ceb0/rtabmap_slam/src/CoreWrapper.cpp#L1062-L1085