ICP 匹配定位算法学习记录

icp 算法原理是：

选取目标点云P和源点云Q，按照一定的约束条件，找到最邻近点（pi,qi）,然后计算出最优R和t（旋转和平移），

使得误差函数最小，误差函数E（R，t）：

                                                                  

　　　　　　　　　　　　　　　　　　　　　　

基本算法流程：

1、在目标点云P中选取点集pi。

2、在源点云Q中选取对应的点集qi，使得||qi-pi||==min，也就是距离最小，这里的距离为欧氏距离。

3、计算旋转R和t，使得误差函数最小。

4、此时经过步骤3的R旋转和t平移后得到新的点集pi`，pi`=Rpi+t

5、计算pi`到点集qi的平均距离d。

6、如果d小于预设的阈值或者超过了迭代的次数，则停止，否则跳到步骤2，直到满足收敛条件。

 

算法重点问题：

1，原始点集的采集方法：

1）均匀采样、

2）随机采样

3）法矢采样

 

2，确定对应的点集方法：

1）点到点

2）点到投影

3）点到面

 

3，计算变化矩阵：

1）四元素法

2）svd奇异值法

 

欧式距离：

1、二维： 

2、三维