ICP算法（迭代最近点算法）详细推导
达代最近点算法（ Iterative closest point，简称ICP），是一种广泛使用的点云匹配方法，是求解两个点云集合转换关系的常用方法。本文将首先给出问题的数学描述与模型，列出具体的计算方法，并一步步详细进行推导。详细的内容请参考论文[1]。
1. 问题描述
有两组点云，分别用,
表示，其中
为参考帧，
为当前帧的点云，共有N个点。我们需要计算从
到
的变化参数，即旋转R和半移T。如果变换参数是准确的，那么点云
中的每一个点
，经过变换后应该与点云
中的点完全重合，即：
。但由于有噪声的存在，不可能所有点都完全重合，所以我们定义目标函数：
使目标函数最小的R，T即为所求变换参数。
2. 计算方法
首先计算两组点云的质心，分别记为，
：
，
对两组点云进行去质心，得到，
，
构建矩阵H：
对H矩阵进行SVD分解：
得到R与T：