DSO全家桶（四）——DSO前端：前端跟踪

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. 点 在参考帧相机坐标系中的点表示为 ；

　　2. 点 转换到当前帧相机坐标系中的点表示为 ；

　　3. 点 在当前帧相机坐标系中的归一化坐标表示为 ；

　　4. 点 在当前帧相机平面上的像素点坐标表示为 。

　　值得注意的是，公式 中我们忽略了部分齐次坐标的转换，这些转换尽管影响不大，但是还是希望读者们能够知道这里哪些地方的维度发生了变化。

　　OK，接下来我们开始讨论对于上述定义的光度误差函数，我们会用什么方式来优化？

　　我们假设待优化的变量为 。其中 为单一一个点的逆深度值，对于单个点的能量函数，我们有：

　　\begin{equation}
　　　　E(x) = r(x)^{T}r(x)
　　\end{equation}

　　实际上，残差的权重已经是经过Huber核函数决策得到的了。DSO论文中提到一个关于梯度的权重，这个权重是用在滑窗优化中的，在跟踪阶段并不涉及。因此我们不做讨论。

　　对于公式 $(4)$ 中的能量函数 ，我们采用高斯-牛顿法进行迭代优化，每次添加一个增量 ，通过泰勒展开可以得到：

　　\begin{equation} \begin{aligned}
　　　　E(x + \Delta x) &= r(x + \Delta x)^{T} r(x + \Delta x) \\ &\approx (r(x) + J\Delta x)^{T}(r(x) + J\Delta x) \\ &= r(x)^{T}r(x) + 2\Delta x^{T} J^{T}r(x) + \Delta x^{T}J^{T}J\Delta x
　　\end{aligned} \end{equation}

　　其中， 表示残差 对变量 的雅克比矩阵 。

　　通过对增量 进行求导，并令导数结果为 ，我们可以得到：

　　\begin{equation}
　　　　\frac{\partial E}{\partial \Delta x} = 2J^{T}r(x) + 2J^{T}J\Delta x = 0 \Rightarrow J^{T}J\Delta x = -J^{T} r(x) \Rightarrow H\Delta x = -b
　　\end{equation}

　　在DSO中通常包含两种待优化变量，第一种是跟机器人自身本体相关的变量：位姿+光度仿射参数；第二种是跟环境相关的变量：环境的点云信息(即观测点)。但是在前端的跟踪中，我们并不考虑优化点云的深度值，因此我们可以采用schur消元法来简化上述的优化问题。我们用下标 来表示位姿+光度仿射参数，用下标 表示点的逆深度信息，那么我们可以将增量方程进行分块：

　　\begin{equation}
　　　　H = \begin{bmatrix}H_{xx} & H_{xd} \\ H_{dx} & H_{dd}\end{bmatrix}
　　\end{equation}　　

　　那么增量方程采用schur消元结果如下：

　　\begin{equation} \begin{aligned}
　　　　\begin{bmatrix}I & -H_{xd}H_{dd}^{-1} \\ 0 & I\end{bmatrix} \begin{bmatrix}H_{xx} & H_{xd} \\ H_{dx} & H_{dd}\end{bmatrix} \begin{bmatrix}\Delta x_{x} \\ \Delta x_{d}\end{bmatrix} =  -\begin{bmatrix}I & -H_{xd}H_{dd}^{-1} \\ 0 & I\end{bmatrix} \begin{bmatrix}b_{x} \\ b_{d}\end{bmatrix} \\ \Rightarrow \begin{bmatrix}(H_{xx} - H_{xd}H_{dd}^{-1}H_{dx})\Delta x_{x} \\ H_{dx}\Delta x_{x} + H_{dd}\Delta x_{d}\end{bmatrix} = -\begin{bmatrix}b_{x} - H_{xd}H_{dd}^{-1}b_{d} \\ b_{d}\end{bmatrix}
　　\end{aligned} \end{equation}

　　我们最终需要求解的增量方程就是下面这个：

　　\begin{equation} \begin{aligned}
　　　　 (H_{xx} - H_{xd}H_{dd}^{-1}H_{dx})\Delta x_{x} = b_{x} - H_{xd}H_{dd}^{-1}b_{d}
　　\end{aligned} \end{equation}

　　实际上，我们知道 ，而 可由 计算得到， 表示残差对位姿+光度仿射参数的雅克比， 表示残差对点的逆深度的雅克比。

