0. 简介

作为常用的配准方法，ICP和NDT两种匹配被广泛应用于激光雷达的点云配准方法中。我们知道IPC的匹配主要是描述了点到点的匹配方法，而无法胜任点到面以及面到面的匹配，而本博客主要就是将向读者分析《Generalized-ICP》这篇论文，GICP可以通过点到点的距离作为损失函数求解point-to-point的损失函数，点到局部目标点局部拟合的平面距离作为point-to-plane的损失函数，而文中主要提到的plane-to-plane损失函数则是假设点云具有平面特征，这意味着在3D空间处理采样2D流形。

1. GICP统一模型

GICP引入了概率信息（使用协方差阵），提出了ICP的统一模型。本文方法的核心思想是如何从概率的角度去看待和推导出ICP算法的目标函数。这里我们直接看原文就好，原文提到： 假设有两个匹配好的点集， $A = \{a_i\}_{i = 1 , 2... N} , B = \{ b_i \}_{i = 1 , 2... N}$ , 且 $a_i$ 和 $b_i$ 是对应点（A为source，B为target）

再假设两个点云中的每个点，都是服从高斯分布的，其原因是由于测量等环节的误差，每个点的位置的测量值实际上是和真值 $\hat{a_i},\hat{b_i}$ 存在偏差 $a_i\sim \mathcal{N}(\hat{a_i},C_i^{A})\\ b_i\sim \mathcal{N}(\hat{b_i},C_i^{B})$

对于 $\hat{a_i},\hat{b_i}$ 有：

$\hat{b}_i=T^*\hat{a}$

$T^*$ （注意有上标 $^*$ ）是理想中的correct rigid transform。代表了两个点云真实的转换关系，我们需要一个目标函数来寻找出最佳的 $T^*$ ，以下是目标函数的推导过程：

首先定义残差 $d_i^{(T)}=b_i-Ta_i$ ， $d_i^{(T)}$ 代表了对原始点云使用 $T$ 做转换后，第 $i$ 个点对的有向距离。

它是由分布采样而来

$\begin{aligned}d_i^{(T)} &\sim \mathcal{N}(\hat{b_i},C_i^{B}) - T \mathcal{N}(\hat{a_i},C_i^{A}) \\ &= \mathcal{N}(\hat{b_i}-T\hat{a_i},C_i^{B}+(T)C_i^{A}(T)^T)\end{aligned}$

其中的等号变换可以参考这篇文章。

因为 $a_i,b_i$ 都被我们假设为独立的、服从高斯分布的随机变量，所以将上式中的 $T$ 替换为 $T^*$ ，则可以变为：

$\begin{aligned}d_i^{T*} &\sim \mathcal{N}(\hat{b_i},C_i^{B}) - T^* \mathcal{N}(\hat{a_i},C_i^{A}) \\& = \mathcal{N}(\hat{b_i}-T^*\hat{a_i},C_i^{B}+(T^*)C_i^{A}(T^*)^T)\\ &= \mathcal{N}(0,C_i^{B}+(T^*)C_i^{A}(T^*)^T)\end{aligned}$

接下来就是这篇文章的重点， $T$ 可以被看作 $d_i^T$ 的概率分布中待估计的分布参数，借助最大似然估计(MLE)的思想，我们寻找一个是的当前样本 $d_i$ 出现概率最大的 $T$ ：
$\begin{aligned}T&=\mathop{\arg\max}\limits_{}\bold{T}\prod_ip(d_i^{T})\\&= \mathop{\arg\max}\limits_\bold{T} \sum\limits_{i}\log (p(d_i^{(\bold{T})}))\end{aligned}$

这一部分是执行了取log的操作，然后进一步化简

$= \mathop{\arg\max}limits_\bold{T} \sum\limits_i\log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\ -\frac{1}{2}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i}))^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i}))$

