Open3D ICP精配准（点到点）

一、算法步骤

（1）在目标点云P中取点集pi∈P；
（2）找出源点云Q中的对应点集qi∈Q，这里使用的是点到点之间的欧式距离，使得||qi-pi||=min；
（3）计算旋转矩阵R和平移矩阵t，使得误差函数最小；
（4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的对应点集pi’={pi’=Rpi+t,pi∈P}；
（5）计算pi’与对应点集qi的平均距离；
（6）如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。否则返回第2步，直到满足收敛条件为止。

二、实现代码

相关配置可以参看：Open3D C++文章目录汇总中的基础配置部分，相关代码如下所示：