一.点云配准介绍

1.点云配准的概念

         图像配准是图像处理研究领域中的一个典型问题和技术难点，其目的在于比较或融合针对同一对象在不同条件下获取的图像，例如图像会来自不同的采集设备，取自不同的时间，不同的拍摄视角等等，有时也需要用到针对不同对象的图像配准问题。具体地说，对于一组图像数据集中的两幅图像，通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。 一个经典的应用是场景的重建，比如说一张茶几上摆了很多杯具，用深度摄像机进行场景的扫描，通常不可能通过一次采集就将场景中的物体全部扫描完成，只能是获取场景不同角度的点云，然后将这些点云融合在一起，获得一个完整的场景。

        简单点说，点云配准（Point Cloud Registration）指的是输入两幅点云 (source) 和 (target) ，输出一个变换使得和的重合程度尽可能高。或者说，对于两个不同视角下的坐标系，比如世界坐标系和相机坐标系，我们需要求出一个变换使得两个坐标系变换到统一视角下。我们这里只考虑刚性变换，即变换只包括旋转、平移。

2.点云配准的过程

        目前，传统的主流点云配准技术主要包括粗配准和精配准两个阶段。粗配准阶段的目的是，对于任意初始状态的两片点云，使得两片点云大致对齐，给旋转矩阵R和平移向量T提供初值。而精配准是在粗配准的基础上，进行更精确、更细化的配准。总而言之，点云配准的过程就是矩阵变换的过程。

二.点云粗配准算法

        点云粗配准又被称为点云初始配准，旨在对任意初始位置的两片点云进行粗略的配准，使其大致对齐，从而为点云的精配准提供良好的初始位置。点云粗配准算法主要分为两大类，分别是基于全局搜索思想的配准方法和基于几何特征描述的配准方法。

1.基于全局搜索思想的配准方法

         基于全局搜索思想的配准方法通常从源数据中随机地选择几个点（通常是三个），并根据对目标数据的穷举搜索从目标数据中找到对应的点，计算所有可能的变换矩阵，通过投票的方式或者选取误差函数最小的方式确定最优变换。这种通过考虑所有可能的对应关系，可以得到较好的配准效果，但往往会产生很大的计算负荷。其中最常用的算法框架是RANSAC（随机采样一致性）算法。

1.1 RANSAC点云配准算法

        RANSAC 算法最早是在数学/统计学领域提出，它是一种利用随机采集的样本来准确拟合出整体数学模型参数的方法。它的主要思想是从给定的样本集中随机选取一些样本并估计一个数学模型，将样本中的其余样本带入该数学模型中验证，如果有足够多的样本误差在给定范围内，则该数学模型最优，否则继续循环该步骤。
        后来，RANSAC算法被引入三维点云配准领域，产生了基于RANSAC思想的RANSAC点云配准算法，其算法过程主要如下：

         其本质就是不断的对源点云进行随机样本采样并求出对应的变换模型，接着对每一次随机变换模型进行测试，并不断循环该过程直到选出最优的变换模型作为最终结果。根据大数定律，可知道在进行大量重复采样实验的情况之下，随机模拟可以近似地将正确结果求解出来。 当然，RANSAC配准算法也存在着有限次随机性带来的不稳定配准和计算量大等弊端。

1.2 4PCS算法

        4PCS算法全称为 4-Points Congruent Sets 即 4点全等集配准算法。该算法也是基于RANSAC算法框架，对两片点云的初始姿态不做约束，针对搜索对应点的策略进行了优化，将基本的三组对应点扩展到了四组具有一定约束性的对应点集，大大增加了算法的鲁棒性，提高了算法的搜索效率，算法的时间复杂度约为。该算法的核心思想就是利用刚体变换中的几何不变性（向量/线段比例、点间欧几里得距离），根据刚性变换后交点所占线段比例不变以及点之间的欧几里得距离不变的特性，在目标点云中尽可能寻找４个近似共面点（近似全等四点集）与之对应，从而利用最小二乘法计算得到变换矩阵，基于RANSAC算法框架迭代选取多组基，根据最大公共点集（LCP）的评价准则进行比较得到最优变换。

（1）全等四点集

        在4PCS算法中，我们将局部配准点云由三个点扩展为四个点，并且这四个点具有一定的附加约束，如果能够在目标点云中也找到相应的近似满足约束的四点集，我们就将这两个对应四点集称为全等四点集，用于求解点云变换。相比传统的RANSAC配准算法中完全随机采样的方式，通过全等四点集的应用，一方面算法减少了计算量，提高了效率，使得全局搜索更有目标性；另一方面算法使用带有约束的局部四点配准，准确性和鲁棒性更高。四点集的选择和约束标准如下：