上面的式子是参考了Multivariate normal distribution的取对数以及代入的方法。 对于多元常态分布 $\textbf{X} \sim \mathcal{N}(\mu, \Sigma)$ ，其概率密度函数可以表示为 $f_x(x_1, ..., x_k) = \frac{1}{\sqrt{(2\pi)^k|\Sigma|}}e^{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)}, |\Sigma| \triangleq \textbf{det} \Sigma$ 对上面的式子取log可以得到： $\begin{aligned} \log (f_x(x_1, ..., x_k)) &= \log (\frac{1}{\sqrt{(2\pi)^k|\Sigma|}}e^{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)}) \\&= \log (\frac{1}{\sqrt{(2\pi)^k|\Sigma|}}) + \log(e^{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)}) \\&= \log (\frac{1}{\\sqrt{(2\\pi)^k|\Sigma|}}) -\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu) \end{aligned}$ 代入 $d_i^{(\bold{T})} \sim \mathcal{N}(\hat{b_i} - \bold{T}\hat{a_i}, C_i^B+\bold{T}C_i^A\bold{T}^T)$ ，得到： $\begin{aligned}\log (p(d_i^{(\bold{T})})) &= \log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\&-\frac{1}{2}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a_i}))^T(C\_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}(d_i^{(\bold{T})}-(\hat{b_i} - \bold{T}\hat{a\_i})) \\&= \log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\&-\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}\end{aligned}$ 这样也就得到了我们上面的输出结果。这里的结果如果发现正态分布的协方差矩阵的行列式为常数时，则只需要优化最后一项就可以了。最后一项的二次型又被称作马哈拉诺比斯距离（马氏距离），极大似然估计等价于最小化样本点与均值之间的马氏距离。更详细的内容可以参考 高翔《视觉SLAM14讲》6.1 状态估计问题 。

这一部分则是对上一步的进一步化简，在 $\bold{T}=\bold{T}^*$ 的情況下 $\hat{b_i} - (\bold{T}^*)\hat{a_i} =0$

$= \mathop{\arg\max}\limits_\bold{T} \sum\limits_i\log (\frac{1}{\sqrt{(2\pi)^k|C_i^B+\bold{T}C_i^A\bold{T}^T|}}) \\ -\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}$

然后又因为三维刚体变换矩阵中的旋转矩阵行列式值为1，平移矩阵行列式值也为1。又因为 $T$ 是旋转平移矩阵，可以拆成旋转矩阵和平移矩阵的乘积。且 $\textbf{det}(AB) = \textbf{det}(A)\textbf{det}(B)$ ，所以有矩阵的行列式值 $\textbf{det}(\bold{T}) = 1$ ，因此 $\textbf{det}(\bold{T}C_i^A\bold{T}^T)=\textbf{det}(C_i^A)$

$=\mathop{\arg\max}\limits_\bold{T}\sum\limits_i-\frac{1}{2}{d_i^{(\bold{T})}}^T(C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}$

照视觉十四讲所说，这里对 $T$ 做优化。其中第一项为常数，则可以忽略，其中 $\textbf{det}(A+B)$ 可以参考这个推导。

然后舍去负号，则可以将上式化简为论文中的公式2：
$T=\mathop{\arg\min}\limits_\bold{T}\sum_id_i^{(\bold{T})^{T}} (C_i^B+\bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})}$

到此为止我们学习了GICP中最主要的公式推导公式了。

2. ICP应用

这里我们直接参照keineahnung2345的文章。文中介绍了三种ICP的推导，这一节要借助上文的结论。

2.1 point-to-point

传统的点到点ICP可以用GICP的框架表示如下 $C_i^B=I, C_i^A=0$ 验证： $\begin{aligned}\bold{T} &= \mathop{\arg\min}\limits_\bold{T} \sum\limits {d_i^{(\bold{T})}}^T (C_i^B + \bold{T}C_i^A\bold{T}^T)^{-1}d_i^{(\bold{T})} \\ &= \mathop{\arg\min}\limits_\bold{T} \sum\limits {d_i^{(\bold{T})}}^T d_i^{(\bold{T})} \\ &= \mathop{\arg\min}\limits_\bold{T} \sum\limits {\|d_i^{(\bold{T})}\|^2}\end{aligned}$ 可以看出其目标为最小化点对间距离的平方和，也就是点到点ICP更新公式

