search

koide3 / glim

GLIM: versatile and extensible range-based 3D localization and mapping framework
MIT License
578 stars 77 forks source link

global_mapping_pose_graph fast forwards too much when standing still at the start #113

Open sgvandijk opened 1 week ago

sgvandijk commented 1 week ago

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: https://github.com/koide3/glim/blob/e27b4c675aa278ddc48d5483904524447c68cb2e/src/glim/mapping/global_mapping_pose_graph.cpp#L315 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 commented 3 days ago

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 commented 1 day ago

I just merged a patch to limit the fast forward steps up to 10 https://github.com/koide3/glim/pull/120.