　　知道了我们的终极目标以后，接下来就是把这个目标分解成一个个小问题。显然，我们需要求 和 ，下面我们就开始推导了。

---

 直接法雅克比推导

　　首先是推导 ，这里我们参考了涂金戈同学的博客直接法光度误差导数推导，将雅克比的计算过程重新梳理了一番。

光度仿射参数

　　由公式 $(2)$ 可知：

　　\begin{equation}
　　　　\frac{\partial r}{\partial a_{cr}} = -w I_{r}[p_{r}]
　　\end{equation}

　　\begin{equation}
　　　　\frac{\partial r}{\partial b_{cr}} = -w
　　\end{equation}

　　其实在金戈同学的博客中，他认为DSO的代码写错了，因为DSO中的代码对应的公式为：。在这个问题上，我和金戈同学的观点有点不太一样。特别是，金戈同学有过疑惑，这个地方到底是 对 的雅克比，还是 对 的雅克比？

　　实际上笔者认为这个地方应该问题不大，我们简单看看：

　　1. 如果是 对 的雅克比，那么结果应该就是 ；

　　2. 如果是  对 的雅克比，那么结果应该就是 。

　　我们仔细分析一下哈，如果说是第一种情况，那么代码中就不需要多一个 ，显然这个是多余的。如果是第二种情况，这里有个特例需要注意一下，我们在初始化阶段， 是为 的，因此可以退化为成DSO初始化代码中的形式，即 ，并且 。

　　值得注意的是，在后续 tracking阶段，DSO采用的都是对 的雅克比，即第二种情况。如果从代码的一致性来说，笔者也比较倾向第二种，初始化阶段这里也是第二种情况。欢迎有不同观点的读者朋友一起交流哈。

位姿

　　回顾一下公式 ，笔者之所以将其写得那么细致，目的是为了在这里求雅克比的时候，可以非常自然而然做链式法则。

　　\begin{equation} \begin{aligned}
　　　　\frac{\partial r}{\partial \xi_{cr}} &= \frac{\partial r}{\partial I_{c}}  \frac{\partial I_{c}}{\partial p_{c}} \frac{\partial p_{c}}{\partial X_{c}^{'}} \frac{\partial X_{c}^{'}}{\partial \xi_{cr}} \\ &= w \begin{bmatrix}g_{x} & g_{y} & 0\end{bmatrix} \begin{bmatrix}f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0\end{bmatrix}  \frac{\partial d_{c}X_{c}}{\partial \xi_{cr}}
　　\end{aligned} \end{equation}

　　值得注意的是， 表示点 在当前帧图像 中的梯度值。那么接下来，我们就剩最后一项要求了。

　　假设 ，那么，所以两个变量都跟 有关，因此求雅克比的时候需要我们需要展开：

　　\begin{equation}
　　　　\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}} = \frac{\partial d_{c}}{\partial \xi_{cr}}X_{c} + \frac{\partial X_{c}}{\partial \xi_{cr}}d_{c}
　　\end{equation}

　　在视觉SLAM十四讲中，李代数那一讲中介绍了基于左扰动模型时， 对李代数 求导的结果是 ，那么在这里我们就直接写结果了：

　　\begin{equation}
　　　　\frac{\partial X_{c}}{\partial \xi_{cr}} = \begin{bmatrix}1 & 0 & 0 & 0 & z_{c} & -y_{c} \\ 0 & 1 & 0 & -z_{c} & 0 & x_{c} \\ 0 & 0 & 1 & y_{c} & -x_{c} & 0 \end{bmatrix}
　　\end{equation}

　　我们知道 ，那么由链式法则可知：。综上，我们可以知道 的结果如下：

