global_mapping_pose_graph fast forwards too much when standing still at the start

[sgvandijk]

Describe the bug
Thanks very much for making this great project available! I have tried out global_mapping_pose_graph since in some cases it seems to give better loop closures for me when the travel distance between those closures are very far (>1km).

However, in some cases it fails to find any closures, and I tracked it down to the fast forwarding logic in find_loop_condidates. Specifically here:

glim/src/glim/mapping/global_mapping_pose_graph.cpp

Line 315 in e27b4c6

const int step = 0.8 * direct_dist / std::min(travel_dist_avg, 100.0);

When the travel distance between the first submaps is very small, in my case in the order of centimetres, and the direct distance is large, then the fast forward step is very big. In my case this can be in the 1000s, and it always fast forwards past submaps.size().

I removed the fast forward logic, and then things worked well for me.

Expected behavior
I am not sure what the ideal behavior would. Setting an upper limit on step size may make sense, but that would reduce the usefulness of the fast forwarding in cases where big steps are the correct thing to do. Alternatively a lower limit on travel_dist_avg could help, but a reasonable value of that may differ case by case.

[koide3]

Thanks for reporting the issue. I'll add a patch to limit the maximum fast forward steps as a quick fix. I'm also thinking of implementing a better proximate keyframe selection (maybe with a dynamic KdTree).

[koide3]

I just merged a patch to limit the fast forward steps up to 10 #120.