ICP算法理解

1. ICP算法的含义
   ICP算法英文全称为iterative closest point，即最近点迭代算法。该算法用于具有公共点的两幅点云间的匹配或者点云与模型的匹配问题。
2. ICP具体流程
   设扫描仪拍摄到的点集为P、Q，假设Q为参考点集，为了将它们匹配起来，采用ICP需要两个步骤：
   1.选取P中的点集，通过计算该点集中每个点到参考点集的最小欧氏距离，计算得到对应点集。
   2.通过采用SVD分解求解刚体变换矩阵。
   3.计算P映射到Q的点集P`与Q中的对应点集的距离平方和d，若d小于设定阈值或者超过迭代次数停止迭代。
3. SVD分解详解
   对于两个位置拍摄的同一个物体的两幅点云 ，两组点云的质心及去质心坐标为：
   
   计算误差矩阵为：
   
   由于误差函数前两项均为常数项，为使误差函数最小，则第三项最大，第三项按照Trace性质可以化成：
   
   将H进行SVD分解得：
   
   因此上式可变形为：
   由柯西-施密特不等式得：
   
   当且仅当两个向量平行时取等。因此S=V，故矩阵R为：
   
   平移向量t可由下式计算：