2.2 point-to-plane

首先定义一个为正交的投影矩阵 $\bold{P_i}$ ，有以下性质 $\bold{P_i} = \bold{P_i}^2 = \bold{P_i}$ 。 其中 $\bold{P_i}$ 会将向量投影到目标点云 $a$ 中的第 $i$ 点 $b_i$ 法向量的局部平面上，因此 $\bold{P_i}\cdot d_i^{(\bold{T})}$ 也就是转换后的 $a_i$ 到 $b_i$ 所在平面的距离。 验证： $\begin{aligned}\bold{T} &=\mathop{\arg\min}\limits_\bold{T} \{\sum\limits_i \|\bold{P_i} \cdot d_i^{(\bold{T})}\|^2\} \\&= \mathop{\arg\min}\limits_\bold{T} \{\sum\limits_i (\bold{P_i} \cdot d_i^{(\bold{T})})^T(\bold{P_i} \cdot d_i^{(\bold{T})})\} \\&= \mathop{\arg\min}\limits_\bold{T} \{\sum\limits_i{d_i^{(\bold{T})}}^T \cdot \bold{P_i}^2 \cdot d_i^{(\bold{T})}\} \\&= \mathop{\arg\min}\limits_\bold{T} \{\sum\limits_i{d_i^{(\bold{T})}}^T \cdot \bold{P_i} \cdot d_i^{(\bold{T})}\}\end{aligned}$

和GICP比较我们就可以发现关系为

$C_i^B=\bold{P_i}^{-1}, C_i^A=0$

2.3 plane-to-plane

这里是GICP专门提出的一种方法，即相对于点到点和点到面加入概率模型（协方差阵）

平面到平面算法的做法是，假设点云具有平面特征，这意味着在3D空间处理采样2D流形。 由于现实世界的曲面至少是分段可微的，我们可以假设我们的数据集是局部平面的。此外，由于我们从两个不同的角度对流形进行采样，因此通常不会对完全相同的点进行采样（即，对应关系永远不会是精确的）。 从而导致采样点在局部拟合的平面方向上的不确定性较大，但是在法向量方向上不确定性较小。

为此，每个测量点仅提供沿其曲面法线的约束。为了对这种结构进行建模，我们考虑每个采样点沿其局部平面以高协方差分布，而在曲面法线方向（垂直于平面方向）以极低协方差分布（即点云法线方向不在局部平面上）。假设局部拟合平面上某一点的法向量 $e_1$ 是沿X轴的，则该点协方差矩阵变为：

$\left(\begin{array}{lll} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right)$

$\epsilon$ 为沿着法线方向极小的常数。

因为实际上法向量并不一定是沿 $x$ 轴方向，所以需要进行坐标转换。假设 $b_i,a_i$ 对应的法向量分别为 $u_i,v_i$ ,则它们对应的协方差阵为： $\begin{array}{l} C_{i}^{B}=\mathbf{R}_{\mu_{i}} \cdot\left(\begin{array}{ccc} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) \cdot \mathbf{R}_{\mu_{i}}^{T} \\C_{i}^{A}=\mathbf{R}_{\nu_{i}} \cdot\left(\begin{array}{ccc} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) \cdot \mathbf{R}_{\nu_{i}}^{T} \end{array}$

$\mathbf{R}_{\nu_{i}}$ 为 $e_1$ 到 $v_i$ 旋转矩阵。
上述协方差计算过程可以表述如下图：
确定协方差阵后利用 $T$ 得到最大似然估计推导。 这里其实就是怎么确定协方差阵。
当然也可以通过PCA计算协方差阵（代替上述求解过程）：

$C=\frac{1}{N} \cdot \sum_{i=1}^{N} \cdot\left(p_{i}-\bar{p}\right) \cdot\left(p_{i}-\bar{p}\right)^{T}$

