点云配准--Fast Global Registration

3两个点云配准

3.1目标函数

对于俩个点集$P$和$Q$，我们的目的是找到一个刚体变换，对齐这两个点集。我们的方法是优化关于$P$和$Q$对应关系的稳健性目标函数。
设$K=(p,q)$是由匹配点从P和Q中获得的对应集合，我们的目标是优化位姿T，使得对应点之间的距离最小化，同时拒绝$K$中的的错误对应关系。目标函数具有如下形式：
$$E(\mathbf{T})=\sum _{(\mathbf{p},\mathbf{q})\in \mathcal{K}}\rho (\Vert \mathbf{p}-\mathbf{Tq}\Vert )\text{\hspace{1em}(1)}$$
$\rho (.)$是罚函数。罚函数的选择是至关重要的，一个好的罚函数可以规避错误的匹配关系。我们使用Geman-McClure估计器：
$$\rho (x)=\frac{\mu x^2}{\mu +x^2}\text{\hspace{1em}(2)}$$
目标函数1是很难直接进行优化的，我们利用Black -Rangaration对偶（对偶：即等价的）处理目标函数1，Black -Rangaration对偶是稳健估计和线性的。具体地，令 L = { l p ， q } L = \lbrace l_{p，q} \rbrace L={lp，q​}为对应关系上的线过程。我们在T和L上优化以下联合目标函数：
$$E(\mathbf{T},\mathbb{L})=\sum _{(\mathbf{p},\mathbf{q})\in \mathcal{K}}{l}_{\mathbf{p},\mathbf{q}}\Vert \mathbf{p}-\mathbf{Tq}{\Vert }^2+\sum _{(\mathbf{p},\mathbf{q})\in \mathcal{K}}\Psi \left(l_{\mathbf{p},\mathbf{q}}\right)\text{\hspace{1em}(3)}$$
$\Psi \left(l_{\mathbf{p},\mathbf{q}}\right)$是先验概率：
$$\Psi \left(l_{\mathbf{p},\mathbf{q}}\right)=\mu {\left(\sqrt{l_{\mathbf{p},\mathbf{q}}}-1\right)}^2\text{\hspace{1em}(4)}$$
为了最小化$E(\mathbf{T},\mathbb{L})$，需要对每个$l_{\mathbf{p},\mathbf{q}}$求偏导：
$$\frac{\partial E}{\partial l_{\mathbf{p},\mathbf{q}}}=\Vert \mathbf{p}-\mathbf{Tq}{\Vert }^2+\mu \frac{\sqrt{l_{\mathbf{p},\mathbf{q}}}-1}{\sqrt{l_{\mathbf{p},\mathbf{q}}}}=0\text{\hspace{1em}(5)}$$
所以：
$$l_{\mathbf{p},\mathbf{q}}={\left(\frac{\mu }{\mu +\Vert \mathbf{p}-\mathbf{Tq}{\Vert }^2}\right)}^2\text{\hspace{1em}(6)}$$
将$l_{\mathbf{p},\mathbf{q}}$带入公式3，得到公式1。因此优化目标函数3即是优化目标函数1。

3.2优化

等式3的好处就是可以交替优化T和L。
当L固定时：等式3转变为对应关系的距离的$L^2$罚函数的权重之和。通过文献9可以轻松的得到封闭解。但是这种求解方式只适合两两点云配准，不适合求解多个点云同时配准。因此提出一种新的计算方法：假设两次变化矩阵的变化很小，则变换矩阵$T$用下面公式进行近似：
$$\mathbf{T}\approx \left(\begin{array}{cccc}1& -\gamma & \beta & a\\ \gamma & 1& -\alpha & b\\ -\beta & \alpha & 1& c\\ 0& 0& 0& 1\end{array}\right){\mathbf{T}}^k\text{\hspace{1em}(7)}$$
$\alpha ,\beta ,\gamma ,a,b,c$分别是三个旋转量和三个偏移量，$T^k$是上一次迭代得到的变换矩阵。则通过高斯牛顿法可以求解公式3.
当T固定时，当$l_{p,q}$满足公式6时，等式3可以求得封闭解。

3.3对应关系

