In order to be able to integrate external sensors data to the ICP optimization step, the KeypointsRegistration class has to be refactored. The idea is to have an independent optimizer which can receive as many ceres residuals (cost function + robustifier) as required. These residuals could be supplied by any sensor. The optimization would then be solved after each ICP matching step integrating new computed matches and available external sensor constraints for the current studied frame.
Here are the major changes implied by this MR:
-
Split KeypointsRegistration file into LocalOptimizer and KeypointsMatcher files.
-
LocalOptimizer
- It is an object created locally in Ego-Motion and/or Localization functions.
- A residual structure is created in this file. It contains pointers to a cost function and a loss function (optimization estimator to remove outliers).
- It receives the ceres LiDAR residuals.
- It will be able to receive other sensors' residuals.
- It is entirely cleared at each registration step.
- It contains a ceres problem shared pointer. This problem is created and filled after each new matching step of ICP. The problem is maintained in order to be able to recover covariance from the optimization after convergence for last ICP iteration.
-
KeypointsMatcher
- It is an object created locally in Ego-Motion and/or Localization functions.
- It builds the LiDAR residuals (as described above) from matching between frame keypoints and map neighborhoods.
- It contains a vector of residuals (directly added in the matching result structure).
- A residual is also added to the matching info structure.