PCA求解时要注意公式（4）对协方差有个求逆过程，需要注意当协方差阵奇异时，用微小量替代0值（类似NDT中处理方式）。

3. PCL中GICP代码

#include <iostream>

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/registration/gicp.h>  

#include <pcl/visualization/pcl_visualizer.h>

#include <boost/thread/thread.hpp>

#include <pcl/console/time.h>   // 利用控制台计算时间

​

using namespace std;

​

int

main()

{

​

    pcl::console::TicToc time;

    // -----------------加载点云数据---------------------------

    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ>("30m1A.pcd", *target);

    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ>("30m2A.pcd", *source);

    cout << "读取源点云个数：" << source->points.size() << endl;

    cout << "读取目标点云个数：" << target->points.size() << endl;

    time.tic();

    //-----------------初始化GICP对象-------------------------

    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp; 

    //-----------------KD树加速搜索---------------------------

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);

    tree1->setInputCloud(source);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);

    tree2->setInputCloud(target);

    gicp.setSearchMethodSource(tree1);

    gicp.setSearchMethodTarget(tree2);

    //-----------------设置GICP相关参数-----------------------

    gicp.setInputSource(source);  //源点云

    gicp.setInputTarget(target);  //目标点云

    gicp.setMaxCorrespondenceDistance(100); //设置对应点对之间的最大距离

    gicp.setTransformationEpsilon(1e-10);   //为终止条件设置最小转换差异

     /* gicp.setSourceCovariances(source_covariances);

    gicp.setTargetCovariances(target_covariances);*/

    gicp.setEuclideanFitnessEpsilon(0.001);  //设置收敛条件是均方误差和小于阈值， 停止迭代

    gicp.setMaximumIterations(35); //最大迭代次数  

    //gicp.setUseReciprocalCorrespondences(true);//使用相互对应关系

    // 计算需要的刚体变换以便将输入的源点云匹配到目标点云

    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    gicp.align(*icp_cloud);

    //---------------输出必要信息到显示--------------------

    cout << "Applied " << 35 << " GICP iterations in " << time.toc()/1000 << " s" << endl;

    cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << endl;

    cout << "变换矩阵：\n" << gicp.getFinalTransformation() << endl;

    // 使用变换矩阵对为输入点云进行变换

    pcl::transformPointCloud(*source, *icp_cloud, gicp.getFinalTransformation());

    //pcl::io::savePCDFileASCII (".pcd", *icp_cloud);

   // -----------------点云可视化--------------------------

    boost::shared_ptr<pcl::visualization::PCLVisualizer>

        viewer_final(new pcl::visualization::PCLVisualizer("配准结果"));

    viewer_final->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色

    // 对目标点云着色可视化 (red).

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>

        target_color(target, 255, 0, 0);

    viewer_final->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");

    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,

        1, "target cloud");

    // 对源点云着色可视化 (green).

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>

        input_color(source, 0, 255, 0);

    viewer_final->addPointCloud<pcl::PointXYZ>(source, input_color, "input cloud");

    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,

        1, "input cloud");

    // 对转换后的源点云着色 (blue)可视化.

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>

        output_color(icp_cloud, 0, 0, 255);

    viewer_final->addPointCloud<pcl::PointXYZ>(icp_cloud, output_color, "output cloud");

    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,

        1, "output cloud");

​

    while (!viewer_final->wasStopped())

    {

        viewer_final->spinOnce(100);

        boost::this_thread::sleep(boost::posix_time::microseconds(100000));

    }

​

    return (0);

}

参考链接

https://blog.csdn.net/xinxiangwangzhi_/article/details/125236953

https://blog.csdn.net/pingjun5579/article/details/119029370

https://blog.csdn.net/keineahnung2345/article/details/122836492

https://www.freesion.com/article/24001018167/

https://tongtianta.site/paper/14239

https://www.cnblogs.com/mosquitoam/p/14849784.html