　　\begin{equation} \begin{aligned}
　　　　\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}} &= \frac{\partial d_{c}}{\partial \xi_{cr}}X_{c} + \frac{\partial X_{c}}{\partial \xi_{cr}}d_{c} \\ &= -\frac{1}{z_{c}^{2}} \begin{bmatrix}0 & 0 & x_{c}& x_{c}y_{c} & -x_{c}^{2} & 0 \\ 0 & 0 & y_{c} & y_{c}^{2} & -x_{c}y_{c} & 0 \\ 0 & 0 & z_{c} & y_{c}z_{c} & -x_{c}z_{c} & 0\end{bmatrix} + \frac{1}{z_{c}}\begin{bmatrix}1 & 0 & 0 & 0 & z_{c} & -y_{c} \\ 0 & 1 & 0 & -z_{c} & 0 & x_{c} \\ 0 & 0 & 1 & y_{c}& -x_{c} & 0\end{bmatrix} \\ &= \begin{bmatrix}\frac{1}{z_{c}} & 0 & -\frac{x_{c}}{z_{c}^{2}} & -\frac{x_{c}y_{c}}{z_{c}^{2}} & 1 + \frac{x_{c}^{2}}{z_{c}^{2}} & -\frac{y_{c}}{z_{c}} \\ 0 & \frac{1}{z_{c}} & -\frac{y_{c}}{z_{c}^{2}} & -1 - \frac{y_{c}^{2}}{z_{c}^{2}} & \frac{x_{c}y_{c}}{z_{c}^{2}} & \frac{x_{c}}{z_{c}} \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix}
　　\end{aligned} \end{equation}

　　由公式 我们知道， 是 的归一化坐标，我们假设 ，那么公式 可简化为：

　　\begin{equation} \begin{aligned}
　　　　\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}} &=  \begin{bmatrix}d_{c} & 0 & -d_{c}u_{c}^{'} & -u_{c}^{'}v_{c}^{'} & 1 + u_{c}^{'2} & -v_{c}^{'} \\ 0 & d_{c} & -d_{c}v_{c}^{'} & -1-v_{c}^{'2} & u_{c}^{'}v_{c}^{'} & u_{c}^{'} \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix}
　　\end{aligned} \end{equation}

　　联立公式 ，我们可以得到最终的雅克比 ：

　　\begin{equation} \begin{aligned}
　　　　J_{x} &= \begin{bmatrix}\frac{\partial r}{\partial \xi_{cr}} & \frac{\partial r}{\partial a_{cr}} & \frac{\partial r}{\partial b_{cr}}\end{bmatrix} \\ &= \begin{bmatrix}wg_{x}f_{x}d_{c} \\ wg_{y}f_{y}d_{c} \\ -d_{c}(wg_{x}f_{x}u_{c}^{'} + wg_{y}f_{y}v_{c}^{'}) \\ -wg_{x}f_{x}u_{c}^{'}v_{c}^{'}-wg_{y}f_{y}(1+v_{c}^{'2}) \\ wg_{x}f_{x}(1+u_{c}^{'2}) + wg_{y}f_{y}(u_{c}^{'}v_{c}^{'}) \\ -wg_{x}f_{x}v_{c}^{'} + wg_{y}f_{y}u_{c}^{'2} \\ -wa_{cr}I_{r}[p_{r}] \\ -w \end{bmatrix}^{T}
　　\end{aligned} \end{equation}

　　OK，至此，我们就把跟机器人本体状态相关的变量雅克比求解完成。接下来再把这个逆深度值的雅克比算一算，我们这一讲的内容就完整了。

逆深度

　　同样，我们参考公式 ，我们有 ，我们可以参考涂金戈同学的博客将 看成一个整体，定义为矩阵 ，实际上根据矩阵相乘的维度变化我们可以知道，最终 是三维的点。那么我们可以将公式 写成如下形式：

 　　\begin{equation} \begin{aligned}
