LSD-SLAM笔记之一致性约束

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

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

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

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

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

• 视角和距离判别
• SE3跟踪检测
• Sim3跟踪检测

其实第一个步骤是根据每个关键帧求得的位姿取判断的，其实这是一个因果倒置的方式。在代码中把候选帧分为近处的候选帧和远处的候选帧，对于第二个步骤，只是检测处与当前闭环检测关键帧相近的候选帧。第三步则是核心，把删选处的每一个候选帧与检测的关键帧做双向的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)$变换有：

$$\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)$代价函数和SE3相比多了一项尺度项，并且依旧使用归一化方差：
$$\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跟踪是一样的:
$$\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}$$
以及深度残差和方差定义如下：
$$\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是在最外面的，其最终是等效为一个权重