我们选择Fast Point Feature Histogram (FPFH) feature。记 F ( P ) = { F ( p ) : p ∈ P } F(P)=\lbrace{F(p):p\in{P}}\rbrace F(P)={F(p):p∈P},$F(p)$是点p对应的FPFH，同样地，记 F ( Q ) = { F ( q ) : q ∈ Q } F(Q)=\lbrace{F(q):q\in{Q}}\rbrace F(Q)={F(q):q∈Q}。对于点p的F§，在F(Q)中找到p点的最近邻点，同样对于点q，在F§中找到q的最近邻点。记$K1$为对应关系集合，但是这里面有很多错误的匹配对，因此运用一下两个准则进行筛选：
（1）互惠性检测
如果F§和F(q)互为最近邻点，则通过互惠性检验，是一个对应关系对，将所有通过互惠性检验的对应关系集合记为$K2$.
(2)元组检测
从$K2$随机选择3个匹配对$(p1,q1),(p2,q2),(p3,q3)$,如果满足以下关系上，表明3个匹配对是相容的，将所有通过测试的匹配对集合记为$K3$，$K3$即为最终的匹配对集合。
$$\forall i\ne j,\tau <\frac{\Vert {\mathbf{p}}_i-{\mathbf{p}}_j\Vert }{\Vert {\mathbf{q}}_i-{\mathbf{q}}_j\Vert }<1/\tau \text{\hspace{1em}(8)}$$
整个算法如下图所示：

4多个点云配准

我们开发一种方法：直接将所有点云同时配准，而不是优化单独的成对的点云，然后优化两两配准结果。我们直接优化所有点云的全局位姿。

4.1目标函数

对于点云集合 { P i } \lbrace{P_i}\rbrace {Pi​},其对应的变换矩阵集合为 T = { T i } \mathbb{T}=\lbrace{T_i}\rbrace T={Ti​}.对于所有点云而言，公式1将变为如下等式(i表示点云序号，j表示每个点云中第j个匹配对)：
$$E(\mathbb{T})=\lambda \sum _i\sum _{(\mathbf{p},\mathbf{q})\in {\mathcal{K}}_i}{\Vert {\mathbf{T}}_i\mathbf{p}-{\mathbf{T}}_{i+1}\mathbf{q}\Vert }^2+\sum _{i<j}\sum _{(\mathbf{p},\mathbf{q})\in {\mathcal{K}}_{ij}}\rho \left(\Vert {\mathbf{T}}_i\mathbf{p}-{\mathbf{T}}_j\mathbf{q}\Vert \right)\text{\hspace{1em}(10)}$$
根据公式3和公式4，等式10可以变为如下形式：
$$\begin{array}{rl}E(\mathbb{T},\mathbb{L})& =\lambda \sum _i\sum _{(\mathbf{p},\mathbf{q})\in {\mathcal{K}}_i}{\Vert {\mathbf{T}}_i\mathbf{p}-{\mathbf{T}}_{i+1}\mathbf{q}\Vert }^2\\ & +\sum _{i<j}\left(\sum _{(\mathbf{p},\mathbf{q})\in {\mathcal{K}}_{ij}}{l}_{\mathbf{p},\mathbf{q}}{\Vert {\mathbf{T}}_i\mathbf{p}-{\mathbf{T}}_j\mathbf{q}\Vert }^2+\sum _{(\mathbf{p},\mathbf{q})\in {\mathcal{K}}_{ij}}\Psi \left(l_{\mathbf{p},\mathbf{q}}\right)\right)\end{array}\text{\hspace{1em}(11)}$$

4.2优化

我们再次使用交替优化求解最小化问题。在每次迭代中，首先使用L最小化$E(\mathbb{T},\mathbb{L})$：
$$l_{\mathbf{p},\mathbf{q}}={\left(\frac{\mu }{\mu +{\Vert {\mathbf{T}}_i\mathbf{p}-{\mathbf{T}}_j\mathbf{q}\Vert }^2}\right)}^2\text{\hspace{1em}(12)}$$
然后使用所有变换矩阵$\mathbb{T}$最小化$E(\mathbb{T},\mathbb{L})$（这里和两两配准优化顺序是反的）。记 ${\mathbf{T}}_i^k$ 为上一次迭代计算的变换矩阵. ${\mathbf{T}}_i$ 可以被线性化为以下形式：
$$\begin{array}{rl}{\mathbf{T}}_i& \approx \left(\begin{array}{cccc}1& -{\gamma }_i& {\beta }_i& a_i\\ {\gamma }_i& 1& -{\alpha }_i& b_i\\ -{\beta }_i& {\alpha }_i& 1& c_i\\ 0& 0& 0& 1\end{array}\right){\mathbf{T}}_i^k.\end{array}\text{\hspace{1em}(13)}$$
使用最小二乘计算旋转和平移量，然后更新${\mathbf{T}}_i$。
注意：对应关系只计算一次，并不是每次都更新。每次迭代只更新$l_{\mathbf{p},\mathbf{q}}$和$T$。
文献来源：《Fast Global Registration》
开源代码(两两配准)