LSD-SLAM笔记之一致性约束

建图一致性约束也就是做闭环检测和全局优化。由于论文中采用的是直接法，虽然代码中有fabmap检测闭环的部分，但是其默认检测闭环的方式是帧与帧之间做双向跟踪。

这部分的代码入口在函数SlamSystem::constraintSearchThreadLoop，代码主要分了两种情况：

新关键帧队列为空，则在所有关键帧中随机选取测试闭环
新关键帧队列不为空，则取最早的新关键帧测试闭环

在测试闭环时调用的函数为SlamSystem::findConstraintsForNewKeyFrames。该函数主要是根据视差、关键帧连接关系，找出并且在删选处候选帧，然后对每个候选帧和测试的关键帧之间进行双向sim3跟踪，如果求解出的两个李代数满足马氏距离在一定范围内，则认为是闭环成功，并且在位姿图中添加边的约束。而图优化的部分在另外一个线程SlamSystem::optimizationThreadLoop，基本就是使用g2o，这部分就略过了。

关于如何选择候选帧，主要步骤如下：

其实第一个步骤是根据每个关键帧求得的位姿取判断的，其实这是一个因果倒置的方式。在代码中把候选帧分为近处的候选帧和远处的候选帧，对于第二个步骤，只是检测处与当前闭环检测关键帧相近的候选帧。第三步则是核心，把删选处的每一个候选帧与检测的关键帧做双向的Sim3跟踪，如果都成功，并且两个 $\mathfrak{sim}(3)$ 的马氏距离足够小，则认为是检测处闭环，并且在位姿图中构建 $\mathfrak{sim}(3)$ 的边。

接下来主要看一下Sim(3)求解以及双向跟踪检测（reciprocal tracking check）。

1. Sim3求解

1.1 原理分析

1.1.1 Sim3图像对齐

首先和 $\mathfrak{se}(3)$ 变换一样，对于两帧图像上对应归一化图像点 $\mathbf{p}'$ 和 $\mathbf{p}$ 通过 $\mathfrak{sim}(3)$ 变换有：

p' = ⎛ ⎝ ⎜ x' / z' y' / z' 1 ⎞ ⎠ ⎟                  ω n with ⎛ ⎝ ⎜ ⎜ ⎜ x' y' z' 1 ⎞ ⎠ ⎟ ⎟ ⎟ : = exp s i m (3) (ξ) ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ p x / d p y / d 1 / d 1 ⎞ ⎠ ⎟ ⎟ ⎟ ⎟                                    w s (1)

$\begin{equation} \underbrace{\mathbf{p}' = \begin{pmatrix}x'/z'\\y'/z'\\1\end{pmatrix}}_{\omega_n} \quad\text{with}\quad \underbrace{\begin{pmatrix}x'\\y'\\z'\\1\end{pmatrix} := \text{exp}_{\mathfrak{sim}(3)}(\mathbf\xi)\begin{pmatrix}\mathbf{p}_x/d\\\mathbf{p}_y/d\\1/d\\1\end{pmatrix}}_{w_s} \tag{1} \end{equation}$
然后我们看代价函数，

(1) $(1)$ 代价函数和SE3相比多了一项尺度项，并且依旧使用归一化方差：

E (ξ j i) = \sum p \in Ω D i ∥ ∥ ∥ r 2 p ( p , ξ j i ) σ 2 r p ( p , ξ j i ) + r 2 d ( p , ξ j i ) σ 2 r d ( p , ξ j i ) ∥ ∥ ∥ δ (2)

$\begin{equation} E(\mathbf\xi_{ji}) =\sum_{\mathbf{p}\in\Omega_{D_i}}\Biggl\| \frac{r_p^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2} + \frac{r_d^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2} \Biggr\|_\delta \tag{2} \end{equation}$
这里的光度残差和方差的定义和SE3跟踪是一样的:

r p (p, ξ j i) : σ 2 r p (p, ξ j i) : = I i (p) - I j (ω (p, D i (p), ξ j i)) = 2 σ 2 I + (\partial r p ( p , ξ j i ) \partial D i ( p )) 2 V i (p) (3) (4)

$\begin{align} r_p(\mathbf{p},{\mathbf\xi_{ji}}) :&= I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji}))\tag{3}\\ \sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2 :&= 2\sigma_I^2 + (\frac{\partial{r_p(\mathbf{p}, \mathbf\xi_{ji})}}{\partial{D_i(\mathbf{p})}})^2V_i(\mathbf{p})\tag{4} \end{align}$
以及深度残差和方差定义如下：

r d (p, ξ j i) σ 2 r d (p, ξ j i) = [p'] 3 - D j ([p'] 1, 2) = V j ([p'] 1, 2) (\partial r d ( p , ξ j i ) \partial D j ( [ p ' ] 1 , 2 )) + V i (p) (\partial r d ( p , ξ j i ) \partial D i ( p )) (5) (6)

$\begin{align} r_d(\mathbf{p},\mathbf\xi_{ji}) &= [\mathbf{p}']_3-D_j([\mathbf{p}']_{1,2}) \tag{5}\\ \sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2 &= V_j([\mathbf{p}']_{1,2})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_j([\mathbf{p}']_{1,2})}\end{pmatrix} + V_i(\mathbf{p})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_i(\mathbf{p})}\end{pmatrix} \end{align}\tag{6}$

这里的 $\mathbf{p}':=\omega_s(\mathbf{p},D_i(\mathbf{p}),\mathbf\xi_{ji})$ 表示从图像帧 $j$ 变换到图像帧 $i$ 上的点。这里在求解 $\mathfrak{sim}(3)$ 的时候也是同求 $\mathfrak{se}(3)$ 一样，使用了迭代变权重高斯牛顿算法。

注意：论文中提及，把深度图的梯度近似为 $0$ ，因此对 $D_j(\mathbf{u})$ 求的偏导都可以忽略。

根据高斯牛顿算法，对 $(2)$ 式子进行处理。由于Huber-Norm是在最外面的，其最终是等效为一个权重