[笔记]ICP算法（迭代最近点）——LOAM系列代码前序

最近在学习LOAM一系列的算法，看到激光SLAM前端匹配主要是基于ICP算法变体，所以学习一下。下面是个人愚见，期待大佬指正。

ICP(迭代最近点）算法

ICP（Iterative Closest Point 迭代最近点）算法是一种点集对点集配准方法。如下图所示，红色线 和 蓝色线 是两个点集，该算法就是计算怎么把红色线的点集平移旋转，使 红色线 和 蓝色线 尽量重叠。

一、原理

1、数学描述

用数学语言描述如下，即ICP算法的实质是基于最小二乘法的最优匹配，它重复进行“确定对应关系的点集→计算最优刚体变换”的过程，直到某个表示正确匹配的收敛准则得到满足。

假设我们通过LiDAR(激光雷达）得到了第一组点云 X = { x 1 , x 2 , . . . , x n } \boldsymbol X\boldsymbol=\mathbf{\{x_{1,}x_2,...,x_n\}} X={x1,​x2​,...,xn​}，LiDAR经过位姿变换（旋转加平移）后又得到了第二组点云 P = { p 1 , p 2 , . . . , p n } \boldsymbol P\boldsymbol=\mathbf{\{p_{1,}p_2,...,p_n\}} P={p1,​p2​,...,pn​} 。注意这里的$\mathbf{X}$和$\mathbf{P}$的坐标分别对应移动前和移动后的坐标系（即坐标原点始终为LiDAR自身中心的右手坐标系，这里我们有移动前、移动后两个坐标系），并且我们通过相关算法筛选和调整了点云存储的顺序，使得$\mathbf{X}，\mathbf{P}$中的点一一对应，如$\left(x_{11},p_{11}\right)$在三维空间中对应同一个点。

现在我们要解决的问题是：计算LiDAR的旋转$\mathbf{R}$和平移$\mathbf{t}$，在没有误差的情况下，从 $\mathbf{X}$坐标系转换到$\mathbf{P}$的公式为

$${\mathbf{p}}_{\mathbf{i}}={\mathbf{R}}_{{\mathbf{x}}_{\mathbf{i}}}+\mathbf{t}$$

但由于噪声及错误匹配（如$\left(x_{11},p_{11}\right)$其实并不对应空间中同一点，但特征匹配算法错误地认为二者是同一点）的存在，上式不总是成立，所以我们要最小化的目标函数为$$E\left(R,t\right)=\frac{1}{\mathbf{n}}\sum _{i=1}^n{\Vert p_i-R_{x_i}-t\Vert }^2$$
常用的求解$\mathbf{R}$和$\mathbf{t}$的方法有：

1. SVD
2. 非线性优化

非线性优化的描述比较繁琐，这次先介绍SVD方法。

2、SVD求解$\mathbf{R}$、$\mathbf{t}$方法

首先计算两个点云$\mathbf{X}$和$\mathbf{P}$的质心（center of mass），分别为${\mu }_x$和${\mu }_p$
$${\mu }_x=\frac{1}{n}\sum _{i=1}^n{x}_i$$$${\mu }_p=\frac{1}{n}\sum _{i=1}^n{p}_i$$
并做如下处理：
$$\begin{gathered} \frac{1}{n}\sum _{i=1}^n{\Vert p_i-R_{x_i}-t\Vert }^2 \\ =\frac{1}{n}\sum _{i=1}^n{\Vert p_i-R_{x_i}-t-\left({\mu }_p-R{\mu }_x\right)+\left({\mu }_p-R{\mu }_x\right)\Vert }^2 \\ =\frac{1}{n}\sum _{i=1}^n{\Vert \left(p_i-{\mu }_p-R\left(x_i-{\mu }_x\right)\right)+\left({\mu }_p-R{\mu }_x-t\right)\Vert }^2 \\ =\frac{1}{n}\sum _{i=1}^n\left({\Vert p_i-{\mu }_p-R\left(x_i-{\mu }_x\right)\Vert }^2+{\Vert {\mu }_p-R{\mu }_x-t\Vert }^2+2{\left(p_i-{\mu }_p-R\left(x_i-{\mu }_x\right)\right)}^T\left({\mu }_p-R{\mu }_x-t\right)\right) \end{gathered}$$
然后最后一项处理一下
$$\begin{gathered} {\sum }_{i=1}^n{\left(p_i-{\mu }_p-R\left(x_i-{\mu }_x\right)\right)}^T\left({\mu }_p-R{\mu }_x-t\right) \\ ={\left({\mu }_p-R{\mu }_x-t\right)}^T{\sum }_{i=1}^n\left(p_i-{\mu }_p-R\left(x_i-{\mu }_x\right)\right) \\ ={\left({\mu }_p-R{\mu }_x-t\right)}^T\left(n{p}_i-n{\mu }_p-R\left(n{\mu }_x-n{\mu }_x\right)\right) \\ =0 \end{gathered}$$

再然后在两个点云中分别减去对应的质心（Subtract the corresponding center of mass from every point in the two point sets before calculating the transformation），设为$x_i^{\prime }=x_i-{\mu }_x$和$p_i^{\prime }=p_i-{\mu }_p$,目标函数则可以简化为$$\frac{1}{n}\sum _{i=1}^n\left({\Vert p_i^{\prime }-\mathbf{R}{x}_i^{\prime }\Vert }^2+{\Vert {\mu }_p-\mathbf{R}{\mu }_x-t\Vert }^2\right)$$

设${\mathbf{R}}^{\ast }$和${\mathbf{t}}^{\ast }$为最优解，可将最优化问题分为两步：

1. ${\mathbf{R}}^{\ast }=\underset{\mathbf{R}}{\mathrm{argmin}}\frac{1}{n}{\sum }_{i=1}^n{\Vert p_i^{\prime }-\mathbf{R}{x}_i^{\prime }\Vert }^2$
2. ${\mathbf{t}}^{\ast }={\mu }_p-\mathbf{R}{\mu }_x$

重点在于步骤1，将它展开得:
$$\begin{gathered} {\mathbf{R}}^{\ast }=\underset{\mathbf{R}}{\mathrm{argmin}}\frac{1}{n}\sum _{i=1}^n\left(p_i^{{}^{\prime }T}{p}_i^{\prime }+x_i^{{}^{\prime }T}{\mathbf{R}}^T\mathbf{R}{x}_i^{\prime }-2p_i^{{}^{\prime }T}\mathbf{R}{x}_i^{\prime }\right) \\ =\underset{\mathbf{R}}{\mathrm{argmin}}\frac{1}{n}\sum _{i=1}^n\left(p_i^{{}^{\prime }T}{p}_i^{\prime }+\mathbf{I}-2p_i^{{}^{\prime }T}\mathbf{R}{x}_i^{\prime }\right) \\ =\underset{\mathbf{R}}{\mathrm{argmin}}\frac{1}{n}\sum _{i=1}^n\left(-p_i^{{}^{\prime }T}\mathbf{R}{x}_i^{\prime }\right) \end{gathered}$$

令$\mathbf{W}={\sum }_{i=1}^n{p}_i^{\prime }{x}_i^{{}^{\prime }T}$,通过SVD分解：
$$\mathbf{W}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathbf{T}}$$
当$\mathbf{W}$满秩时有$$\mathbf{\Sigma }=\left[\begin{array}{ccc}{\sigma }_1& & \\ & {\sigma }_2& \\ & & {\sigma }_3\end{array}\right]$$
且对应唯一的$\mathbf{U},\mathbf{V}$组合，对应地 $${\mathbf{R}}^{\ast }=\mathbf{U}{\mathbf{V}}^{\mathbf{T}}$$$${\mathbf{t}}^{\ast }={\mu }_p-\mathbf{R}{\mu }_x$$就是我们要找的最优解（至于为什么${\mathbf{R}}^{\ast }$具有以上形式请参见此链接）

3、非线性优化方法求解$\mathbf{R}$、$\mathbf{t}$（TODO）

二、ICP算法分析

1、ICP算法优点

1. 可以获得非常精确的配准效果
2. 不必对处理的点集进行分割和特征提取
3. 在较好的初值情况下，可以得到很好的算法收敛性

2、ICP算法的不足之处

4. 在搜索对应点的过程中，计算量非常大，这是传统ICP算法的瓶颈
5. 标准ICP算法中寻找对应点时，认为欧氏距离最近的点就是对应点。这种假设有不合理之处，会产生一定数量的错误对应点。

3、ICP算法改进方面

针对标准ICP算法的不足之处，许多研究者提出ICP算法的各种改进版本，主要涉及如下所示的6个方面。
标准ICP算法中，选用点集中所有的点来计算对应点，通常用于配准的点集元素数量都是非常巨大的，通过这些点集来计算，所消耗的时间很长。在后来的研究中，提出了各种方法来选择配准元素，这些方法的主要目的都是为了减小点集元素的数目，即如何选用最少的点来表征原始点集的全部特征信息。在点集选取时可以：

1. 选取所有点；
2. 均匀采样（Uniform sub-sampling ）
3. 随机采样（Random sampling）
4. 按特征采样（Feature based Sampling ） 备注：LOAM系列主要采用此方法降低ICP运算复杂度
5. 法向空间均匀采样（Normal-space sampling）

按特征采样的最近邻点（LOAM用此方法）

基于特征的采样使用一些具有明显特征的点集来进行配准，大量减少了对应点的数目。

最近邻点（Closet Point）：

法方向最近邻点（Normal Shooting）：

投影法（Projection）：

三、例程

MATLAB2018例程

MATLAB R2018a提供了ICP例程，需要安装Computer Vision System Toolbox。
调用ICP函数

tform = pcregistericp(ptCloudTformed,ptCloud,'Extrapolate',true);

python例程

import numpy as np

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 = np.dot(BB.T, AA)
    U, s, VT = np.linalg.svd(W)
    R = np.dot(U, VT)

    # 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

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

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
        distances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T)

        # compute the transformation between the current source and nearest destination points
        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
        src = np.dot(T, src)

        # check error
        mean_error = np.sum(distances) / distances.size
        if abs(prev_error-mean_error) < tolerance:
            break
        prev_error = mean_error

    # calculcate final tranformation
    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)

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

