Robust Graph Optimization does not work when Reduce Graph is enabled

[borongyuan]

Hi, I only recently noticed this issue, because in most of the scenes we tested, there was no situation where the images captured at different positions were highly similar. But I later found a scene where it was easy to study this situation. In the staircase scene shown in the video below, the images on different floors are highly similar, resulting in many incorrect loop closure dections. Vertigo successfully rejected these constraints, so the graph was not damaged. But this is when Reduce Graph is not enabled. Once Reduce Graph is enabled, Robust Graph Optimization no longer takes effect. This is because switchable edge factor is not used for Link::kNeighborMerged. But using switchable edge factor for Link::kNeighborMerged is not a good idea either, because it might completely ignore constraints between neighbors. We probably need to solve this issue in other parts. Some nodes should not be merged in the first place because the detected loop closure is wrong. Is there a better fix for this?

rtabmap/corelib/src/optimizer/OptimizerGTSAM.cpp

Lines 724 to 737 in 52e770e

	#ifdef RTABMAP_VERTIGO
	if(this->isRobust() &&
	iter->second.type() != Link::kNeighbor &&
	iter->second.type() != Link::kNeighborMerged)
	{
	// create switchable edge factor
	graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
	}
	else
	#endif
	{
	graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
	lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
	}

Robust_Graph_Optimization_Test.mp4

[matlabbe]

Comparison between Optimizer/Robust (Vertigo: ignore bad loop closures during optimization, but they are still there in the graph) and RGBD/OptimizeMaxError (reject bad loop closures when they occur) approaches is shown in https://github.com/introlab/rtabmap/wiki/Robust-Graph-Optimization

It is stated in RGBD/OptimizeMaxError description that it cannot be used with Optimizer/Robust at the same time, but maybe they could now, as I don't see why RGBD/OptimizeMaxError could not also see that a loop closure constraint (presumably detected bad from Vertigo) is more deformed than it should be, thus rejecting it. More tests would be required to make sure of it.

Mem/ReduceGraph assumes that all loop closures are good, thus we should make sure that no bad loop closures are accepted. Currently only RGBD/OptimizeMaxError will reject them. If bad loop closures are accepted, maybe it is because the odometry covariance is over-estimated, allowing too much deformation of the odometry constraints. Either reduce the odometry covariance if you have control over it, or decrease RGBD/OptimizeMaxError parameter.

Don't know if you can share that database reproducing the issue, could be useful to test on.

[borongyuan]

It is stated in RGBD/OptimizeMaxError description that it cannot be used with Optimizer/Robust at the same time, but maybe they could now, as I don't see why RGBD/OptimizeMaxError could not also see that a loop closure constraint (presumably detected bad from Vertigo) is more deformed than it should be, thus rejecting it. More tests would be required to make sure of it.

You are right. In this scenario, if RGBD/OptimizeMaxError is set to 3, these wrong detections can also be rejected. But to show the effect of robust graph optimization, I used the following configuration when recording this demo:

RGBD/OptimizeMaxError=0.0
Optimizer/Robust=true
Mem/ReduceGraph=false

Mem/ReduceGraph assumes that all loop closures are good, thus we should make sure that no bad loop closures are accepted.

Thanks, I just wanted to confirm this statement. Even if it may not be easy to solve the compatibility issues of these features, it is important to make this clear.

Don't know if you can share that database reproducing the issue, could be useful to test on.

I didn't save the data from that day's test. Because the running results of different configurations are actually in line with our expectations. I'll re-record the data in a few days if that helps resolve this issue. I think I need to set Mem/ReduceGraph to false when recording. We can change it to true later when reprocessing the data. Otherwise, once some nodes are merged, we will not be able to restore the constraints to the way they were before the map was reduced.

[matlabbe]

I think I need to set Mem/ReduceGraph to false when recording. We can change it to true later when reprocessing the data. Otherwise, once some nodes are merged, we will not be able to restore the constraints to the way they were before the map was reduced.

Good point! Enabling Mem/ReduceGraph disables the ability to replay/reprocess the database. I'll try to improve the description of Mem/ReduceGraph with those caveats.
EDIT: it seems that we may not need to remove the kNeighbor links of the removed node here, so that we could reprocess a database that has been reduced.

rtabmap/corelib/src/Memory.cpp

Line 1268 in cb0cc9e

s->removeLink(iter->first);

[borongyuan]

https://drive.google.com/file/d/1ZfQMK5wsRMqQrlcfoBnkFLTUtNULCTKs/view?usp=sharing

I recorded the data using the following configuration:

RGBD/OptimizeMaxError=0.0
Optimizer/Robust=true
Mem/ReduceGraph=false
VhEp/Enabled=false

In this scenario, only the start and end points constitute a true loop closure. All other detections should be rejected.

[matlabbe]

Hi,

Your database looks fine. When processing it, rtabmap would report "accepted loop closure" but in reality the graph is still correctly optimized (the link is ignored). Here I highlighted in green the links that are "ignored" during optimization by setting outlier threshold to 1 meter (see right click-menu in the Graph View, then "Set link color...")

Based on the 3D Graph, it seems ok. So the original issue was that because the wrong loop closures are still "accepted" while been correctly turned off in graph optimization, if Mem/ReduceGraph is enabled, it will still merge those wrong loop closures. When reprocessing the database, it seems I disabled usage of both RGBD/OptimizeMaxError and Optimizer/Robust are the same time:

In the commit above, I removed that check to allow it. It seems to work as expected when both are used together. We should use at least RGBD/OptimizeMaxError with Mem/ReduceGraph, but Optimizer/Robust can also be used at the same time. I updated parameters' description.

For the "reprocessing" of a database with reduced links, it seems my suggestion of my previous post works, but I needed to fix also how DBReader was reading the covariance. I did other QoL updates at the same time.