LSD-SLAM笔记之SE3Tracking

LSD-SLAM的Tracking是算法框架中三大部分之一。其主要实现函数为SlamSystem::trackFrame

void SlamSystem::trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp)

这个函数的代码主要分为如下几个步骤实现：

1. 构造新的图像帧：把当前图像构建为新的图像帧
2. 更新参考帧：如果当前参考帧不是最近的关键帧，则更新参考帧
3. 初始化位姿：把上一帧与参考帧的位姿当做初始位姿
4. SE3求解：调用SE3Tracker计算当前帧和参考帧间的位姿变换
5. 判断是否跟踪失败：根据跟踪的像素点个数多少以及跟踪质量来判断
6. 关键帧筛选：通过计算得分确定是否构造新的关键帧

接下来主要介绍SE3求解和关键帧筛选

1. SE3求解

LSD-SLAM在位姿估计中采用了直接法，也就是通过最小化光度误差来求解两帧图像间的位姿变化。并且采用了LM算法迭代求解。LSD-SLAM在求解两帧图像间的SE3变换主要在SE3Tracker类中进行实现。该类中有四个函数比较重要，分别为

• SE3Tracker::trackFrame
• SE3Tracker::calcResidualAndBuffers
• SE3Tracker::calcWeightsAndResidual
• SE3Tracker::calculateWarpUpdate

函数SE3Tracker::trackFrame需要传入三个形参，从参考帧跟踪到当前帧，首先是两帧图像帧的实例，另外就是给定的初始位姿。对于初始位姿，LSD-SLAM中使用了上一帧和参考帧间的位姿作为当前位姿估计的初值(见上面的第３个步骤)。

SE3 SE3Tracker::trackFrame(
    TrackingReference* reference,
    Frame* frame,
    const SE3& frameToReference_initialEstimate);

SE3Tracker::trackFrame函数的主体是一个for循环，从图像金字塔的高层level-4开始遍历直到底层level-1。在循环内实现加权高斯牛顿优化算法（Weighted Gauss-Newton Optimization），其实就是增加了鲁棒函数的高斯牛顿。首先看一下SE3Tracker::trackFrame函数的代码结构，可以归纳为如下：

图像金字塔迭代level-4到level-1
Step1: 对参考帧当前层构造点云(reference->makePointCloud)
Step2: 计算变换到当前帧的残差和梯度(calcResidualAndBuffers)
Step3: 计算法方差归一化残差(calcWeightsAndResidual)
Step4: 计算雅克比向量以及A和b(calculateWarpUpdate)
Step5: 计算得到收敛的delta，并且更新SE3(inc = A.ldlt().solve(b))
重复Step2-Step5直到收敛或者达到最大迭代次数
计算下一层金字塔

接下来首先对SE3求解的算法原理做个详细的介绍。

1.1 直接法SE3图像对齐算法

1.1.1 优化模型——归一化方差的光度误差

在LSD-SLAM论文的3.3节介绍了这个对齐算法。首先给出优化模型（这里的标号与论文中的统一），论文采用了最小化归一化方差的光度误差（variance-normalized photometric error）：

$$\begin{align} E_p(\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}\Biggr\|_\delta \tag{12}\\ r_p(\mathbf{p},{\mathbf\xi_{ji}}) :&= I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji})) \tag{13}\\ \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{14} \end{align}$$
这里的 $\|\ast\|_\delta$ 为Huber-norm，表示为：
$$\|r^2\|_\delta:=\begin{cases} \frac{r^2}{2\delta} & \text{if}\quad |r|\le \delta \\[2ex] |r|-\frac{\delta}{2} & \text{otherwise} \end{cases}\tag{15}$$
式子中的 $\xi_{ji}$就是要求的两帧间的SE3变换（从 $i\rightarrow{j}$，也就是参考帧到当前帧的变换），这里是用李代数的形式表示；这里的 $\mathbf{p}$是在参考帧 $I_i$观测到的有深度信息（ $\mathbf{p}\in\Omega_{D_i}$）的 归一化图像点；函数 $D_i(\mathbf p)$ 是点 $\mathbf p$在参考帧下的 逆深度， $V_i(\mathbf p)$ 是对应逆深度的方差；函数 $\omega({\mathbf p}, D_i({\mathbf p}), \xi_{ji})$ 是3D投影变换（3D projective warp），把参考帧下 ${\mathbf p}$对应的3D点投影到当前帧的图像平面；这里的 $\sigma_I^2$是图像的高斯噪声。

我们考虑，这里的归一化方差是什么意思？论文中在估计两帧间位姿变换的时候，把所有有逆深度假设的像素都用上了。但是每个逆深度的确定性不同，也就是有些逆深度比较准确，有些不准确。而准确与否则体现在逆深度的方差上了。因此在公式$(12)$中在残差上除以了方差做了归一化。在论文中考虑了两个方面的方差，一个是由逆深度估计不准确引入的，另外一个是由图像高斯噪声引起的。也即是说$(14)$式前面的是连个图像的图像高斯噪声，后面的是逆深度造成的误差。这里逆深度误差不确定性是根据下式计算得到的：

$$\mathbf{\Sigma}_f = \mathbf{J}_f \mathbf{\Sigma}_{\mathbf x} \mathbf{J}_f^T \tag{11}$$
$(14)$式的具体推导在代码部分讲详细说明。

论文中给出了3D投影变换$\omega$的表达式，也就是论文中的公式$(3)$：

$$\begin{split} \omega({\mathbf p}, D_i({\mathbf p}), \xi)) := \begin{pmatrix}x'/z'\\y'/z'\\\color{red}{1/z'}\end{pmatrix} \quad\text{with}\quad \begin{pmatrix}x'\\y'\\z'\\1\end{pmatrix} := \text{exp}_{\mathcal se(3)}(\xi)\begin{pmatrix}\mathbf{p}_x/d\\\mathbf{p}_y/d\\1/d\\1\end{pmatrix} \end{split}\tag{3}$$
其实这个公式只是把3D点变换到单位平面上，这里有一个问题：一个是标红的 $1/z'$应该是 $1$，正确的写法应该是：

ω(p,Di(p),ξ))=⎛⎝⎜x′/