ICP 算法的直观想法如下:
- 如果我们知道两幅点云上点的对应关系,那么我们可以用 Least Squares 来求解 R, t 参数;
- 怎么知道点的对应关系呢?如果我们已经知道了一个大概靠谱的 R, t 参数,那么我们可以通过贪心的方式找两幅点云上点的对应关系(直接找距离最近的点作为对应点)。
ICP 算法实际上就是交替进行上述两个步骤,迭代进行计算,直到收敛。
ICP 一般算法流程为:
1. 点云预处理
- 滤波、清理数据等
2. 匹配
- 应用上一步求解出的变换,找最近点
3. 加权
- 调整一些对应点对的权重
4. 剔除不合理的对应点对
5. 计算 loss
6. 最小化 loss,求解当前最优变换
7. 回到步骤 2. 进行迭代,直到收敛
整体上来看,ICP 把点云配准问题拆分成了两个子问题:
- 找最近点
- 找最优变换
"""
ref: https://github.com/ClayFlannigan/icp/blob/master/icp.py
try this later: https://github.com/agnivsen/icp/blob/master/basicICP.py
"""
import numpy as np
from sklearn.neighbors import NearestNeighbors
def best_fit_transform(A, B):
'''
Calculates t