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

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 a_i ai​和 b i b_i bi​ 是对应点（A为source，B为target）

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

对于 a i ^ , b i ^ \hat{a_i},\hat{b_i} ai​^​,bi​^​有：

b ^ i = T ∗ a ^ \hat{b}_i=T^*\hat{a} b^i​=T∗a^

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

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

它是由分布采样而来

d i ( T ) ∼ N ( b i ^ , C i B ) − T N ( a i ^ , C i A ) = N ( b i ^ − T a i ^ , C i B + ( T ) C i A ( 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)\end{aligned} di(T)​​∼N(bi​^​,CiB​)−TN(ai​^​,CiA​)=N(bi​^​−Tai​^​,CiB​+(T)CiA​(T)T)​

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

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

d i T ∗ ∼ N ( b i ^ , C i B ) − T ∗ N ( a i ^ , C i A ) = N ( b i ^ − T ∗ a i ^ , C i B + ( T ∗ ) C i A ( T ∗ ) T ) = N ( 0 , C i B + ( T ∗ ) C i A ( 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} diT∗​​∼N(bi​^​,CiB​)−T∗N(ai​^​,CiA​)=N(bi​^​−T∗ai​^​,CiB​+(T∗)CiA​(T∗)T)=N(0,CiB​+(T∗)CiA​(T∗)T)​

接下来就是这篇文章的重点， T T T可以被看作 d i T d_i^T diT​的概率分布中待估计的分布参数，借助最大似然估计(MLE)的思想，我们寻找一个是的当前样本 d i d_i di​出现概率最大的 T T T： T = arg ⁡ max ⁡ T ∏ i p ( d i T ) = arg ⁡ max ⁡ T ∑ i log ⁡ ( p ( 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} T​=argmax​Ti∏​p(diT​)=Targmax​i∑​log(p(di(T)​))​

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

= arg ⁡ max ⁡ l i m i t s T ∑ i log ⁡ ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 ( d i ( T ) − ( b i ^ − T a i ^ ) ) T ( C i B + T C i A T T ) − 1 ( d i ( T ) − ( b i ^ − T a i ^ ) ) = \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})) =argmaxlimitsT​i∑​log((2π)k∣CiB​+TCiA​TT∣ ​1​)−21​(di(T)​−(bi​^​−Tai​^​))T(CiB​+TCiA​TT)−1(di(T)​−(bi​^​−Tai​^​))

上面的式子是参考了Multivariate normal distribution的取对数以及代入的方法。 对于多元常态分布 X ∼ N ( μ , Σ ) \textbf{X} \sim \mathcal{N}(\mu, \Sigma) X∼N(μ,Σ)，其概率密度函数可以表示为 f x ( x 1 , . . . , x k ) = 1 ( 2 π ) k ∣ Σ ∣ e − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) , ∣ Σ ∣ ≜ det Σ 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 fx​(x1​,...,xk​)=(2π)k∣Σ∣ ​1​e−21​(x−μ)TΣ−1(x−μ),∣Σ∣≜detΣ 对上面的式子取log可以得到： KaTeX parse error: \cr valid only within a tabular/array environment 代入 d i ( T ) ∼ N ( b i ^ − T a i ^ , C i B + T C i A T T ) 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) di(T)​∼N(bi​^​−Tai​^​,CiB​+TCiA​TT)，得到： log ⁡ ( p ( d i ( T ) ) ) = log ⁡ ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 ( d i ( T ) − ( b i ^ − T a i ^ ) ) T ( C _ i B + T C i A T T ) − 1 ( d i ( T ) − ( b i ^ − T a _ i ^ ) ) = log ⁡ ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( 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} log(p(di(T)​))​=log((2π)k∣CiB​+TCiA​TT∣ ​1​)−21​(di(T)​−(bi​^​−Tai​^​))T(C_iB+TCiA​TT)−1(di(T)​−(bi​^​−Ta_i^​))=log((2π)k∣CiB​+TCiA​TT∣ ​1​)−21​di(T)​T(CiB​+TCiA​TT)−1di(T)​​ 这样也就得到了我们上面的输出结果。这里的结果如果发现正态分布的协方差矩阵的行列式为常数时，则只需要优化最后一项就可以了。最后一项的二次型又被称作马哈拉诺比斯距离（马氏距离），极大似然估计等价于最小化样本点与均值之间的马氏距离。更详细的内容可以参考 高翔《视觉SLAM14讲》6.1 状态估计问题 。

