[开源]超全ICP实现，g2o,ceres,G-N,SVD等

Posted 2022-05-25 chengwei0019

最近在看高博的《视觉SLAM十四讲》的第6章，就做了一些小测试，整理了一下各种ICP实现方式，包括高斯牛顿法求解，SVD求解，ceres求解，g2o求解，还包含了一些开源库的测试，如icp，ndt，sicp，ndt_cpu等方法；

代码已经上传到github，如需下载请戳这里：源码（记得给我点个Star哈，嘿嘿）

关于SVD的求解方法，可以参考我上一篇博客，这里就不展示了。

关于高斯牛顿法/ceres/g2o求解的方法，主要是扰动模型的求导，具体如下：

其中，x,y为两组最近邻点对，i为索引值；R，t为两组点云之间的旋转和平移；

将上述公式对t求导，

对R进行求导，这里使用扰动模型求解： 

 

则雅克比矩阵为

海森矩阵为 

求导出雅克比矩阵后，剩下的就直接套用第6章 非线性优化里的例子，写一下代码即可。

 测试：

输出：