《视觉SLAM十四讲》-- 视觉里程计2

07 视觉里程计 2

7.1 直接法的引出

特征点的缺点：

• 关键点的提取与描述子的计算非常耗时，实时性差；

• 使用特征点时，忽略了特征点以外的所有信息，而丢弃了部分可能有用的信息；

• 有时会出现特征缺失的情况，如白墙或者空荡荡的走廊等。

为了克服上述问题，提出了 光流法 和 直接法。

光流法：只计算关键点，不计算描述子，采用光流跟踪。

直接法：由光流法演变而来。根据像素灰度信息同时估计相机运动和点的投影，不要求必须为角点，甚至可以是随机点。

7.2 2D 光流

光流描述了像素随时间在图像之间运动的过程。其中，计算部分像素的运动称为 稀疏光流，计算所有像素的运动称为 稠密光流。下面主要介绍以 LK 光流为代表的稀疏光流。

7.2.1 Lucas-Kanade 光流

（1）我们认为图像是随时间变化的，也就是说图像可以看做时间的函数，那么，一个在 $t$ 时刻，位于 $(x,y)$ 处的像素，它的灰度可以写成

$$\mathbf{I}(x,y,t)$$

首先，引入光流法的基本假设：

灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

（2）对于 $t$ 时刻位于 $(x,y)$ 处的像素，在 $t+\mathrm{d}t$ 时刻运动到 $(x+\mathrm{d}x,y+\mathrm{d}y)$ 处，根据灰度不变假设

$$\mathbf{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)=\mathbf{I}(x,y,t)\text{\hspace{1em}(7-1)}$$

对左边进行泰勒展开，并保留一阶项：

$$\mathbf{I}(x+\mathrm{d}t,y+\mathrm{d}y,t+\mathrm{d}t)\approx \mathbf{I}(x,y,t)+\frac{\partial \mathbf{I}}{\partial x}\mathrm{d}x+\frac{\partial \mathbf{I}}{\partial y}\mathrm{d}y+\frac{\partial \mathbf{I}}{\partial t}\mathrm{d}t\text{\hspace{1em}(7-2)}$$

由灰度不变，所以

$$\frac{\partial \mathbf{I}}{\partial x}\mathrm{d}x+\frac{\partial \mathbf{I}}{\partial y}\mathrm{d}y+\frac{\partial \mathbf{I}}{\partial t}\mathrm{d}t=0\text{\hspace{1em}(7-3)}$$

两边同除 $\mathrm{d}t$，得

$$\frac{\partial \mathbf{I}}{\partial x}\frac{\mathrm{d}x}{\mathrm{d}t}+\frac{\partial \mathbf{I}}{\partial y}\frac{\mathrm{d}y}{\mathrm{d}t}=-\frac{\partial \mathbf{I}}{\partial t}\text{\hspace{1em}(7-3)}$$

其中，$\mathrm{d}x/\mathrm{d}t$、$\mathrm{d}y/\mathrm{d}t$ 分别为像素在 $x$、$y$ 轴上运动的速度，记为 $u$、$v$；$\partial \mathbf{I}/\partial x$、$\partial \mathbf{I}/\partial y$ 分别为图像在 $x$、$y$ 方向上的梯度，记为 ${\mathbf{I}}_x$、${\mathbf{I}}_y$；把像素灰度对时间的变化量记为 ${\mathbf{I}}_t$。那么上式写成矩阵形式

$$\left[\begin{array}{ll}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]\left[\begin{array}{l}u\\ v\end{array}\right]=-{\mathbf{I}}_t\text{\hspace{1em}(7-4)}$$

我们希望求出 $u$ 和 $v$，但上式是一个二元一次方程，条件不足，因此还需引入额外的约束。假设在一个大小为 $w\times w$ 的图像窗口中，这 $w^2$ 个像素都具有相同的运动，那么就可以得到 $w^2$ 个方程：

$${\left[\begin{array}{ll}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]}_k\left[\begin{array}{l}u\\ v\end{array}\right]=-{\mathbf{I}}_{tk}，k=1,2,...,w^2\text{\hspace{1em}(7-5)}$$

令

