点云精配准-ICP

Iterative Closest Point（ICP）

简介

• ICP 算法的第一步就是找到 Source 点云与 Target 点云中的对应点（corresponding point sets）（此时的 Source 点云已经使用初始R进行转换），然后针对对应点，通过最小二乘法构建目标函数，进行迭代优化。

算法步骤

• 点云预处理：滤波，清理
• 使用最近邻算法找到最近领点（距离，ann：kdtree和八叉tree）
• 调整对应点的权重**？**
• 剔除不合理的对应点**？**
• 计算并最小化 loss：
  对每个点去质心（以质心为原点，去掉距离的影响），构建目标函数（变换后距离最小化）
  每对对应点的乘起来构成3*3的矩阵（列x行）累加和，对该矩阵进行svd分解，根据推导得到R=vuT（这里中间的R是不是=1，这里需要看vuT行列式是不是=1），平移为p质心-Rq质心。这部分推导
• 迭代
  用上述求的rt继续loss