• 首先在源点云中随机选择三个点，要求这三点所构成的三角形面积尽量的大（三点确定一个平面，向量叉积可以计算面积），但是三点间的距离不能超过一定的阈值上限，该上限由两片点云的给定重叠率 f 确定。因为三点之间距离越大，配准的鲁棒性越高；但距离过大，三点均在两点云的重叠区域之外了，配准效果又不好。因此需要在满足上限的基础之上，尽可能保证大的三级形面积。若没有给定点云重叠率f，则可以进行f=1.0,0.75,0.5...重叠率递减测试，选择最优变换。
• 确定三点后，源点云四点集中第四点的选择方式为：遍历源点云中所有的点，对每一个点进行计算验证选择最优的第四点。第四点需要与其他三点组成的平面尽可能的共面（即不强制要求四点共面，但第四点到其他三点平面的距离尽可能小），并且第四点与其他三点的距离也要满足距离阈值范围（不能太近不能太远）。
• 源点云中的四点集选择完成后，就可以计算其四点构成的两线段交点的变换不变比，根据不变比在目标点云中遍历搜索对应的满足该约束所有四点集用于配准计算，这就是（近似）全等四点集。

（2）4PCS算法流程

         在了解了全等四点集这一核心概念之后，我们来介绍一下基于全等四点集寻找对应点的4PCS的算法步骤如下：

• STEP1：在源点云P中，使用上述的四点集的选择方法随机选择一个四点集B={b1,b2,b3,b4}，其中(b1,b2)确定线段1，(b3,b4)确定线段2。接着去计算不变量d1=|b1-b2|，d2=|b3-b4|，不变比r1=|b1-e|/|b1-b2|，r2=|b3-e|/|b3-b4|。注意因为四点不一定共面，两条线段也不一定相交，所以可以使用连接两个线段的最近点的中心点作为“交点”。
• STEP2：在目标点云Q中，遍历所有的点对，筛选满足第一个约束（线段长度在d1或d2误差范围内）的点对集合R1，R2。其表示为：

• STEP3：遍历点对集合R1中的所有点对元素，计算其线段上满足不变比r1的目标交点，然后将所有计算结果e存入搜索树ANN Tree（近似最邻近搜索树，最常见的是K-D Tree算法），并构建相应的映射
• STEP4：遍历点对集合R2中的所有点对元素，计算其线段上满足不变比r2的目标交点，并构建相应的映射。然后遍历所有的点，在之前构建的ANN Tree中搜索可接受误差范围内的重合点，若可找到则说明能在Q中找到一个对应的近似全等四点集。最终求得所有的近似全等四点集
• STEP5：遍历所有的近似全等四点集，对每一个，通过最小二乘法计算其与B的对应变换矩阵。然后使用该变换矩阵对源点云P进行变换得到P'，统计P'与Q中的最大公共点集（LCP），记max(LCP)的变换矩阵为本次迭代的最优变换矩阵T并保留。
• STEP6：不断迭代这个过程，记录最优的变换。迭代结束后得到的变换矩阵即为最优变换矩阵。原论文算法框架如下：

注意：该算法的实现过程中还使用了一些增强方法。比如在上述不变量的基础上，添加了对应点的法向量计算，只有满足线段长度近似相等且端点法向量夹角也近似相等的前提下，才认为是一对对应线段/向量，进一步加强搜索条件，减少了搜索数量。

（3）原论文及代码地址

• 论文地址：4-points Congruent Sets for Robust Surface Registration (stanford.edu)
• 代码地址：http://vecg.cs.ucl.ac.uk/Projects/SmartGeometry/fpcs/paper_docs/4pcs_1.3.tar.gz

2.基于几何特征描述的配准方法

待补。

2.1 FPFH算法

待补。

三.点云精配准算法

        经过粗配准之后，两片点云的重叠部分已经可以大致对齐，但精度还远远达不到我们的要求。精配准算法就是在粗配准的基础上再进行进一步的配准，提升配准的精度。目前精配准算法中主流的是ICP（Iterative Closest Point，迭代最近点）算法。

1.ICP算法

        ICP算法要求待配准的两片点云具有较好的初始位置（初始变换R和T），即要求两片点云大致对齐。其基本思想是选取两片点云中距离最近的点作为对应点，通过所有对应点对求解旋转和平移变换矩阵，并通过不断迭代的方式使两片点云之间的误差越来越小，直至满足我们提前设定的阈值要求或迭代次数。 ICP算法的算法框架如下：

         可以看到，整个ICP算法迭代可以拆解为两个核心子问题，即寻找匹配最近点和计算最优变换。接下来我们将对这两个核心问题分别进行说明。

1.1 ICP算法核心说明

（1）寻找匹配最近点

        利用初始变换  、或上一次迭代得到的变换，对初始点云P进行变换，得到一个临时的变换点云P'。然后用这个点云P'和目标点云Q进行匹配，找出源点云中每一个点在目标点云中的最近邻点作为对应点，为接下来的计算最优变换做准备。常见的最邻近点匹配方法有：

• 暴力循环法：对两点云分别进行双重循环，遍历匹配每一个点对，计算其欧氏距离，选择距离最近的作为该点的对应点并保存。该方法的复杂度为
• ANN(Approximate Nearest Neighbor) 搜索：使用近似最近邻搜索算法，将点插入搜索树结构上，最常见的搜索树结构算法是K-D Tree，加速搜索匹配过程，该方法的复杂度为。我们主要使用该KD Tree算法进行查找加速。

