[IDEA] ICP pose covariance estimation

[giafranchini]

An estimation for the output odometry pose covariance is needed. The algorithm computes a new pose p2 from the previous one p1 with known covariance applying the transformation p2 = T * p1 = T_icp * T_pred * p1, where:

• T_icp is the transformation between the input cloud and the local map, outputted by the ICP loop. Its covariance tracks:
  • sensor noise
  • randomness inherent to the ICP
• T_pred is the motion prediction used as initial condition for the ICP loop. Its covariance tracks:
  • errors due to wrong initialization

TODO:

• compute covariance of T_icp with the kalman method proposed by Yuan;
• compute covariance of T_pred;
• compose poses and related covariances as described here, chapter 5;