[SLAM基础学习简记]视觉里程计-直接法

视觉里程计-直接法

基于《视觉SLAM14讲》以及深蓝学院SLAM课程的学习笔记

引出

特征点法的缺点

• 耗时
• 丢弃了大部分可能有用的信息
• 运动到特征缺失的地方会失效

解决方法：

• 保留特征点，但只计算关键点，不计算描述子：光流法（Optical Flow）
• 只计算关键点，不计算描述子：直接法（Direct Method）

2D光流

光流是一种描述像素随时间在图像之间运动的方法。随着时间的流逝，同一个像素会在图像中运动，而我们希望追踪它的运动过程。

计算部分像素运动称为稀疏光流（如LK光流），计算所有像素运动为稠密光流

Lucas-Kanade光流

认为相机图像随时间变化，图像可以看作时间与位置的函数$\mathbf{I}(t)$，在$t$时刻，位于$(x,y)$的像素，灰度可以写成：
$$\mathbf{I}(x,y,t)$$
希望估计空间点在其他时刻图像中的位置，引入灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

有方程：
$$\left[\begin{array}{cc}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-{\mathbf{I}}_t$$
式中，${\mathbf{I}}_x=\frac{d\mathbf{I}}{dx},{\mathbf{I}}_y=\frac{d\mathbf{I}}{dy}$为图像在一点$x,y$方向的梯度，${\mathbf{I}}_t$为图像灰度对时间的变化量，$u=\frac{dx}{dt},v=\frac{dy}{dt}$为像素在两个方向的运动速度。要计算$u,v$，需要引入额外约束。

因此，假设某一窗口内的像素具有相同的运动。考虑大小为$w\times w$的窗口，该窗口内像素有同样的运动，因此有$w^2$个方程：
$${\left[\begin{array}{cc}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]}_k\left[\begin{array}{c}u\\ v\end{array}\right]=-{\mathbf{I}}_{tk},k=1,\dots ,w^2$$
记
$$\mathbf{A}=\left[\begin{array}{c}{\left[\begin{array}{cc}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]}_1\\ ⋮\\ {\left[\begin{array}{cc}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]}_k\end{array}\right],\mathbf{b}=\left[\begin{array}{c}{\mathbf{I}}_{t1}\\ ⋮\\ {\mathbf{I}}_{tk}\end{array}\right]$$
于是整个方程为
$$\mathbf{A}\left[\begin{array}{c}u\\ v\end{array}\right]=-\mathbf{b}$$
这是一个超定线性方程，可以求最小二乘解：
$${\left[\begin{array}{c}u\\ v\end{array}\right]}^{\ast }=-({\mathbf{A}}^T\mathbf{A}{)}^{-1}{\mathbf{A}}^T\mathbf{b}$$

得到了像素在图像间的运动速度$u,v$，从而可以估计某块像素在若干个图像中出现的位置。

单层光流与多层光流

把光流看成一个优化问题：通过最小化灰度误差估计最优的像素偏移。其具体函数实现中的优化问题为：
$$\underset{\Delta x,\Delta y}{\min }\Vert {\mathbf{I}}_1(x,y)-{\mathbf{I}}_2(x+\Delta x,y+\Delta y){\Vert }_2^2$$
如果相机运动较快，两张图像差异较明显，那么单层图像光流法容易达到一个局部极小值。可以引入图像金字塔来改善。在计算光流时，先从顶层的图像开始计算，然后把上一层的追踪结果，作为下一层的初始值。也称为**由粗到精（Coarse-to-fine）**的光流。

LK光流跟踪能够直接得到特征点的对应关系。这个对应关系就像是描述子的匹配，只是光流对图像的连续性和光照稳定性要求更高一些。我们可以通过光流跟踪的特征点，用PnP、ICP或对极几何来估计相机运动。

如果角点提的位置不好，光流也容易跟丢或给出错误的结果。

直接法

直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子。

相比于光流法和特征点法，知道点之间的匹配关系，直接法通过优化相机位姿来寻找点的位置。最小化的是两个点像素之间的亮度误差。

根据当前相机位姿估计值和${\mathbf{p}}_1$来寻找${\mathbf{p}}_2$，直接法通过最小化光度误差来优化相机的位姿，使得${\mathbf{p}}_1$和${\mathbf{p}}_2$差异尽可能小：
$$e={\mathbf{I}}_1({\mathbf{p}}_1)-{\mathbf{I}}_2({\mathbf{p}}_2)$$
式中，
$$\begin{gathered} {\mathbf{p}}_1={\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]}_1=\frac{1}{Z_1}\mathbf{K}\mathbf{P} \\ {\mathbf{p}}_2={\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]}_2=\frac{1}{Z_2}\mathbf{K}(\mathbf{R}\mathbf{P}+\mathbf{t}) \end{gathered}$$
基于灰度不变假设。优化目标为误差的二范数：
$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\Vert e{\Vert }^2$$
对$N$个空间点${\mathbf{P}}_i$，有
$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\sum _{i=1}^N{e}_i^T{e}_i,e_i={\mathbf{I}}_1({\mathbf{p}}_1,i)-{\mathbf{I}}_2({\mathbf{p}}_2,i)$$
引入中间变量，分别是$P$在第二个相机坐标系下的坐标$\mathbf{q}$和像素坐标$\mathbf{u}$：
$$\begin{gathered} \mathbf{q}=\mathbf{T}\mathbf{P} \\ \mathbf{u}=\frac{1}{Z_2}\mathbf{K}\mathbf{q} \end{gathered}$$
考虑李代数的左扰动模型，推导出误差关于李代数的雅可比矩阵：
$$\mathbf{J}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}$$
其中，$\delta \mathbf{\xi }$是$\mathbf{T}$的左扰动。$\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}$为$\mathbf{u}$处的像素梯度。
$$\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}=\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}=\left[\begin{array}{cccccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}& \frac{f_{x}XY}{Z^2}& f_x+\frac{f_x{X}^2}{Z^2}& -\frac{f_{x}Y}{Z}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}& -f_y-\frac{f_y{Y}^2}{Z^2}& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z}\end{array}\right]$$

直接法的讨论

从直接法的推导可见，需要已知空间点$P$的位置。根据$P$的来源可以将直接法分为：

• 稀疏直接法：$P$来自于稀疏关键点，不必计算描述子，速度最快
• 半稠密（Semi-Dense）直接法：只使用带有梯度的像素点（因为梯度像素为0时，整项雅可比矩阵就为0，不会对计算运动增量有任何贡献）
• 稠密直接法：需要计算所有像素，需要GPU加速计算