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:

rtabmap/corelib/src/Rtabmap.cpp

Lines 2669 to 2679 in 2a840fe

	Transform currentPoseInv = _optimizedPoses.at(signature->id());
	for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
	{
	const std::map<int, Transform> & path = iter->second;
	float highestLikelihood = 0.0f;
	int highestLikelihoodId = iter->first;
	float smallestDistanceSqr = -1;
	for(std::map<int, Transform>::const_iterator jter=path.begin(); jter!=path.end(); ++jter)
	{
	float v = uValue(likelihood, jter->first, 0.0f);
	float distance = (currentPoseInv * jter->second).getNormSquared();

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 4776de7

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 7c601bb