0.友情链接

1. FGR基本介绍 CSDN博客：Fast Global Registration
2. FGR的github链接：
3. FGR的论文下载地址
4. FGR复现代码（open3d）
5. FPFH论文原文

1. 论文核心思想

1.1. 点云特征匹配

\qquadFast Global Registration（FGR）是ECCV 2016的一篇关于点云全局配准的论文。所谓点云配准就是将点云A通过六自由度刚性变换矩阵T，将其变换到点云B，达到场景合并的目的，常常用于点云场景重建、SLAM、多传感器融合领域。
\qquadFGR的基本理论建立在特征匹配的基础上的，FGR使用的特征是比较快速的Hand-crafted的FPFH（Fast Point Feature Histogram，快速点云特征直方图，论文见友情链接）。FPFH相比其他learning-based的特征Recall比较低，但是优点是不需要GPU，运算速度快。
\qquad对于源点云$P$和模板点云$Q$，首先使用FPFH提取每个点的特征组成$F(P)$和$F(Q)$，然而通过特征距离的最近邻对二者进行匹配（这点和ICP是类似的），FPFH原文的特征距离法则略为复杂，而FGR实现代码中则是采用是更简单的向量二范数计算特征距离，感兴趣的可以看下FPFH的原文。
\qquad对于$F(Q)$中的每个特征$f(q_i),(q_i\in Q)$，找出$F(P)$中最接近$f(q_i)$的$f(p_j)$，即
$$\arg \underset{p_j\in P}{\min }\Vert f(p_j)-f(q_i){\Vert }_2^2$$
\qquad以上即是FGR特征匹配的基本思想了。需要知悉的是，整个点云配准过程，FPFH的匹配只会操作一次，这便是FGR相比其他Robust Point Cloud Registration快的原因，然而这个操作其实也占用了FGR大部分的运行时间。

1.2. 两个校验

\qquad接下来进入FGR的核心部分——outlier rejection。由于特征匹配存在较多的outlier （如下图红色连线所示）。对于correspondence-based 方法而言，这些outlier会对最后的SVD过程造成干扰，因而FGR的目的就是滤除这些红色的错误匹配。

本图摘自论文（Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection）
\qquadFGR的第一校验是双向校验（dual validation），在大多数特征匹配方法里还是比较常见的。记1.1节得到的匹配好的特征为$K_1=\{(p_{t_1},q_1),(p_{t_2},q_2),...,(p_{t_n},q_n)\}$，（n应该等于点云$Q$的点数），对于$K_1$中的每个特征对$(p_{t_i},q_i)$，寻找
$$q_i^{\ast }=\arg \underset{q_k\in Q}{\min }\Vert p_{t_i}-q_k{\Vert }_2^2$$
若$q_i^{\ast }\ne q_i$，则排除这个点云对。
\qquad第一校验筛选完之后的点云对记为$K_2$。
\qquad第二校验是距离差校验，选取$K_2$中任意三个点云对$(p_1,p_2,p_3)$和$(q_1,q_2,q_3)$，它们需要满足如下的不等式：
$$\forall i\ne j,0.9<\frac{\Vert p_i-p_j\Vert }{\Vert q_i-q_j\Vert }<\frac{1}{0.9}$$
若不满足，则排除$(p_1,q_1),(p_2,q_2),(p_3,q_3)$三对点。以此得到$K_3$。
\qquad如果是理想情况，应该满足$\frac{\Vert p_i-p_j\Vert }{\Vert q_i-q_j\Vert }=1$，FGR通过一个距离范围排除那些明显不符合距离检验的点，但需要注意的是，这个检验并不是始终有效的。TEASER那篇论文就证明，当outlier ratio特别大时，距离检验也失效了，原因可能是$K_2$中每三组对应点中都包含outlier，导致选不出几组正确的点。

1.3. 鲁棒函数与BR对偶