上面代码创建一个源点集A（在0-100的整数范围内随机生成20个3维数据点），然后将A绕Z轴旋转45°并沿XYZ轴分别移动一小段距离，得到点集B。结果如下，可以看出ICP算法正确的计算出了变换矩阵。

需要注意几点：

1. 首先需要明确公式里的变换是T（X→P）， 即通过旋转和平移把点集P变换到X。我们这里求的变换是T（A→B），要搞清楚对应关系。
2. 本例只用了20个点进行测试，ICP算法在求最近邻点的过程中需要计算20×20次距离并比较大小。如果点的数目巨大，那算法的效率将会非常低。
3. 两个点集的初始相对位置对算法的收敛性有一定影响，最好在“足够近”的条件下进行ICP配准。

C++例程

计算点云中心点

//计算点云P的中心点mean  
void CalculateMeanPoint3D(vector<Point3D> &P, Point3D &mean)  
{  
    vector<Point3D>::iterator it;  
    mean.x = 0;  
    mean.y = 0;  
    mean.z = 0;  
    for(it=P.begin(); it!=P.end(); it++){  
        mean.x += it->x;  
        mean.y += it->y;  
        mean.z += it->z;  
    }  
    mean.x = mean.x/P.size();  
    mean.y = mean.y/P.size();  
    mean.z = mean.z/P.size();  
}  

