三维点云配准 ICP 算法的python实现（仅仅依赖numpy）

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