[feat][refact] Add loop closure

• This MR supersedes an old merge request #!190 (closed)

This loop closure is triggered by the user. The user has to specify a revisit frame with which a query frame is closing a loop. This feature should be triggered as a postprocess (because it is not real time and played on the same thread as local SLAM).

To use this feature, the user must follow this pipeline :

1. Stop the local SLAM when a loop closure is observed
2. Enter manually the query frame and the revisit frame indices for the detection of loop closure
3. Estimate the transform and the corresponding covariance between the query frame and the revisit frame
4. Add loop closure constraint to the pose graph optimization
5. Optimize the graph and rebuild the maps with optimized poses

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

------------------------Refact in CMake------------------------

• Optimize graph requires g2o, g2o definition is updated in CMakeLists

------------------------Refact in SLAM------------------------

• Refact ClearMaps : add maps argument to clear any maps
• Add helper function BuildMaps :
  • Create maps with keyframes stored in Logstates
  • With default values, the function creates maps with all frames stored in LogStates.
    • This process replaces the previous function UpdateMaps.
  • Otherwise, it creates sub maps with frames lying in an indices window [windowStartIdx, windowEndIdx].
  • Maps are built in world coordinates by default or in base coordinate of frame #idxFrame when baseCoordinate is enabled
• Refact UpdateMaps :
  • Add an option to choose whether reset the full maps before updating.
• Add helper function EstimatePose :
  • Estimate pose by ICP-LM optimization
  • Estimate a pose of a keypoints set w.r.t another one.
  • Replace ICP-LM optimization used in functions ComputeEgoMotion, Localization and LoopClosureRegistration.
• Add InitSubMaps function to initialize maps with the same parameters as LocalMaps.

------------------------Functions for loop closure------------------------

• Add LoopClosureRegistration function in SLAM
  • Build submaps around the revisit frame.
  • If LoopClosureICPWithSubmap is true, build another submap around the query frame.
  • If EnableLoopClosureOffset is true, add an offset to prior pose. This is used in case of big drift to recenter one onto the other.
  • Call EstimatePose to get the pose of the query frame w.r.t to the submaps of the revisit frame.
  • It computes the relative transform and the covariance between the revisit frame and the query frame.
• Add AddLoopClosureConstraint function in PoseGraphOptimizer
  • Add an edge between 2 vertices in the pose graph. The vertices' indices are inputs of the function.
• Add loop closure process in OptimizeGraph function in SLAM
  • Step 1: call LoopClosureRegistration to estimate pose and covariance for the source frame w.r.t. the target frame
  • Step 2: call AddLoopClosureConstraint to add the edge into pose graph

------------------------LidarView interface------------------------

• Add Rebuild maps in inputs/outputs category.

  • Add Reset maps before rebuild maps Re-calculates the maps with all states saved in LogStates.

• Add Use pose graph in General Parameters category to display 'Pose graph optimization' category.

• Add Pose graph optimization category.

  • OptimizeGraph is a button to launch the PGO process : call OptimizeGraph function in SLAM and update the trajectory after optimization.
  • Save g2o file : path to the g2o file where to save the pose graph (if empty, nothing is saved).
  • Logging timeout : Maximum duration on which to keep states in memory. WARNING : It should be an adequate value to launch loop closure process.
  • Covariance scale : Factor applied to the output SLAM covariance.
  • Graph iterations : Number of G2O graph optimization iterations.
  • Fix first vertex : if enabled, first vertex will not be changed.
  • Fix last vertex : if enabled, last vertex will not be changed.
  • Enable loop closure : to display the loop closure category.

• Add a loop closure parameters category for SLAM online. This category is visible when Enable loop closure is selected.

  • External detect loop closure frame to enable the manual detection of loop closure. If enabled, need to indicate manually the index of the query and the revisit frame index of loop closure.
  • Query frame index
  • Query map window start interval LC and Query map window end interval LC : sizes of the submaps around the query frame index [query frame index + query map window start interval LC, query frame index + query map window end interval LC]
  • Revisit frame index
  • Revisit map window start interval LC and Revisit map window end interval LC
  • Advanced settings : to enable the category of optimization parameters

• Add loop closure Registration ICP matching and optimization parameters category.

  • Add offset could be enabled when 2 frames of loop closure is too far.
  • Use submaps to enable the registration between two submaps instead of registering a single frame onto a submap.
  • Other parameters of matching and optimization parameters.

------------------------Demo for loop closure------------------------

A result of pose graph optimization with loop closure constraint on the car data : loop closure demo