\qquad这可能是FGR论文最难懂也是最核心的部分。在完成了一次匹配和两轮校验之后，FGR需要通过Gradually Non-Convexity（GNC，渐进非凸过程）的方式完成outlier rejection。经典ICP在已完成特征匹配后的目标函数是
$$\arg \underset{T\in SE(3)}{\min }\sum _{(p,q)\in K}\Vert q-\text{T}p\Vert$$
其中$K$是matched feature的集合，在FGR中指的是$K_3$。
\qquad然而最小二乘的函数对于outlier非常敏感，即使经过两轮校验，仍然会存在大量的outlier。
\qquadFGR的目标函数是鲁棒函数Geman-McClure，即$\rho =\frac{\mu x^2}{\mu +x^2}$，即
$$\arg \underset{T\in SE(3)}{\min }\sum _{(p,q)\in K}\rho (\Vert q-\text{T}p\Vert )$$
对于鲁棒函数的使用，FGR原文是这样解释的：

使用适当的鲁棒惩罚函数是至关重要的，因为目标函数中很多匹配约束都是伪约束。为了实现高计算效率，我们不希望在优化过程中采样、验证、修剪或重新计算关联。一个精心选择的估计器ρ将自动执行验证和剪枝，而不施加额外的计算成本。

说的直白一些，鲁棒函数可以在不改变匹配对集合$K$的情况下，使得最优解$T^{\ast }$自然地满足内点优先匹配的原则。其中的原因其实是由鲁棒函数非凸性造成的。对于outlier具有较高的、较为统一的损失，对于inlier具有较低的损失。

\qquadFGR使用的Geman-McClure是一个经典的GNC函数，随着$\mu$的减小，越来越接近于非凸函数，而当$\mu \to \infty$时，等价于$x^2$，即理想凸函数。为什么要使用GNC而不直接采用一个鲁棒函数（如Huber Loss）优化呢？因为一开始得到的T可能是个非常错误的值，就会造成$$\sum _{(p,q)\in K}\rho (\Vert q-\text{T}p\Vert )$$也是个错误的函数，因此需要逐步降低函数$\rho$的凸性逐步逼近正确的T。

\qquad承接上文，FGR为了不增加额外的计算量（其实主要是不重新计算correspondence），采用了BR对偶的方式完成鲁棒函数的优化求解。
\qquad其做法就是增加额外的线性过程$\mathcal{L}$，构造目标函数$E(\text{T},\mathcal{L})$，并通过对$\mathcal{L}$和$\text{T}$交替优化的方式完成的推导过程及优化求解。相关截图如下：

这一套操作对于非专业人士确实非常不友好，文中的解释也只是a prior（一个先验知识），实际上它牵涉到BR对偶性的知识，想完全看懂FGR的读者务必得往下看。

