ICP算法（迭代最近点算法）详细推导

达代最近点算法（ Iterative closest point，简称ICP），是一种广泛使用的点云匹配方法，是求解两个点云集合转换关系的常用方法。本文将首先给出问题的数学描述与模型，列出具体的计算方法，并一步步详细进行推导。详细的内容请参考论文[1]。

1. 问题描述

有两组点云，分别用,表示，其中为参考帧，为当前帧的点云，共有N个点。我们需要计算从到的变化参数，即旋转R和半移T。如果变换参数是准确的，那么点云中的每一个点，经过变换后应该与点云中的点完全重合，即：。但由于有噪声的存在，不可能所有点都完全重合，所以我们定义目标函数：

                                                         

使目标函数最小的R，T即为所求变换参数。

2. 计算方法

首先计算两组点云的质心，分别记为，：

                                                              ，

对两组点云进行去质心，得到，

                                                          ， 

构建矩阵H：

                                                                     

对H矩阵进行SVD分解：

                                                                        

得到R与T：