Major refactoring of undistortion process.
OPTIMIZEDundistortion mode : rarely used because it is slower and unstable
- Perform undistortion outside from
KeypointsRegistrationto greatly simplify code organization
APPROXIMATEDundistortion mode into
- Add/fix undistortion support in pose graph optimization
This also removes the prior that the
Tworld transform which was being optimized in
KeypointsRegistration had to correspond to the frame end pose time.
Tworld always correspond to the transform at input lidar frame header timestamp, which could correspond to the timestamp of any point (even no point) of the frame.
WithinFrameMotion is now expressed in BASE coordinates system, and is really used only for undistortion in BASE. It obeys
WithinFrameMotion(0) = Identity, to respect the fact that
Tworld is the transform at the frame header timestamp.