这一部分则是对上一步的进一步化简，在 T = T ∗ \bold{T}=\bold{T}^* T=T∗的情況下 b i ^ − ( T ∗ ) a i ^ = 0 \hat{b_i} - (\bold{T}^*)\hat{a_i} =0 bi​^​−(T∗)ai​^​=0

= arg ⁡ max ⁡ T ∑ i log ⁡ ( 1 ( 2 π ) k ∣ C i B + T C i A T T ∣ ) − 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) = \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})} =Targmax​i∑​log((2π)k∣CiB​+TCiA​TT∣ ​1​)−21​di(T)​T(CiB​+TCiA​TT)−1di(T)​

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

= arg ⁡ max ⁡ T ∑ i − 1 2 d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) =\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})} =Targmax​i∑​−21​di(T)​T(CiB​+TCiA​TT)−1di(T)​

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

然后舍去负号，则可以将上式化简为论文中的公式2： T = arg ⁡ min ⁡ T ∑ i d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) 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})} T=Targmin​i∑​di(T)T​(CiB​+TCiA​TT)−1di(T)​

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

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

传统的点到点ICP可以用GICP的框架表示如下 C i B = I , C i A = 0 C_i^B=I, C_i^A=0 CiB​=I,CiA​=0 验证： T = arg ⁡ min ⁡ T ∑ d i ( T ) T ( C i B + T C i A T T ) − 1 d i ( T ) = arg ⁡ min ⁡ T ∑ d i ( T ) T d i ( T ) = arg ⁡ min ⁡ T ∑ ∥ d i ( T ) ∥ 2 \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} T​=Targmin​∑di(T)​T(CiB​+TCiA​TT)−1di(T)​=Targmin​∑di(T)​Tdi(T)​=Targmin​∑∥di(T)​∥2​ 可以看出其目标为最小化点对间距离的平方和，也就是点到点ICP更新公式

首先定义一个为正交的投影矩阵 P i \bold{P_i} Pi​，有以下性质 P i = P i 2 = P i \bold{P_i} = \bold{P_i}^2 = \bold{P_i} Pi​=Pi​2=Pi​。 其中 P i \bold{P_i} Pi​会将向量投影到目标点云 a a a中的第 i i i点 b i b_i bi​法向量的局部平面上，因此 P i ⋅ d i ( T ) \bold{P_i}\cdot d_i^{(\bold{T})} Pi​⋅di(T)​也就是转换后的 a i a_i ai​到 b i b_i bi​所在平面的距离。 验证： 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 = P i − 1 , C i A = 0 C_i^B=\bold{P_i}^{-1}, C_i^A=0 CiB​=Pi​−1,CiA​=0

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

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

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

( ϵ 0 0 0 1 0 0 0 1 ) \left(\begin{array}{lll} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) ⎝⎛​ϵ00​010​001​⎠⎞​

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

因为实际上法向量并不一定是沿 x x x轴方向，所以需要进行坐标转换。假设 b i , a i b_i,a_i bi​,ai​对应的法向量分别为 u i , v i u_i,v_i ui​,vi​,则它们对应的协方差阵为： C i B = R μ i ⋅ ( ϵ 0 0 0 1 0 0 0 1 ) ⋅ R μ i T C i A = R ν i ⋅ ( ϵ 0 0 0 1 0 0 0 1 ) ⋅ R ν i T \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} CiB​=Rμi​​⋅⎝⎛​ϵ00​010​001​⎠⎞​⋅Rμi​T​CiA​=Rνi​​⋅⎝⎛​ϵ00​010​001​⎠⎞​⋅Rνi​T​​