Initial guess during scan matching with offset

[ikhann]

For example, if parameter enable_imu_frontend has value false, than matrix from some sensor fusion algorithm msf_delta.matrix() is identity. So we provide transformation matrix on the previous step prev_trans as initial guess:

https://github.com/koide3/hdl_graph_slam/blob/95677a67e508e0cfebb5b0cf4ce0469214480673/apps/scan_matching_odometry_nodelet.cpp#L188

After registration procedure we get a new estimation of transformation and update prev_trans matrix:

https://github.com/koide3/hdl_graph_slam/blob/95677a67e508e0cfebb5b0cf4ce0469214480673/apps/scan_matching_odometry_nodelet.cpp#L211

But if we have too large delta between neighbouring transformations (when condition on line 219 is True), we reset the transformation matrix prev_trans and set it to identity: https://github.com/koide3/hdl_graph_slam/blob/95677a67e508e0cfebb5b0cf4ce0469214480673/apps/scan_matching_odometry_nodelet.cpp#L219-L226

Thus in the next iteration on line 188 scan_matching_odometry_nodelet.cpp we will again have identity matrix as an initial guess.

My question: Why do we reinitialize registration procedure and reset the accumulated transformation matrix prev_trans and not use it as an initial guess, even if the offset is large between scans? Is it less accurate than an identity matrix as an initial guess?

[koide3]

This scan_matching_odometry_nodelet updates the scan matching target frame only when the sensor moves a certain distance because if we update the target every frame, the registration preprocessing (e.g., NDT voxelization) cost gets larger, and the error accumulates rapidly. (You can find keyframe TF frame on rviz that is the pose of the scan matching target frame updated at a lower frequency) The if-branch at L219 updates the target frame, and it needs to reset prev_trans because the reference coordinate is moved.