Graph Optimization in Localization Mode

[g.bartoli]

Hi Mathieu,
as I told you in a previous post, now I have two machines, one with RTAB-Map 0.10.4 installed via apt-get (without SIFT/SURF/g2o/GTSAM support) and another one with RTAB-Map 0.10.6 built from source (with SURF/SIFT/g2o/GTSAM support). I am making tests with the same 3D sensor in the same room environment and with the same settings.

I noticed one strange behaviour, but I don't know if it's intended or not. In both version I can successfully map the environment, this the result of one mapping:



After that, I save the database, reload it and go in Localization mode. In both versions, after some seconds the sensor is relocalized in the environment and the correct pose is recovered, but in 0.10.4 the actual environment map is not altered in this mode, while in 0.10.6 the graph is continuosly optimized also Localization mode, which I think is not correct, since the map is already built and shouldn't be modified by localization, right? Additionaly, the pose is almost always correct, but the graph optimization in 0.10.6 is a bit unstable, i.e. the overall 3D structure slightly varies continuosly even when the sensor is in fixed position, the point cloud is always moving around adjusting itself and never reaching a still configuration. If you need, I can make an actual screencast to show you this behaviour.

Did you change something from 0.10.4 to 0.10.6 or do I need to change a parameter somewhere?

Thanks

[matlabbe]

Yes, I changed the localization behavior on loop closure detection to make it exactly like in mapping mode (so re-optimizing the graph). This fixed a localization jump bug when the memory management was activated and both approaches re-used the same code. When some nodes are retrieved from the Long-Term Memory to the Working Memory, the more convenient way to add them back in the optimized graph is to re-optimize it.

However, as you pointed out, when the whole graph is already in Working Memory, re-optimizing it is useless. I updated the code to handle this case (this commit). In localization mdoe, it will then re-optimize only when some nodes are brought back to Working Memory (extending the current map).

Thx for the observation!

cheers,
Mathieu