【附优化方法的ICP源码】ICP与NDT匹配算法精度对比，以及手动实现的ICP和基于优化方法的ICP精度对比

补充内容(2021年3月31号)

很多朋友给我留言想要源代码，但是最近实在太忙不能一一回复，实在是抱歉！

由于，这个项目的代码有别的用处，所以不能全部公开给大家，但是我把关于ICP的部分整理出来了，上传在github上了，你可以去下载，里面是我通过优化的方法实现的ICP算法，其迭代方法是高斯牛顿。

代码我上传了github上了，如需下载请点这里：源码在此（记得给我点Star哈，哈哈）

关于优化方法的ICP具体推导过程，我就不在此展示了，给出一个推导结果，大家直接对照代码看，很容易理解。

公式推导如下：
$$f=minE(R,t)=min\frac{1}{N_y}\sum \Vert R{y}_i+t-x_i\Vert \text{\hspace{1em}(0)}$$
其中：

• $i$为通过最近邻搜索匹配上的点对，$x_i$和$y_i$分别为目标点云和待匹配点云中的点
• $R$，$t$分别为两个点云之间的旋转和平移变换

将（0）式分别对R和t求导，关于$R$的求导使用的是扰动模型，求导结果如下：
$$\frac{\partial f}{\partial t}=I$$$$\frac{\partial f}{\partial R}=-(R\ast x_i{)}^{\wedge }$$

雅克比矩阵为$J=[\frac{\partial f}{\partial t},\frac{\partial f}{\partial R}]$
则，海森矩阵为$H=J^{T}J$

求出雅克比矩阵之后，剩下的就很简单了，套用高斯牛顿的优化方法进行迭代就能获得最终的结果了。具体流程参考代码：快速直达

=============================== 我是分割线 ===============================

1.主要内容

本篇博客主要对比Iterative Closest Point（ICP）和Normal Distribution Transform（NDT）两种匹配算法的精度。分别依赖ICP和NDT作为匹配方法，生成前端里程计，没有回环检测，没有优化。

2.实验条件

①. 所用数据集: KITTI 00数据

②. 计算机平台

• 处理器：Intel® Core™ i7-10510U CPU @ 1.80GHz × 8
• 内存：8G

③. ICP和NDT算法来自PCL 1.8库

④. 精度评估工具evo

⑤. 算法所用参数

• ICP

    icp_ptr_->setTransformationEpsilon(0.01);
    icp_ptr_->setMaximumIterations(30);
    icp_ptr_->setEuclideanFitnessEpsilon(0.01);
    icp_ptr_->setMaxCorrespondenceDistance(1)

• NDT

    ndt_ptr_->setResolution(1.0);
    ndt_ptr_->setStepSize(0.1);
    ndt_ptr_->setTransformationEpsilon(0.01);
    ndt_ptr_->setMaximumIterations(30);

3. 实验结果

依赖ICP匹配的里程计

依赖NDT的里程计

这里我就不展示相对轨迹误差和绝对轨迹误差相关的结果，图上的结果很明显！

4.结论

对于SLAM的里程计而言，NDT的匹配精度明显优于ICP。此结论只对里程计有效，对于一些别的应用场景下的点云匹配，该结果尚未验证！另外，请谨慎使用pcl的icp算法，似乎它的精度并不高！

补充（2020年12月1日）

根据icp的论文，我自己手动实现了一个icp算法，发现无论是速度和精度上都明显高于pcl的icp算法，我试图去看pcl的源码找到这其中的原因，但是我并没有花太多时间，所以也没有找到原因。（如果你知道为什么，麻烦给我留言，感激不尽！）

另外icp的论文对于残差函数选择的是推导，然后使用svd分解。我尝试过对残差函数进行直接求导，然后通过高斯牛顿的迭代方法求解，发现这种优化的方法精度更加高，但是计算资源和时间会明显高于svd分解方式的icp。

以下两张图是手动实现的icp算法，一张是通过svd分解的icp，一张是通过优化的方法实现的icp，将它们加入到没有回环检测的前端，获得的效果如下：

推导公式方式实现的icp

基于icp残差公式，采用高斯牛顿方法优化的icp

如果需要代码和数据集，请给我留言！

Hello World