Prevent possible infinite loop in keyframe traversal

[mdailey]

Love this project!

I ran into a deadlock running ORB SLAM right after a loop closure after about 3 minutes of video. Tracking had stopped, local mapping had stopped, and the process was using about 1.2 CPUs but doing nothing visible. Attaching to the running process in gdb revealed that the tracking thread was waiting for the mpMap->mMutexMapUpdate lock, but it couldn't acquire it because the loop closure thread never completed its spanning tree traversal. I guess that somehow a cycle must have been introduced into the map's keyframe spanning tree.

I suppose the inconsistency in the tree should really be further investigated and fixed, but here I've just added a "visited" flag to the keyframe data structure to prevent loop closing from getting stuck indefinitely. That seems like a good idea regardless.

[AlejandroSilvestri]

@mdailey , that's a good idea indeed.

Did it eliminate your problem? Does the loop close as expected?

[mdailey]

@AlejandroSilvestri , I've done quite a few runs with the modification, I haven't gotten any deadlock, and loop closing seems to be working as expected. However, on the other hand, I also haven't gone into more detailed debugging to find out what if anything caused the cycle in the spanning tree in the first place. Still, seems to me that cycle avoidance is important.

[AlejandroSilvestri]

@mdailey , I must say I found a lot of broken pointers (mappoints and keyframes) while saving maps, I can't identify their cause, but the spanning tree is difficult to debug, I think there may be a bug in there.