利用控制点求初始旋转矩阵
在确定对应关系时，所使用的几何特征是空间中位置最近的点。这里，我们甚至不需要两个点集中的所有点。可以指用从某一点集中选取一部分点，一般称这些点为控制点（Control Points）。这时，配准问题转化为：
这里，pi，qi为最近匹配点。
在Geomagic Studio中利用三个点就可以进行两个模型的“手动注册”（感觉这里翻译的不好，Registration，应该为“手动匹配”）。
我们将手动选择的三个点导出，作为实验初始的控制点：
对于每一对矩阵Ai，计算矩阵B:

double B[16];  
        for(int i=0;i<16;i++)  
            B[i]=0;  
        for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){  
            double divpq[3]={itp->x,itp->y,itp->z};  
            double addpq[3]={itp->x,itp->y,itp->z};  
            double q[3]={itq->x,itq->y,itq->z};  
            MatrixDiv(divpq,q,3,1);  
            MatrixAdd(addpq,q,3,1);  
            double A[16];  
            for(int i=0;i<16;i++)  
                A[i]=0;  
            for(int i=0;i<3;i++){  
                A[i+1]=divpq[i];  
                A[i*4+4]=divpq[i];  
                A[i+13]=addpq[i];  
            }  
            double AT[16],AMul[16];  
            MatrixTran(A,AT,4,4);  
            MatrixMul(A,AT,AMul,4,4,4,4);  
            MatrixAdd(B,AMul,4,4);  
        }  
        //使用奇异值分解计算B的特征值和特征向量  
        double eigen, qr[4];  
        MatrixEigen(B, &eigen, qr, 4);  
        //计算n阶正定阵m的特征值分解：eigen为特征值，q为特征向量  