　　　　p_{c} &= Kd_{c}(d_{r}^{-1}R_{cr}K^{-1}p_{r} + t_{cr}) \\ &= Kd_{c}(d_{r}^{-1}A + t_{cr}) \\ &= Kd_{c}\begin{bmatrix}d_{r}^{-1}a_{1} + t_{x} \\ d_{r}^{-1}a_{2} + t_{y} \\ d_{r}^{-1}a_{3} + t_{z}\end{bmatrix} \\ &= KX_{c}^{'}
　　\end{aligned} \end{equation}

　　殊途同归，残差对点的逆深度值雅克比可以采用链式法则求解：

　　\begin{equation} \begin{aligned}
　　　　\frac{\partial r}{\partial d_{r}} &= \frac{\partial r}{\partial I_{c}}  \frac{\partial I_{c}}{\partial p_{c}} \frac{\partial p_{c}}{\partial X_{c}^{'}} \frac{\partial X_{c}^{'}}{\partial d_{r}} \\ &= w \begin{bmatrix}g_{x} & g_{y} & 0\end{bmatrix} \begin{bmatrix}f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0\end{bmatrix}  \frac{\partial X_{c}^{'}}{\partial d_{r}}
　　\end{aligned} \end{equation}

　　OK，我们的最终目标就是求解 这个东西了。由公式 得到：

　　\begin{equation} \begin{aligned}
　　　　X_{c}^{'} &= d_{c}\begin{bmatrix}d_{r}^{-1}a_{1} + t_{cr}^{x} \\ d_{r}^{-1}a_{2} + t_{cr}^{y} \\ d_{r}^{-1}a_{3} + t_{cr}^{z}\end{bmatrix} = \begin{bmatrix}u_{c}^{'} \\ v_{c}^{'} \\ 1\end{bmatrix}
　　\end{aligned} \end{equation}

　　显然，我们可以得到 ，那么 可以记为：

　　\begin{equation} \begin{aligned}
　　　　\begin{bmatrix}u_{c}^{'} \\ v_{c}^{'}\end{bmatrix} = \begin{bmatrix}\frac{d_{r}^{-1}a_{1} + t_{cr}^{x}}{d_{r}^{-1}a_{3} + t_{cr}^{z}} \\ \frac{d_{r}^{-1}a_{2} + t_{cr}^{y}}{d_{r}^{-1}a_{3} + t_{cr}^{z}}\end{bmatrix}
　　\end{aligned} \end{equation}

　　接下来就是单变量的求导，笔者就不展开了，直接写结果：

　　\begin{equation} \begin{aligned}
　　　　\frac{\partial X_{c}^{'}}{\partial d_{r}} = \begin{bmatrix}d_{r}^{-1}d_{c}(t_{cr}^{x} - u_{c}^{'}t_{cr}^{z}) \\ d_{r}^{-1}d_{c}(t_{cr}^{y} - v_{c}^{'}t_{cr}^{z}) \\ 0\end{bmatrix}
　　\end{aligned} \end{equation}

　　那么根据链式法则，我们最终的雅克比矩阵应该是：

　　\begin{equation} \begin{aligned}
　　　　\frac{\partial r}{\partial d_{r}} &= \frac{\partial r}{\partial I_{c}} \frac{\partial I_{c}}{\partial p_{c}} \frac{\partial p_{c}}{\partial X_{c}^{'}} \frac{\partial X_{c}^{'}}{\partial d_{r}} \\ &= w \begin{bmatrix}g_{x} & g_{y} & 0\end{bmatrix} \begin{bmatrix}f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0\end{bmatrix}  \begin{bmatrix}d_{r}^{-1}d_{c}(t_{cr}^{x} - u_{c}^{'}t_{cr}^{z}) \\ d_{r}^{-1}d_{c}(t_{cr}^{y} - v_{c}^{'}t_{cr}^{z}) \\ 0\end{bmatrix} \\ &= wd_{r}^{-1}d_{c}(g_{x}f_{x}(t_{cr}^{x} - u_{c}^{'}t_{cr}^{z}) + g_{y}f_{y}(t_{cr}^{y} - v_{c}^{'}t_{cr}^{z}))
　　\end{aligned} \end{equation}

　　至此，两部分的雅克比我们都已经求出来了。那么上述的问题我们也算讲明白了。

总结

　　终于到了总结的阶段了，这一讲的篇幅相当大，笔者也写了很久，特别是公式特别多，写得相当不容易。

　　这一讲，我们主要讲了DSO的直接法运动估计，特别是高斯牛顿法优化。我们详细介绍了Schur消元法精简优化模型的方案，由于在跟踪阶段只需要估计相对运动，而不需要估计点的深度值，因此我们优化问题可以变得非常高效。

　　为了计算增量方程，我们推导了位姿和点逆深度的雅克比矩阵，这里我们通过公式 将整体的雅克比求解过程进行分阶段求解，通过链式法则将求解位姿和点逆深度的雅克比进行同构，只在最后一项有所区别，这也是一种比较有用的数学思想，通过将有共性的问题进行同构，使得解决方案更加简洁。

　　笔者参考了涂金戈同学的博客，也借鉴了部分推导流程，非常欢迎同学们去阅读金戈同学的博客园。

参考文献

[1] 直接法光度误差导数推导

[2] 视觉SLAM十四讲

[3] Jinge

版权说明

　　如需转载，请联系本人邮箱peichu.ye at mail2.gdut.edu.cn。未经允许，不可转载。