迭代最近点(ICP)算法-手动实现

目录

一、算法介绍

1 引言

2 具体求解过程

二、代码实现

1 头文件

2.源文件

总结

---

一、算法介绍

1 引言

        在点云粗配准之后，我们还需要对点云进行精配准。对于点云的精配准问题，学者们提出了许多不同的算法，例如点标记法、自旋图像等，但其中应用最广泛、最为大众熟知的便是由Besl和Mckay于1992年提出的迭代最近点（Iterative Closest Point，ICP）算法。是最为经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.ICP算法本质上是一种基于最小二乘最优匹配思想的配准算法，它不需要事先知道对应的特征匹配点对，可以直接对点云进行操作，该算法流程简单，容易实现，在初始位置较好的情况下可以快速获得较好的配准结果。下面探讨ICP算法的实现原理。ICP算法通过迭代求解点云之间的最优旋转平移矩阵。其基本思想是选取欧式距离作为衡量是否为对应点的原则，建立对应点集；在此基础上