void MatrixEigen(double *m, double *eigen, double *q, int n)  
{  
    double *vec, *eig;  
    vec = new double[n*n];  
    eig = new double[n];  
    CvMat _m = cvMat(n, n, CV_64F, m);  
    CvMat _vec = cvMat(n, n, CV_64F, vec);  
    CvMat _eig = cvMat(n, 1, CV_64F, eig);  
    //使用OpenCV开源库中的矩阵操作求解矩阵特征值和特征向量  
    cvEigenVV(&_m, &_vec, &_eig);  
    *eigen = eig[0];  
    for(int i=0; i<n; i++)  
        q[i] = vec[i];  
    delete[] vec;  
    delete[] eig;  
}  
  
//计算旋转矩阵  
void CalculateRotation(double *q, double *R)  
{  
    R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];  
    R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);  
    R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);  
    R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);  
    R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];  
    R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);  
    R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);  
    R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);  
    R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];  
}  

平移矩阵计算

//通过特征向量计算旋转矩阵R1和平移矩阵T1  
        CalculateRotation(qr, R1);  
        double mean_Q[3]={_mean_Q.x,_mean_Q.y,_mean_Q.z};  
        double mean_P[3]={_mean_P.x,_mean_P.y,_mean_P.z};  
        double meanT[3]={0,0,0};  
        int nt=0;  
        for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){  
            double tmpP[3]={itp->x,itp->y,itp->z};  
            double tmpQ[3]={itq->x,itq->y,itq->z};  
            double tmpMul[3];  
            MatrixMul(R1, mean_P, tmpMul, 3, 3, 3, 1);  
            MatrixDiv(tmpQ,tmpMul,3,1);  
            MatrixAdd(meanT,tmpQ,3,1);  
            nt++;  
        }  
        for(int i=0; i<3; i++)  
            T1[i] = meanT[i]/(double)(nt);  

迭代最近点

//计算误差和d  
        d = 0.0;  
        if(round==1){  
            FindClosestPointSet(data,P,Q);  
        }  
        int pcount=0;  
        for(itp = P.begin(),itq=Q.begin();itp!=P.end(); itp++, itq++){  
            double sum = (itp->x - itq->x)*(itp->x - itq->x) + (itp->y - itq->y)*(itp->y - itq->y)   
                + (itp->z - itq->z)*(itp->z - itq->z);  
            d += sum;  
            pcount++;  
        }  
        d=d/(double)(pcount);  

参考资料：
[1]高翔，视觉SLAM十四讲7.9 3D-3D: ICP
[2]Wolfram Burgard, et al. Introduction to Mobile Robotics: Iterative Closest Point Algorithm
[3]ICP迭代最近点算法综述
[4]知乎答主：灯灯
[5]博主：XXX已失联