ICP_TEST

this program is just for testing.

• implement point to point and point to plane 2D ICP using ceres.
• check the unobservability based on hessian matrix.
• calculating covariance with removing unobservable component.

references:

• 1.An ICP variant using a point to line metric —— pl-icp theory
• 2.An accurate closed-form estimate of ICP'S covariance - covariance estimate
• 3.A Closed-form Estimate of 3D ICP Covariance - extend 2d to 3d and it has opensource code in github.
• 4.Observability, Covariance and Uncertainty of ICP Scan Matching - Observability.
• And there is CSM package in Andrea Censi personal website which contains pl-icp and covariance code.
• you can also see my blog https://zhuanlan.zhihu.com/p/367705790.