[feat][refact] Update loop closure
This MR contains updates for the loop closure detection part:
--------In SLAM------
-
Create a new structure of LoopIndices containing QueryIdx, RevisitedIdx and Time
-
A vector LoopDetections of LoopIndices to is added in SLAM to store detected loop indices. This vector can be filled with a external csv file which contains loop indices. Or it can be filled with detected loop indices by function
DetectLoopClosureIndices
.Function
AddLoopClosureIndices
is added to fill this vector. -
DetectLoopClosureIndices
becomes a public function which is a wrapping function of loop detection methods. (Only teaser++ for now, other loop closure detection methods will be added in the future.)
--------In PV wrapping------
-
Add
LoadLoopDetectionIndices
function to load loop closure indices from a csv file.A helper function
CreateCSVLoader
is added -
Add
DetectLoop
function. Users can trigger a detection via buttondetect loop
. If a loop is detected, indices are added into vector LoopDetections. They can be used for pose graph optimization. -
Some refacts for the interface
Here is a demo which shows the pose graph optimization with loop closure. The loop indices are loaded from an external csv file.