初入pcl之ICP算法简介

ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P = {Pi, i = 0,1, 2,…,k}及U = {Ui,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k ≥ n。配准过程就是求取 2 个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：
(1)计算最近点，即对于集合U中的每一个点，在集合P中都找出距该点最近的对应点，设集合P中由这些对应点组成的新点集为Q = {qi,i = 0,1,2,…,n}。
(2)采用最小均方根法，计算点集 U 与 Q 之间的配准，使 得到配准变换矩阵 R，T，其中R是 3×3 的旋转矩阵，T 是 3×1 的平移矩阵。
(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U1，即U1 = RU + T
(4)计算U1与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U1替换U，重复上述步骤。
VTK中有一个类vtkIterativeClosestPointTransform实现了ICP 算法，并将ICP算法保存在一个4×4的齐次矩阵中。那么如何使用这个类的函数内？以下是一个可参考的DEMO，功能是获得两个坐标系内的点之间的对应关系，也就是求这两个坐标系之间的平移和旋转矩阵。