WIP : [feat] Add loop closure

This loop closure is triggered by the user. The user has to specify the old frame with which the current frame is closing a loop. This feature should be triggered as a postprocess (same thread)

Loop closure works as follows :

1. Detect manually the loop closure
2. Stop the local SLAM
3. Build submaps around old frame specified
4. Find transform to register current frame onto the submap
5. Add transform to the pose graph
6. Optimize the graph and rebuild the maps

To perform this pipeline, the changes that were made are :

• Add LoopClosureSubMap function

  • It has 2 inputs: idxFrame and windowNb.
  • It creates a submap using frames between 'idxFrame - windowNb/2' and 'idxFrame + windowNb/2'.

• Add Interface for loop closure in LidarView

  • Logging timeout is a new field to set the oldest states stored in memory.
  • Auto detect loop closure frame to enable the automatic detection of loop closure. If disabled, need to indicate manually the index of the loop closure frame.
  • Frame index to enter the loop closure frame index. It is visible when 'Auto detect loop closure frame' is not selected
  • Window size to indicate the number of neighbor frames to use.