泛化的最近点迭代法（Generalized-ICP）

        Generalized-ICP算法是由斯坦福大学的Aleksandr V. Segal、Dirk Haehnel和Sebastian Thrun提出的，于2009年在Robotics science and system会议上发表。

        GICP是一种ICP算法的变体，其原理与ICP算法相同，之所以称为泛化的ICP算法是因为大多数ICP框架没有被修改，仍用kd树检索临近点以保持速度和简单性，GICP所提出的泛化只处理目标函数的迭代计算，对收敛函数进行优化，将协方差矩阵计算加入误差函数。

        GICP是一种ICP算法的变体，其原理与ICP算法相同，之所以称为泛化的ICP算法是因为大多数ICP框架没有被修改，仍用kd树检索临近点以保持速度和简单性，GICP所提出的泛化只处理目标函数的迭代计算，对收敛函数进行优化，将协方差矩阵计算加入误差函数。

其标准算法可以分为两步： （a）确定两组点云之间的点的对应关系； （b）计算一个使对应点之间的距离最小化的变换；

  ICP算法核心的核心部分在伪码的第五行：在点云A中查找R·bi + t的对应点，为了保持算法的简单性，选择欧式距离最小的点作为对应点；      第七行：寻找最佳的将R·bi + t变换到mi的变换函数。这里最佳变化函数通过使一个特定的损失函数最小化确定，这个损失函数的不同也是本节提到的三种算法的根本区别。

        ICP算法有很多变体，其中point-to-plane的变体利用表面法线信息提高了性能。在标准ICP算法中，通过迭代使得对应点的欧氏距离即∑▒||R·b_i+t−m_i||^2最小化，但point-to-plane变体中，是使源表面上的点沿目标表面的曲面法线投影到切平面子空间上的误差最小化，如图，源表面上的点s1沿目标表面上的点d1的法线方向l1投影到d1的切平面上，投影点到d1的距离即为所求误差。

这一变化是通过改变标准ICP算法Algorithm 1中的第5行完成的，其中表示mi的曲面法线：

 而GICP则是在这一步对目标函数的迭代估计上引入了概率模型，但是对应点的查找仍然使用欧式距离，而非概率度量，从而仍能使用kd树来查找最近点，保持了ICP相对于其他完全依赖概率模型的算法的主要优势——速度和简单性。     假设点云A={ai}和B={bi}（i=1,...,n）是已经完成对应点匹配的两个点云，即ai与bi是对应点。建立一种点云概率模型，假设存在一组潜在的点A ̂={(a_i) ̂}和B ̂={(b_i) ̂}符合高斯分布，且点ai和点bi是从和中选取，即：

ai和bi是点(a_i) ̂和(b_i) ̂的真实位置，{C_i^A}和{C_i^B}是已知的协方差矩阵。定义残差d_i^R,t=b_i− R·a_i−t，则有：

 对于真实位置的变换(R*，t*)则有：

 接下来通过最大似然估计法（Maximum Likelihood Estimation, MLE）对R*和t*进行估计，则有最大似然函数：

由于上式不是凸函数，不便求其最大值，所以将(R,t)放在协方差中再利用MLE进行估算，将优化问题简化为最小化损失函数：

 其中，M_i=(C_i^B+RC_i^AR^T)^−1     上式即是GICP算法的核心内容。容易发现当令{C_i^A}=0和{C_i^B}=1时，得到的正是标准ICP的算式，即标准ICP算法是泛化ICP算法的一种特殊情况。

在这种形式下也简化了梯度计算，用r_i表示残差，即r_i=b_i−R·a_i−t，梯度可以表示为：

   由于这是一个约束优化问题，R必须是一个旋转矩阵，所以不能使用共轭梯度下降来解决。引入欧拉参数对旋转矩阵R进行描述，欧拉参数化的优点是使用了三个独立的参数(θ_x,θ_y,θ_z)。

   由于这是一个约束优化问题，R必须是一个旋转矩阵，所以不能使用共轭梯度下降来解决。引入欧拉参数对旋转矩阵R进行描述，欧拉参数化的优点是使用了三个独立的参数(θ_x,θ_y,θ_z)。

则有R相对于θ的梯度：

通过链式规则，有：

GICP中使用的点云协方差矩阵与传统的点云协方差矩阵的定义不同，由于现实物体表面是分段可微的，因此GICP假设数据集是局部平面的，即点服从平面分布，每个采样点在其局部平面方向上分布的协方差很高，而在曲面法线方向上分布的协方差很低。则可以设计一个模型，使得点在法线方向具有较小的方差，设为，值为0.001；在平面方向具有较大的方差，设为1：

 这里的R_ni是一个旋转矩阵，将基底向量e1旋转到点的法线方向。实际计算中，这个旋转矩阵是通过计算该点附近的点的协方差矩阵，并进行奇异值分解（Singular Value Decomposition，SVD）得到的，即为Σ ̂=UDU^T中的矩阵U。