1.4.1. Black-Rangarjan Duality (BR对偶性）

原文【PDF】链接

\qquad这是构造鲁棒函数和线性过程关联性的非常有名的方法，用于解决early-vision（例如平面重建、三维重建、图像恢复）滤除outlier的问题。这篇20世纪论文的引用量高达900+，足以证明它完全没有过时。
\qquad原文的理论对于非数学专业的同学可能非常难懂，本文只讲解它当中最重要的部分——定理和鲁棒函数的构造方法。定理如下：

图片源于：https://arxiv.org/pdf/1909.08605?ref=https://githubhelp.com
\qquad这条定理说的是给定一个鲁棒函数$\rho$，定义$\varphi (z)=\rho (\sqrt{z})$，若$\varphi (z)$满足一定的条件，则可以将原鲁棒函数优化的问题转化为定理中的weighted process的等价形式。而这个新的二元函数的优化过程详见1.4.3节。

本文并不会证明这条定理，但需要读者知悉以下几点：

1. $\varphi (\cdot )$和鲁棒函数$\rho (\cdot )$的关系
2. 等价命题的形式（$\varphi (\cdot )$需满足的条件）
3. 等价命题${\Phi }_{\rho }(\cdot )$的含义（关于$w_i$的惩罚项）

最难理解的是${\Phi }_{\rho }$的构造过程，下面将会给出FGR中选取${\Phi }_{\rho }$（详见1.4节）的推导过程。

1.4.2.Derivation of ${\Phi }_{\rho }$

Black-Rangarjan 对偶性原文给出寻找的close-form的${\Phi }_{\rho }$的算法步骤如下：

实际上，大部分论文都将$\tau$默认为1处理。FGR中给出的形式为

这里的$l_{\text{p},\text{q}}$对应原文中的$z$，$$\rho (x)=\frac{\mu x^2}{\mu +x^2}$$
根据$\varphi (\cdot )$与$\rho (\cdot )$的关系
$$\varphi (z)=\rho (\sqrt{x^2})=\frac{\mu z}{\mu +z}(z\ge 0)$$
$\varphi (z)$需要满足Black-Rangarajan对偶性的三个条件，即
$$\{\begin{array}{ll}& {\lim }_{x\leftarrow 0}{\varphi }^{\prime }(x)={\lim }_{x\leftarrow 0}\frac{{\mu }^2}{(\mu +x{)}^2}=1\\ & {\lim }_{x\leftarrow \infty }{\varphi }^{\prime }(x)={\lim }_{x\leftarrow \infty }\frac{{\mu }^2}{(\mu +x{)}^2}=0\\ & {\varphi }^{\prime \prime }(x)=-\frac{2{\mu }^2}{(\mu +x{)}^3}<0\end{array}$$
根据上述推导，显然$\varphi (\cdot )$都满足了，接下来需要根据Black-Rangarajan原文中提供的方法解出${\Phi }_{\rho }$。
首先，$\varphi (w)=\frac{\mu w}{\mu +w}$, 令$z={\varphi }^{\prime }(w)=\frac{{\mu }^2}{(\mu +w{)}^2}$，则$w=({\varphi }^{\prime }{)}^{-1}(z)=\mu /\sqrt{z}-\mu$

$$\begin{array}{rl}{\Phi }_{\rho }(z)=& \varphi (w)-zw\\ =& \frac{\mu w}{\mu +w}-zw\\ =& \frac{\mu (\frac{\mu }{\sqrt{z}}-\mu )}{\mu +\frac{\mu }{\sqrt{z}}-\mu }-z(\frac{\mu }{\sqrt{z}}-\mu )\\ =& \mu (1-\sqrt{z}-\sqrt{z}+z)=\mu (\sqrt{z}-1{)}^2\end{array}$$
即FGR原文中的鲁棒函数公式。

1.4.3.$E(\mathbf{T},L)$的优化求解方法

利用BR对偶性求解离群值过程的方法是交替优化（Alternating optimization），即对$\mathbf{x}$和$w_i$进行交替优化，具体方法截图如下。

需要说明的是，第二步$w_i$的优化一般是存在闭解的，针对FGR的目标函数求$w_i$的偏导可得：
$$\frac{\partial E(\mathbf{T},L)}{\partial w_i}=\Vert \text{p}-\mathbf{T}\text{q}{\Vert }^2+\mu \frac{\sqrt{l_{p,q}}-1}{\sqrt{l_{p,q}}}$$
导函数在$(0,+\infty )$上单调且存在唯一的零点，即$l_{p,q}$的闭合形式最优解：
$$l_{p,q}={\left(\frac{\mu }{\mu +\Vert \text{p}-\mathbf{T}\text{q}{\Vert }^2}\right)}^2$$

第一步对于$\mathbf{T}$的优化，FGR采用的是一种线性近似的方式，具体方式如下：

这个为什么属于线性近似呢？可以将这个矩阵拆成两个部分看：
$$M=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]$$
平移部分就等于它本身，旋转部分根据李群的计算法则，设
$$R_x=\left[\begin{array}{ccc}0& -\gamma & \beta \\ \gamma & 0& -\alpha \\ -\beta & \alpha & 0\end{array}\right]$$
$$R=exp(R_x)\approx R^0+\frac{R^1}{1!}+...=\left[\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}\right]$$
因此这个近似在$(\alpha ,\beta ,\gamma )$较小的时候还是可以成立的。

其中$T^k$是最后一次迭代的$T$，而$\xi =(\alpha ,\beta ,\gamma ,a,b,c)$实际上是通过高斯-牛顿法这种数值优化方法求解的，求解方法就是上图中的(8)。

4.写在后面

\qquadFGR的全部讲解到此就结束了，实际上，FGR在实现过程逐步地减小$\mu$以实现Graduate Non-Convexity (GNC)。将BR对偶性与GNC相结合，可以解决非凸鲁棒函数的全局最优解的问题，同时还是最优性可证的。本文只是相关理论基础的一个补充，欢迎在阅读相关文献的基础上在本文评论区留言交流学习心得。