$$\mathbf{A}=\left[\begin{array}{c}{\left[{\mathbf{I}}_x,{\mathbf{I}}_y\right]}_1\\ ⋮\\ {\left[{\mathbf{I}}_x,{\mathbf{I}}_y\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}{l}u\\ v\end{array}\right]=-b\text{\hspace{1em}(7-6)}$$

这又是一个超定方程，采用最小二乘解，即

$${\left[\begin{array}{l}u\\ v\end{array}\right]}^{\ast }=-({\mathbf{A}}^{\mathrm{T}}\mathbf{A}{)}^{-1}{\mathbf{A}}^{\mathrm{T}}\mathbf{b}\text{\hspace{1em}(7-7)}$$

7.2.1 实践：LK 光流

7.3 直接法

7.3.1 推导过程

如图，已知空间点 $P$ 的世界坐标为 $[X,Y,Z{]}^{\mathrm{T}}$，在两帧图像上的像素坐标分别为 ${\mathbf{p}}_1$、${\mathbf{p}}_2$（未知）。

我们希望求出第一个相机到第二个相机的相对位姿变换，以第一个相机为初始参照系，经旋转和平移 $\mathbf{R}$、$\mathbf{t}$ 到第二个相机，有

$${\mathbf{p}}_1={\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]}_1=\frac{1}{Z_1}\mathbf{KP}$$
$${\mathbf{p}}_2={\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]}_2=\frac{1}{Z_2}\mathbf{K}(\mathbf{RP}+\mathbf{t})=\frac{1}{Z_2}\mathbf{K}(\mathbf{TP}{)}_{1:3}\text{\hspace{1em}(7-8)}$$

直接法中，由于没有特征点匹配，我们无法知道哪一个 ${\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)\text{\hspace{1em}(7-9)}$$

注意，这里的 $e$ 是标量。目标优化函数为

$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\Vert e{\Vert }^2\text{\hspace{1em}(7-10)}$$

假设一个空间点在各个视角下成像的灰度是不变的，我们有许多个空间点 $P_i$，那么，整个相机位姿估计问题变为

$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\sum _{i=1}^N{e}_i^{\mathrm{T}}{e}_i,e_i={\mathbf{I}}_1\left({\mathbf{p}}_{1,i}\right)-{\mathbf{I}}_2\left({\mathbf{p}}_{2,i}\right)\text{\hspace{1em}(7-11)}$$

定义

$$\mathbf{q}=\mathbf{TP}$$
$$\mathbf{u}=\frac{1}{Z}\mathbf{Kq}$$

那么，误差 $e$ 关于 位姿 $\mathbf{T}$ 的导数为

$$\frac{\partial e}{\partial \mathbf{T}}=\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}\delta \mathbf{\xi }\text{\hspace{1em}(7-12)}$$

其中，$\delta \mathbf{\xi }$ 为 $\mathbf{T}$ 的左扰动。分别看每一项：

（1）第一项 $\partial {\mathbf{I}}_2/\partial \mathbf{u}$ 为 $\mathbf{u}$ 处的像素梯度；

（2）第二项 $\partial \mathbf{u}/\partial \mathbf{q}$，也就是像素坐标关于相机坐标的导数，记 $\mathbf{q}=[X,Y,Z{]}^{\mathrm{T}}$，则有

$$\frac{\partial \mathbf{u}}{\partial \mathbf{q}}=\left[\begin{array}{lll}\frac{\partial u}{\partial X}& \frac{\partial u}{\partial Y}& \frac{\partial u}{\partial Z}\\ \frac{\partial v}{\partial X}& \frac{\partial v}{\partial Y}& \frac{\partial v}{\partial Z}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\end{array}\right]\text{\hspace{1em}(7-13)}$$

（3）第三项 $\partial \mathbf{q}/\partial \delta \mathbf{\xi }$ 为变换后的三维点对李代数的导数，前面已经推导

$$\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}=\left[\mathbf{I},-{\mathbf{q}}^{\wedge }\right]\text{\hspace{1em}(7-14)}$$

将后两式合并（见式（6-43）），得

$$\frac{\partial \mathbf{u}}{\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]\text{\hspace{1em}(7-15)}$$

于是，推导出误差关于李代数的雅克比矩阵：

$$\mathbf{J}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}\text{\hspace{1em}(7-16)}$$

采用迭代优化的方法求解出式（7-11）的最优解。

7.3.2 直接法的优缺点

（1）直接法优点

• 可以省去计算特征点、描述子的时间；

• 只需要像素梯度即可，不需要特征点；

• 可构件半稠密乃至稠密地图，这是特征点法做不到的。

（2）直接法缺点

• 非凸性；

• 单个像素没有区分度：选点少时效果较差，一般用 500 个点以上；

• 灰度不变假设是很强的假设，相机会自动调整曝光参数、或者光照变化，都会使得图像整体亮度发生变化。