经典论文阅读之-GICP（ICP大一统）

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 = \{a_i\}_{i = 1 , 2... N} , B = \{ b_i \}_{i = 1 , 2... N} A={ai​}i=1,2...N​,B={bi​}i=1,2...N​ , 且 $a_i$和 $b_i$ 是对应点（A为source，B为target）

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

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

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

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

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

它是由分布采样而来

$$\begin{array}{rl}{d}_i^{(T)}& \sim \mathcal{N}(\widehat{b_i},C_i^B)-T\mathcal{N}(\widehat{a_i},C_i^A)\\ & =\mathcal{N}(\widehat{b_i}-T\widehat{a_i},C_i^B+(T)C_i^A(T{)}^T)\end{array}$$

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

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

$$\begin{array}{rl}{d}_i^{T\ast }& \sim \mathcal{N}(\widehat{b_i},C_i^B)-T^{\ast }\mathcal{N}(\widehat{a_i},C_i^A)\\ & =\mathcal{N}(\widehat{b_i}-T^{\ast }\widehat{a_i},C_i^B+(T^{\ast })C_i^A(T^{\ast }{)}^T)\\ & =\mathcal{N}(0,C_i^B+(T^{\ast })C_i^A(T^{\ast }{)}^T)\end{array}$$

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

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

$$\begin{gathered} =\mathrm{argmax}limit{s}_{\mathbf{T}}\sum _i\log (\frac{1}{\sqrt{(2\pi {)}^k|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|}}) \\ -\frac{1}{2}(d_i^{(\mathbf{T})}-(\widehat{b_i}-\mathbf{T}\widehat{a_i}){)}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}(d_i^{(\mathbf{T})}-(\widehat{b_i}-\mathbf{T}\widehat{a_i})) \end{gathered}$$

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

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

$$\begin{gathered} =\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i\log (\frac{1}{\sqrt{(2\pi {)}^k|C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T|}}) \\ -\frac{1}{2}{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})} \end{gathered}$$

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

$$=\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i-\frac{1}{2}{d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}$$

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

然后舍去负号，则可以将上式化简为论文中的公式2：
$$T=\underset{\mathbf{T}}{\mathrm{argmin}}\sum _i{d}_i^{(\mathbf{T}{)}^T}(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}$$

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

2. ICP应用

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

2.1 point-to-point

传统的点到点ICP可以用GICP的框架表示如下
$$C_i^B=I,C_i^A=0$$
验证：
$$\begin{array}{rl}\mathbf{T}& =\underset{\mathbf{T}}{\mathrm{argmin}}\sum {d_i^{(\mathbf{T})}}^T(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T{)}^{-1}{d}_i^{(\mathbf{T})}\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum {d_i^{(\mathbf{T})}}^T{d}_i^{(\mathbf{T})}\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum \Vert d_i^{(\mathbf{T})}{\Vert }^2\end{array}$$
可以看出其目标为最小化点对间距离的平方和，也就是点到点ICP更新公式

2.2 point-to-plane

首先定义一个为正交的投影矩阵${\mathbf{P}}_{\mathbf{i}}$，有以下性质${\mathbf{P}}_{\mathbf{i}}={{\mathbf{P}}_{\mathbf{i}}}^2={\mathbf{P}}_{\mathbf{i}}$。
其中${\mathbf{P}}_{\mathbf{i}}$会将向量投影到目标点云$a$中的第$i$点$b_i$法向量的局部平面上，因此${\mathbf{P}}_{\mathbf{i}}\cdot d_i^{(\mathbf{T})}$也就是转换后的$a_i$到$b_i$所在平面的距离。
验证：
T = arg ⁡ min ⁡ T { ∑ i ∥ P i ⋅ d i ( T ) ∥ 2 } = arg ⁡ min ⁡ T { ∑ i ( P i ⋅ d i ( T ) ) T ( P i ⋅ d i ( T ) ) } = arg ⁡ min ⁡ T { ∑ i d i ( T ) T ⋅ P i 2 ⋅ d i ( T ) } = arg ⁡ min ⁡ T { ∑ i d i ( T ) T ⋅ P i ⋅ d i ( T ) } \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} T​=Targmin​{i∑​∥Pi​⋅di(T)​∥2}=Targmin​{i∑​(Pi​⋅di(T)​)T(Pi​⋅di(T)​)}=Targmin​{i∑​di(T)​T⋅Pi​2⋅di(T)​}=Targmin​{i∑​di(T)​T⋅Pi​⋅di(T)​}​

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

$$C_i^B={{\mathbf{P}}_{\mathbf{i}}}^{-1},C_i^A=0$$

2.3 plane-to-plane

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

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

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

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

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

因为实际上法向量并不一定是沿$x$轴方向，所以需要进行坐标转换。假设 $b_i,a_i$对应的法向量分别为$u_i,v_i$,则它们对应的协方差阵为：
$$\begin{array}{l}{C}_i^B={\mathbf{R}}_{{\mu }_i}\cdot \left(\begin{array}{ccc}ϵ& 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}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\cdot {\mathbf{R}}_{{\nu }_i}^T\end{array}$$

…详情请参照古月居