[fix] Fix issue 45 : Infinite loop on Windows

• The issue was linked to the use of std::advance on an iterator which goes out of range.
• Change it by a simple loop and check that the iterator doesn't go too far.

fixes https://gitlab.kitware.com/keu-computervision/slam/-/issues/45