LSD-SLAM笔记之SE3Tracking

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

SlamSystem::trackFrame

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

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

构造新的图像帧

：把当前图像构建为新的图像帧
更新参考帧

：如果当前参考帧不是最近的关键帧，则更新参考帧
初始化位姿

：把上一帧与参考帧的位姿当做初始位姿
SE3求解

：调用SE3Tracker计算当前帧和参考帧间的位姿变换
判断是否跟踪失败

：根据跟踪的像素点个数多少以及跟踪质量来判断
关键帧筛选

：通过计算得分确定是否构造新的关键帧

接下来主要介绍

SE3求解

和

关键帧筛选

1. SE3求解

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

函数
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）：

E p (ξ j i) r p (p, ξ j i) : σ 2 r p (p, ξ j i) : = \sum p \in Ω D i ∥ ∥ ∥ r 2 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) (12) (13) (14)

$\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 ∥ δ : = ⎧ ⎩ ⎨ ⎪ ⎪ r 2 2 δ | r | - δ 2 if | r | \leq δ otherwise (15)

$\|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)$
式前面的是连个图像的图像高斯噪声，后面的是逆深度造成的误差。这里逆深度误差不确定性是根据下式计算得到的：

Σ f = J f Σ x J T f (11)

$\mathbf{\Sigma}_f = \mathbf{J}_f \mathbf{\Sigma}_{\mathbf x} \mathbf{J}_f^T \tag{11}$

$(14)$
式的具体推导在代码部分讲详细说明。

论文中给出了3D投影变换

$\omega$
的表达式，也就是论文中的公式

$(3)$
：

ω (p, D i (p), ξ)) : = ⎛ ⎝ ⎜ x' / z' y' / z' 1 / z' ⎞ ⎠ ⎟ with ⎛ ⎝ ⎜ ⎜ ⎜ x' y' z' 1 ⎞ ⎠ ⎟ ⎟ ⎟ : = exp s e (3) (ξ) ⎛ ⎝ ⎜ ⎜ ⎜ ⎜ p x / d p y / d 1 / d 1 ⎞ ⎠ ⎟ ⎟ ⎟ ⎟ (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, D i (p), ξ)) = ⎛ ⎝ ⎜ x' /

原文链接：https://blog.csdn.net/kokerf/article/details/78005934

1. SE3求解

1.1 直接法SE3图像对齐算法

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

你可能也喜欢