- 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.
- 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.