DSO前端跟踪
嗨,各位读者朋友们好!今天我们接着讲前端跟踪。由于第三讲中的初始化部分包含trackFrame估计初始两帧运动,与本讲内容重叠,因此笔者将其初始化中的跟踪合并到当前讲中,具体的区别稍后我们会在介绍跟踪流程的过程中叙述。
OK,我们还是一如既往地先上个流程图,如图1所示。笔者始终认为只有把流程图画清晰了,才能说明自己对整个流程足够了解。在写博客的时候,流程图有助于梳理思路,便于读者们更加清楚地知道整个运行逻辑。

图1. DSO前端跟踪流程
特征点法SLAM中可以通过首先进行参考帧和当前帧的特征点匹配找到匹配对,构建数据关联关系,从而估计相对运动。而基于直接法的DSO前端跟踪,却没有这么方便了,因为直接法中的点都是局部梯度较大的点,除此之外并无其他显著特征,因为无法做重投影误差。而直接法的策略是将参考帧中所有点(提取到的梯度点)投影到当前帧中,构建光度误差函数,联合优化。那么这里,有个问题就需要特别注意了,直接法无法构建数据关联关系,那么初始的相对运动就无法确定,所以直接法需要首先设置运动模型,才能继续后面的步骤。
接下来,我们按照图1来大致梳理一下DSO前端跟踪的思路:
1. 设置运动模型,如前所述,需要根据先验值给出运动模型。
在DSO初始化阶段,这个先验值被设置为旋转单位阵、位移零向量,表示两帧间完全重合的意思。
在DSO常规的跟踪阶段,这个先验值通常是由前面几帧确定的,比如前一帧和前前帧之间的相对运动作为当前帧的运动模型,称为单倍速运动模型。DSO还添加了两倍速、0.5倍速模型,此外还有在此基础上的一些较小的旋转模型等。
2. 遍历所有运动模型,对于每个运动模型都执行后续的直接法估计。只有前面存在满足要求的运动模型时,后续的模型才不会被继续验证,跳出循环。若是所有运动模型都不满足要求,则说明跟踪丢失。
在DSO初始化阶段,循环迭代会设置跳出条件,除了满足收敛或者迭代次数达到最大值这两种情况,初始化阶段还设置了一个fails次数,若次数过多,则当前初始化失败,重新开始。
在DSO常规的跟踪阶段,循环迭代设置的跳出条件是满足收敛条件或者迭代次数达到最大值。若最终阈值不满足条件,则验证下一个运动模型。
3. 直接法运动估计,采用的是有点类似于光流的方法,在这里需要用到图像金字塔,目的是为了节省计算时间。
从图像金字塔最顶层开始估计相对运动,并逐渐向下传播。值得注意的是,我们在上一讲中提到了makeNN()这个函数,他的目的是创建每一层每个点的10邻域信息,并且关联其上一层的某个最近邻点。显然,在基于图像金字塔的跟踪过程,上一层的深度值可以传播到下一层作为初值,这便是第三讲的内容最重要的意义。
4. 高斯牛顿优化,这部分内容是整个跟踪过程最核心的,也是最难的。
已知:初始的运动模型、参考帧图像中的梯度点、参考帧和当前帧构建的光度误差函数。
问题:通过优化运动模型,使得光度误差函数的代价值最小。
求解策略:利用泰勒展开近似非线性函数,通过高斯牛顿法构建增量方程:$Hx = b$。后续便是基于schur消元法求解增量值,基于判断条件对运动模型进行更新。
在上面的四个主要流程中,最核心的便是第4个——高斯牛顿优化。我们会在这一讲着重介绍一下DSO中在直接法运动估计中,这个高斯牛顿法是怎样操作的。
直接法运动估计
在前面我们提到过,直接法不同于特征点法,可以通过特征匹配来构建数据关联关系。那么直接法通常的做法呢,是将两个参考图像中提取到的梯度点全部投影到当前图像中,通过调整位姿和光度仿射模型使得两帧图像间的光度误差值最小。
为了更好地描述问题,我们会采用数学符号来完成这一讲的内容。在此之前呢,我们要先定义一下符号。(昨天吃饭的时候才刚跟范帝楷聊过这个事情)
假设我们当前有参考帧 和当前帧
,参考帧中的光度仿射参数为
, 同理,当前帧的为
。相机内参数矩阵为
,由参考帧
的坐标系到当前帧
的坐标系相对变换的李代数为
,并且我们有:
\begin{equation}
exp(\xi^{\wedge}_{cr}) = \begin{bmatrix}R_{cr} & t_{cr} \\ 0^{T} & 1\end{bmatrix}
\end{equation}
OK,那么我们接下来开始描述光度误差函数是怎么定义的。
对于单一的一个点,我们先不考虑DSO中描述的pattern,只单纯的考虑对于一个像素,这个光度误差函数怎么描述。
\begin{equation}
r = w((I_{c}[p_{c}] - b_{c}) - \frac{t_{c} e^{a_{c}}}{t_{r}e^{a_{r}}}(I_{r}[p_{r}] - b_{r})) = w(I_{c}[p_{c}] - a_{cr}I_{r}[p_{r}] - b_{cr})
\end{equation}
其中,点为参考帧
中梯度较大的点,
表示点
在当前帧
中的反投影坐标。具体计算方法:
\begin{equation} \begin{aligned}
p_{c} &= KX_{c}^{'} \\ &= Kd_{c}X_{c} \\ &= Kd_{c}(R_{cr}X_{r} + t_{cr}) \\ &= Kd_{c}(d_{r}^{-1}R_{cr}K^{-1}p_{r} + t_{cr}) \\ &= Kd_{c}exp(\xi^{\wedge}_{cr})(d_{r}^{-1}K^{-1}p_{r})
\end{aligned} \end{equation}
在公式 中,我们定义了一些新的符号:
1. 点 在参考帧相机坐标系中的点表示为