计算机视觉中的ICP算法

本文主要参考了
http://www.cnblogs.com/yin52133/archive/2012/07/21/2602562.html
http://blog.sina.com.cn/s/blog_8c84cf510100yv36.html
http://blog.csdn.net/wangjie0377/article/details/7098446

ICP（Iterative Closest Point迭代最近点）算法是一种基于轮廓特征或点集对点集的点配准方法如下图

这里有两个点集，红色部分和蓝色部分。
ICP算法就是计算怎么把PB平移旋转，使PB和PR尽量重叠， 并建立模型。ICP是改进自对应点集配准算法的一种优化算法。
对应点集配准算法是假设一个理想状况，将一个模型点云数据X（如蓝色点集）利用四元数旋转，并平移得到点云P（如红色点集）。而对应点集配准算法主要就是怎么计算出qR和qT的，知道这两个就可以匹配点云了。但是对应点集配准算法的前提条件是计算中的两个点云数据的元素一一对应，这个条件在现实里因误差等问题，不太可能实线，所以就有了ICP算法。
ICP算法是先计算出从源点云上的（蓝色部分）每个点到目标点云（红色部分）的每个点的距离，使每个点和目标云的最近点匹配，（记得这种映射方式叫满射吧）。

这样满足了对应点集配准算法的前提条件、每个点都有了对应的映射点，则可以按照对应点集配准算法计算，但因为这个是假设，所以需要重复迭代运行上述过程，直到均方差误差小于某个阀值。
也就是说 每次迭代，整个模型是靠近一点，每次都重新找最近点，然后再根据对应点集批准算法算一次，比较均方差误差，如果不满足就继续迭代。

这个算法的主要思想如下所示：
基准点在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)计算两个点集Q和U的重心位置坐标，并进行点集中心化生成新的点集； 由新的点集计算正定矩阵N，并计算N的最大特征值及其最大特征向量；
(3)由于最大特征向量等价于残差平方和最小时的旋转四元数，将四元数转换为旋转矩阵R；在旋转矩阵R被确定后，由平移向量t仅仅是两个点集的重心差异，可以通过两个坐标系中的重心点和旋转矩阵确定；其中R是 3×3 的旋转矩阵，T 是 3×1 的平移矩阵。
(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U1，即U1 = RU + T
(4)计算U1与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U1替换U，重复上述步骤。