Difficulty in understanding the code for proximity detection by space

[lockqueue]

Hi, first I want to thanks a lot for this great software library!

When I'm reading the code for proximity detection by space, I have one place that I could not understand: https://github.com/introlab/rtabmap/blob/2a840fe340a31a62a9c4860bb22035f97e6a3f36/corelib/src/Rtabmap.cpp#L2669-L2679

In the Line 2669, we get a pose from _optimizedPoses without inverting it, and assign it into the currentPoseInv. Are all the poses in _optimizedPoses actually the inverse of poses? In the Line 2679, we multiply it with another pose to calculate distance, which makes sense only if this currentPoseInv is truly the inverse of a pose, rather than the pose itself.

Or, might it simply be a typo that we forgot to have .inverse() on the Line 2669?

Thanks!

[matlabbe]

Yeah, I think you are right. The change was introduced in this commit https://github.com/introlab/rtabmap/commit/4776de7931bf7613aae2d00a23258137057c1313

It should be:

Transform currentPoseInv = _optimizedPoses.at(signature->id()).inverse(); 

I think that most of the time it is rare that we get exact same likelihood for 2 nodes in the same proximity path, so the distance check after is ignored in general. I'll make the change though, as when it happens, the result is probably wrong.

EDIT: actually this would happen all the time in lidar-only SLAM (where there would be no likelihood). It doesn't matter too much which node on the proximity path is the main node for the link, though it is preferable to be the closest one. I t should be fixed by https://github.com/introlab/rtabmap/commit/7c601bb6e8701ea211d55d7263d69751b5b833f3