slam-----icp匹配算法学习（基础理解）

slam-----icp匹配算法学习（基础理解）

icp匹配计算的是两幅点云之间的变换矩阵（旋转和平移）。
在ros定位模块中，使用icp算法进行定位，首先把地图转换成点云(source P‘)，再把当前帧的激光变成点云(targe P)，使source与targe配准，即找到两者之间的旋转和平移量(其过程是一个迭代的过程)。最后将其作用到机器人的上一时刻 的base_to_map坐标系上。完成了一次定位优化。
其中心思想就是p’经过旋转R和平移t， 使p‘与p重合，即

求能够使其误差最小的R和t。(构建最小二乘法，使其误差平方和达到最小)

icp匹配的流程如下：
1：寻找p中每一个点云，对应在p’中的最临近点，构成点云集合C。
2：分别选找p'和p的中心点，(计算方式就是横纵坐标分别求和除以点云数量???)
3: 通过SVD先计算R 计算出(旋转量)
4：根据计算出的R，求出t

5：开始迭代，通过R×p‘+t得到新的点云A1，重复1-4步，此时是寻找A1中每一个点云，对应在p’中的最临近点。
6：求得到的点云An和它的最近点集Cn的平均距离dst，当dst小于设定的阈值时，跳出循环。

如果发现还不准确，那么有可能是它的迭代条件——也就是平均距离dst判断出错了，出现这种原因一般就是点云中出现了离散点，导致某两点的距离出现了异常，带动了整个dst判断出错。解决方案如下（很管用）：
遍历p’点找寻最近点，如果p‘中的某个点pi和它的最近点距离大于某个阈值，则剔除，不参与接下来的计算。