（2）计算最优变换

        在找到最近匹配对应点之后，我们需要计算使得两片对应点云配准的最优变换参数R和T。假设分别表示源点云和目标点云，则我们的目标优化函数为最小化变换后对应点之间的距离，即 ：

         在这里我们将使用SVD奇异值分解来计算最优变换参数，下面给出最终计算公式结果。关于该定理的证明可以参考我之前的博客或文章： https://zhuanlan.zhihu.com/p/104735380

1.2 ICP算法代码实现

import numpy as np
#计算最优变换参数R、T（SVD奇异值分解）
def best_fit_transform(A, B):
    '''
    Calculates the least-squares best-fit transform between corresponding 3D points A->B
    Input:
      A: Nx3 numpy array of corresponding 3D points
      B: Nx3 numpy array of corresponding 3D points
    Returns:
      T: 4x4 homogeneous transformation matrix 齐次坐标
      R: 3x3 rotation matrix
      t: 3x1 column vector
    '''

    assert len(A) == len(B)

    # translate points to their centroids
    # 求点云质心，并变换坐标，消除平移的影响
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)
    AA = A - centroid_A
    BB = B - centroid_B

    # rotation matrix
    # 变换后的坐标对应点相乘得到W
    W = np.dot(BB.T, AA)
    U, s, VT = np.linalg.svd(W) #对W进行SVD分解，得到矩阵
    R = np.dot(U, VT) #最优旋转矩阵=UV^T

    # special reflection case
    # 反射特殊情况考虑
    if np.linalg.det(R) < 0:
       VT[2,:] *= -1
       R = np.dot(U, VT)


    # translation
    # 最优平移
    t = centroid_B.T - np.dot(R,centroid_A.T)

    # homogeneous transformation
    # 构造齐次坐标
    T = np.identity(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = t

    return T, R, t

#寻找最近匹配点（暴力二重循环法），可以使用NearestNeighbors替换搜索
def nearest_neighbor(src, dst):
    '''
    Find the nearest (Euclidean) neighbor in dst for each point in src
    Input:
        src: Nx3 array of points
        dst: Nx3 array of points
    Output:
        distances: Euclidean distances (errors) of the nearest neighbor
        indecies: dst indecies of the nearest neighbor
    '''

    indecies = np.zeros(src.shape[0], dtype=np.int)
    distances = np.zeros(src.shape[0])
    for i, s in enumerate(src):
        min_dist = np.inf
        for j, d in enumerate(dst):
            dist = np.linalg.norm(s-d)
            if dist < min_dist:
                min_dist = dist
                indecies[i] = j
                distances[i] = dist    
    return distances, indecies

#ICP算法迭代
def icp(A, B, init_pose=None, max_iterations=50, tolerance=0.001):
    '''
    The Iterative Closest Point method
    Input:
        A: Nx3 numpy array of source 3D points 原点云
        B: Nx3 numpy array of destination 3D point 目标点云
        init_pose: 4x4 homogeneous transformation 初始变换参数
        max_iterations: exit algorithm after max_iterations 最大迭代次数
        tolerance: convergence criteria 收敛误差
    Output:
        T: final homogeneous transformation 齐次坐标变换矩阵
        distances: Euclidean distances (errors) of the nearest neighbor 误差
    '''

    # make points homogeneous, copy them so as to maintain the originals
    src = np.ones((4,A.shape[0]))
    dst = np.ones((4,B.shape[0]))
    src[0:3,:] = np.copy(A.T)
    dst[0:3,:] = np.copy(B.T)

    # apply the initial pose estimation
    # 点云初始变换
    if init_pose is not None:
        src = np.dot(init_pose, src)

    prev_error = 0

    for i in range(max_iterations):
        # find the nearest neighbours between the current source and destination points
        #1.找最近匹配点对应
        distances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T)
        
       
        # compute the transformation between the current source and nearest destination points
        #2.计算最优变换参数（SVD）
        T,_,_ = best_fit_transform(src[0:3,:].T, dst[0:3,indices].T)

        # update the current source
    # refer to "Introduction to Robotics" Chapter2 P28. Spatial description and transformations
    	#3.更新变换点云
        src = np.dot(T, src)

        # check error
        #4.检查误差是否收敛 MSELoss
        mean_error = np.sum(distances) / distances.size
        if abs(prev_error-mean_error) < tolerance:
            break
        prev_error = mean_error

    # calculcate final tranformation
    #5.迭代结束或误差收敛后，计算最终的变换参数（初始A->当前src，因为变换T没有迭代累计）
    T,_,_ = best_fit_transform(A, src[0:3,:].T)

    return T, distances
    
if __name__ == "__main__":
    A = np.random.randint(0,101,(20,3))    # 20 points for test 随机源点云
    
    rotz = lambda theta: np.array([[np.cos(theta),-np.sin(theta),0],
                                       [np.sin(theta),np.cos(theta),0],
                                       [0,0,1]])
    trans = np.array([2.12,-0.2,1.3])
    B = A.dot(rotz(np.pi/4).T) + trans #随即扰动->目标点云
    
    T, distances = icp(A, B) #ICP算法计算得到最优参数

    np.set_printoptions(precision=3,suppress=True)
    print T