[feat] Add bundle adjustment in SLAM

• This merge request is rebased on merge request #!218 (merged) (automatic detection of loop closure)

Bundle adjustment is a postprocess to improve the slam result. It considers two close frames have the similar feature so that they can form a loop closure. The bundle adjustment is applied after slam and add loop closure constraints between two close frames on a defined frequency.

The changes are made for bundle adjustment:

• Refact LoopClosureRegistration function in SLAM

  • Add BUNDLE_ADJUSTMENT into PGOConstraint
  • If enable bundle adjustment of PGOConstraint, build kdtree of target submaps in bounding box of source submaps. Generally, bundle adjustment is between two close frames which the matching keypoints are not so far.

• Add bundle adjustment process in OptimizaGraph in SLAM

  • Parameters for bundle adjustment
    • BAFrequency to set the bundle adjustment frequency. e.g. BAFrequency = 4 means to launch a LoopClosureRegistration each 4 frames.
    • BAInterval to set the interval between a query frame and a revisited frame.
    • BAStartFrameIdx and BAEndFrameIdx to set the range where to apply bundle adjustment
    • BAParams is a LoopClosure::Parameters type, which to tune loop closure parameters for bundle adjustment process.

• Add interface in PV wrapping

  • Each interface correspond to a variable of bundle adjustment

  • Use bundle adjustment constraint -> UsePGOConstraints[BUNDLE_ADJUSTMENT]

  • Bundle adjustment frequency -> BAFrequency

  • Interval size -> BAInterval

  • Start frame index -> BAStartFrameIdx

  • End frame index -> BAEndFrameIdx

  • Query map start range -> BAParams.QueryMapStartRange

  • Query map end range -> BAParams.QueryMapEndRange

  • Revisited map start range -> BAParams.RevisitedMapStartRange

  • Revisited map end range -> BAParams.RevisitedMapEndRange