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中在直接法运动估计中，这个高斯牛顿法是怎样操作的。
直接法运动估计
在前面我们提到过，直接法不同于特征点法，可以通过特征匹配来构建数据关联关系。那么直接法通常的做法呢，是将两个参考图像中提取到的梯度点全部投影到当前图像中，通过调整位姿和光度仿射模型使得两帧图像间的光度误差值最小。
为了更好地描述问题，我们会采用数学符号来完成这一讲的内容。在此之前呢，我们要先定义一下符号。（昨天吃饭的时候才刚跟范帝楷聊过这个事情）
假设我们当前有参考帧 $I_{r}$ 和当前帧 $I_{c}$，参考帧中的光度仿射参数为 $(a_{r}, b_{r})$， 同理，当前帧的为 $(a_{c}, b_{c})$。相机内参数矩阵为 $K$，由参考帧 $I_{r}$ 的坐标系到当前帧 $I_{c}$ 的坐标系相对变换的李代数为 $\xi_{cr}$，并且我们有：
\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}
其中，点 $p_{r} = [u_{r}, v_{r}, 1]^{T} $ 为参考帧 $I_{r}$ 中梯度较大的点，$p_{c} = [u_{c}, v_{c}, 1]^{T}$ 表示点 $p_{r}$ 在当前帧 $I_{c} $ 中的反投影坐标。具体计算方法：
\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}
在公式 $(3)$ 中，我们定义了一些新的符号：
1. 点 $p_{r}$ 在参考帧相机坐标系中的点表示为 $X_{r} = d_{r}^{-1}K^{-1}p_{r}$；
2. 点 $p_{r}$ 转换到当前帧相机坐标系中的点表示为 $X_{c} = R_{cr}X_{r} + t_{cr} = exp(\xi^{\wedge}_{cr})X_{r}$；
3. 点 $p_{r}$ 在当前帧相机坐标系中的归一化坐标表示为 $X_{c}^{'} = d_{c}X_{c}$；
4. 点 $p_{r}$ 在当前帧相机平面上的像素点坐标表示为 $p_{c} = KX_{c}^{'}$。
值得注意的是，公式 $(3)$ 中我们忽略了部分齐次坐标的转换，这些转换尽管影响不大，但是还是希望读者们能够知道这里哪些地方的维度发生了变化。
OK，接下来我们开始讨论对于上述定义的光度误差函数，我们会用什么方式来优化？
我们假设待优化的变量为 $x = [\xi_{cr}, a_{cr}, b_{cr}, d]$。其中 $d$ 为单一一个点的逆深度值，对于单个点的能量函数，我们有：
\begin{equation}
E(x) = r(x)^{T}r(x)
\end{equation}
实际上，残差的权重已经是经过Huber核函数决策得到的了。DSO论文中提到一个关于梯度的权重，这个权重是用在滑窗优化中的，在跟踪阶段并不涉及。因此我们不做讨论。
对于公式 $(4)$ 中的能量函数 $E(x)$，我们采用高斯-牛顿法进行迭代优化，每次添加一个增量 $\Delta x$，通过泰勒展开可以得到：
\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}
其中，$J$ 表示残差 $r$ 对变量 $x$ 的雅克比矩阵 $\frac{\partial r}{\partial x}$。
通过对增量 $\Delta x$ 进行求导，并令导数结果为 $0$，我们可以得到：
\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消元法来简化上述的优化问题。我们用下标 $x$ 来表示位姿+光度仿射参数，用下标 $d$ 表示点的逆深度信息，那么我们可以将增量方程进行分块：
\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}
实际上，我们知道 $H_{xd} = H_{dx}^{T}$，而 $H_{xd}$ 可由 $J_{x}^{T}J_{d}$ 计算得到，$J_{x}$ 表示残差对位姿+光度仿射参数的雅克比，$J_{d}$ 表示残差对点的逆深度的雅克比。
知道了我们的终极目标以后，接下来就是把这个目标分解成一个个小问题。显然，我们需要求 $J_{x}$ 和 $J_{d}$，下面我们就开始推导了。
直接法雅克比推导
首先是推导 $J_{x}$，这里我们参考了涂金戈同学的博客直接法光度误差导数推导，将雅克比的计算过程重新梳理了一番。
光度仿射参数 $(a, b)$
由公式 $(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中的代码对应的公式为：$\frac{\partial r}{\partial a_{cr}} = -wa_{cr}I_{r}[p_{r}]$。在这个问题上，我和金戈同学的观点有点不太一样。特别是，金戈同学有过疑惑，这个地方到底是 $r$ 对 $a_{cr}$ 的雅克比，还是 $r$ 对 $a_{c}$ 的雅克比？
实际上笔者认为这个地方应该问题不大，我们简单看看：
1. 如果是 $r$ 对 $a_{cr}$ 的雅克比，那么结果应该就是 $\frac{\partial r}{\partial a_{cr}} = -w I_{r}[p_{r}]$
2. 如果是 $r$ 对 $a_{c}$ 的雅克比，那么结果应该就是 $\frac{\partial r}{\partial a_{cr}} = -wa_{cr}(I_{r}[p_{r}] - b_{r})$
我们仔细分析一下哈，如果说是第一种情况，那么代码中就不需要多一个 $a_{cr}$，显然这个是多余的。如果是第二种情况，这里有个特例需要注意一下，我们在初始化阶段，$b_{r}$ 是为 $0$ 的，因此可以退化为成DSO初始化代码中的形式，即 $\frac{\partial r}{\partial a_{c}} = -wa_{cr}(I_{r}[p_{r}] - b_{r}) = -wa_{cr}I_{r}[p_{r}]$，并且 $\frac{\partial r}{\partial b_{c}} = \frac{\partial (-w(b_{c} + a_{cr}b_{r}))}{\partial b_{c}} = -w$。
值得注意的是，在后续 tracking阶段，DSO采用的都是对 $a_{c}$ 的雅克比，即第二种情况。如果从代码的一致性来说，笔者也比较倾向第二种，初始化阶段这里也是第二种情况。欢迎有不同观点的读者朋友一起交流哈。
位姿 $\xi_{cr}$
回顾一下公式 $(3)$，笔者之所以将其写得那么细致，目的是为了在这里求雅克比的时候，可以非常自然而然做链式法则。
\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}
值得注意的是，$g_{x}, g_{y}$ 表示点 $p_{c}$ 在当前帧图像 $I_{c}$ 中的梯度值。那么接下来，我们就剩最后一项要求了。
假设 $X_{c} = \begin{bmatrix}x_{c} & y_{c} & z_{c} \end{bmatrix}^{T}$，那么$d_{c} = \frac{1}{z_{c}}$，所以两个变量都跟 $\xi_{cr}$ 有关，因此求雅克比的时候需要我们需要展开：
\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十四讲中，李代数那一讲中介绍了基于左扰动模型时， $Tp$ 对李代数 $\xi$ 求导的结果是 $(Tp)^{\odot}$，那么在这里我们就直接写结果了：
\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}
我们知道 $d_{c} = \frac{1}{z_{c}}$，那么由链式法则可知：$\frac{\partial d_{c}}{\partial \xi_{rc}} = \frac{\partial d_{c}}{\partial z_{c}} \frac{\partial z_{c}}{\partial \xi_{cr}}$。综上，我们可以知道 $\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}}$ 的结果如下：
\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}
由公式 $(3)$ 我们知道， $X_{c}^{'}$ 是 $X_{c}$ 的归一化坐标，我们假设 $X_{c}^{'} = \begin{bmatrix}u_{c}^{'} \\ v_{c}^{'} \\ 1\end{bmatrix}$，那么公式 $(15)$ 可简化为：
\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}
联立公式 $(10-12)(16)$，我们可以得到最终的雅克比 $J_{x}$：
\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，至此，我们就把跟机器人本体状态相关的变量雅克比求解完成。接下来再把这个逆深度值的雅克比算一算，我们这一讲的内容就完整了。
逆深度 $d_{r}$
同样，我们参考公式 $(3)$，我们有 $p_{c} = Kd_{c}(d_{r}^{-1}R_{cr}K^{-1}p_{r} + t_{cr})$，我们可以参考涂金戈同学的博客将 $R_{cr}K^{-1}p_{r}$ 看成一个整体，定义为矩阵 $A$，实际上根据矩阵相乘的维度变化我们可以知道，最终 $A$ 是三维的点。那么我们可以将公式 $(3)$ 写成如下形式：
\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，我们的最终目标就是求解 $\frac{\partial X_{c}^{'}}{\partial d_{r}}$ 这个东西了。由公式 $(18)$得到：
\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}
显然，我们可以得到 $d_{c} = (d_{r}^{-1}a_{3} + t_{cr}^{z})^{-1}$，那么 $u_{c}^{'}，v_{c}^{'}$ 可以记为：
\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消元法精简优化模型的方案，由于在跟踪阶段只需要估计相对运动，而不需要估计点的深度值，因此我们优化问题可以变得非常高效。
为了计算增量方程，我们推导了位姿和点逆深度的雅克比矩阵，这里我们通过公式 $(3)$ 将整体的雅克比求解过程进行分阶段求解，通过链式法则将求解位姿和点逆深度的雅克比进行同构，只在最后一项有所区别，这也是一种比较有用的数学思想，通过将有共性的问题进行同构，使得解决方案更加简洁。
笔者参考了涂金戈同学的博客，也借鉴了部分推导流程，非常欢迎同学们去阅读金戈同学的博客园。
参考文献
[1] 直接法光度误差导数推导
[2] 视觉SLAM十四讲
[3] Jinge
版权说明
如需转载，请联系本人邮箱peichu.ye at mail2.gdut.edu.cn。未经允许，不可转载。
