第 2 章 直接法、光流法

2.1 直接法的引出

特征点法缺点：

• 耗时：关键点的提取与描述子的计算非常耗时。
• 图像信息丢失：使用特征点时，忽略了除特征点以外的所有信息。只使用特征点丢弃了大部分可能有用的图像信息。
• 特征缺失：相机有时会运动到特征缺失的地方，这些地方没有明显的纹理信息，可能找不到足够的匹配点来计算相机运动。

光流法：

使用光流跟踪替换描述子匹配，估计相机运动仍使用对极几何、PnP、ICP 算法。

• 保留特征点，但只计算关键点，不计算描述子。
• 使用光流来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间。但光流本身的计算需要一定时间。

直接法：

根据图像的像素灰度信息（最小化广度误差）计算相机运动：

• 只计算关键点，不计算描述子。同时使用直接法计算特征点在下一时刻图像的位置。这样可以跳过描述子的计算过程，而且直接法的计算更加简单。
• 既不计算关键点，也不计算描述子，而是根据像素灰度的差异，直接计算相机运动。

直接法分类：

稀疏、稠密、半稠密。（特征点法只能重构稀疏特征点，构建稀疏地图）

2.2 光流法

2.2.1 光流

光流：一种描述像素随着时间，在图像之间运动的方法。计算部分像素运动的为稀疏光流，计算所有像素的为稠密光流。

![SLAM-VO2-1]()

2.2.2 LK 光流法推导

灰度不变假设：

同一个空间点的像素灰度值，在各个图像中是固定不变的。如$ t $时刻位于$ (x,y) $处的像素，$ t+\mathrm{t} $时刻运动到$ (x+\mathrm{d}x, y+\mathrm{d}y) $处，灰度不变：

$$
\boldsymbol{I}(x+\mathrm{d}x, y+\mathrm{d}y, t+\mathrm{d}t) = \boldsymbol{I}(x, y, t)
$$

该假设是一个很强的假设，实际当中很可能不成立。

推导：

• 进行一阶泰勒展开，并根据灰度不变假设，有以下结果

$$
\boldsymbol{I}(x+\mathrm{d}x, y+\mathrm{d}y, t+\mathrm{d}t) \approx \boldsymbol{I}(x,y,t) + \frac{\partial \boldsymbol{I}}{\partial x}\rm{d}x + \frac{\partial \boldsymbol{I}}{\partial y}\rm{d}y + \frac{\partial \boldsymbol{I}}{\partial t}\rm{d}t
$$

$$
\frac{\partial \boldsymbol{I}}{\partial x}\rm{d}x + \frac{\partial \boldsymbol{I}}{\partial y}\rm{d}y + \frac{\partial \boldsymbol{I}}{\partial t}\rm{d}t = 0
$$

$$
\frac{\partial \boldsymbol{I}}{\partial x}\frac{\rm{d}x}{\rm{d}t} + \frac{\partial \boldsymbol{I}}{\partial y}\frac{\rm{d}y}{\rm{d}t} = -\frac{\partial \boldsymbol{I}}{\partial t}
$$

• 记$ u=\rm{d}x/\rm{d}t $、$ v=\rm{d}y/\rm{d}t $分别为像素在两轴方向的速度；$ \boldsymbol{I}_x=\partial \boldsymbol{I} / \partial x $、$ \boldsymbol{I}_y=\partial \boldsymbol{I} / \partial y $分别为图像在该点处两轴方向上的梯度。$ \boldsymbol{I}_t $为图像灰度对时间的变化量。

$$
\begin{bmatrix} \boldsymbol{I}_x & \boldsymbol{I}_y \end{bmatrix} \begin{bmatrix} u \\ v \end{bmatrix} = -\boldsymbol{I}_t
$$

• 光流法的目标是跟踪特征点的运动，因此目标是求解$ u,v $。考虑一个大小为$ {\omega}^2 $的窗口，含有$ {\omega}^2 $数量的像素。该窗口内像素具有同样的运动：

$$
\boldsymbol{A} \begin{bmatrix} u\\v \end{bmatrix} = -\boldsymbol{b}
$$

$$
\boldsymbol{A}=\begin{bmatrix} {\begin{bmatrix} \boldsymbol{I}_x&\boldsymbol{I}_y \end{bmatrix}}_1 \\ \vdots \\ {\begin{bmatrix} \boldsymbol{I}_x&\boldsymbol{I}_y \end{bmatrix}}_k \end{bmatrix}, \quad \boldsymbol{b}=\begin{bmatrix} \boldsymbol{I}_{t1} \\ \vdots \\ \boldsymbol{I}_{tk} \end{bmatrix}, \quad k={\omega}^2
$$

• 利用最小二乘法，求解该超定线性方程。关于最小二乘法：http://www.dengke.xyz/archives/2573。

$$
{\begin{bmatrix} u \\ v \end{bmatrix}} = -(\boldsymbol{A}^T \boldsymbol{A})^{-1}\boldsymbol{A}^T \boldsymbol{b}
$$

2.3 直接法

2.3.1 推导

目的：

求第一帧到第二帧的相对位姿变换。

• 世界坐标$ P=[X,Y,Z] $
• 非齐次像素坐标$ \boldsymbol{p}_1,\boldsymbol{p}_2 $

![SLAM-VO2-2]()

优化目标及求导：

优化相机的位姿，来寻找与$ \boldsymbol{p}_1 $更相似的$ \boldsymbol{p}_2 $。（因为没有特征匹配，无法得知哪一个$ \boldsymbol{p}_1 $和$ \boldsymbol{p}_2 $对应同一个空间点）

• 定义光度误差：

$$
e = \boldsymbol{I}_1 (\boldsymbol{p}_1) - \boldsymbol{I}_2 (\boldsymbol{p}_2)
$$

$$
\begin{cases}
\boldsymbol{p}_1 = {\begin{bmatrix} u\\v\\1 \end{bmatrix}}_1 = \frac{1}{Z_1}\boldsymbol{KP} \\
\boldsymbol{p}_2 = {\begin{bmatrix} u\\v\\1 \end{bmatrix}}_2 = \frac{1}{Z_2}\boldsymbol{K}(\boldsymbol{RP}+\boldsymbol{t}) = \frac{1}{Z_2}\boldsymbol{K}(\mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P})_{1:3}
\end{cases}
$$

• 定义优化目标：优化相机位姿，使得光度误差最小（依赖灰度不变假设）。

$$
\min_{\boldsymbol{\xi}}J(\boldsymbol{\xi}) = \min_{\boldsymbol{\xi}}||e||^2 = \min_{\boldsymbol{\xi}} \sum_{i=1}^N e_i^T e_i
$$

• 建立导数关系：根据非线性优化方法求解上述最小二乘问题，需要用到导数。参考 BA 问题，使用李代数的扰动模型求导，给$ \mathrm{exp}(\boldsymbol{\xi}) $左乘扰动$ \mathrm{exp}(\delta \boldsymbol{\xi}) $：
  • 展开

$$
\begin{aligned} e(\boldsymbol{\xi} \bigoplus \delta \boldsymbol{\xi}) &= \boldsymbol{I}_1(\frac{1}{Z_1}\boldsymbol{KP}) - \boldsymbol{I}_2(\frac{1}{Z_2}\boldsymbol{K}\mathrm{exp}(\delta \boldsymbol{\xi}^{\wedge}) \mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P}) \\ &\approx \boldsymbol{I}_1(\frac{1}{Z_1}\boldsymbol{KP}) - \boldsymbol{I}_2(\frac{1}{Z_2}\boldsymbol{K}(1+\delta \boldsymbol{\xi}^{\wedge}) \mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P}) \\ &= \boldsymbol{I}_1(\frac{1}{Z_1}\boldsymbol{KP}) - \boldsymbol{I}_2(\frac{1}{Z_2}\boldsymbol{K}\mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P} + \frac{1}{Z_2}\boldsymbol{K} \delta \boldsymbol{\xi}^{\wedge} \mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P}) \end{aligned}
$$

• 记$ \boldsymbol{q,u} $，其中$ \boldsymbol{q} $为$ \boldsymbol{P} $在扰动后，位于第二个相机坐标系下的坐标；而为$ \boldsymbol{u} $它的像素坐标。代入上式并进行一阶泰勒展开：

$$
\boldsymbol{q}=\delta \boldsymbol{\xi}^{\wedge} \mathrm{exp}(\boldsymbol{\xi}^{\wedge}) \boldsymbol{P}, \quad \boldsymbol{u}=\frac{1}{Z_2}\boldsymbol{Kq}
$$

$$
\begin{aligned} e(\boldsymbol{\xi} \bigoplus \delta \boldsymbol{\xi}) &= \boldsymbol{I}_1(\frac{1}{Z_1}\boldsymbol{KP}) - \boldsymbol{I}_2(\frac{1}{Z_2}\boldsymbol{K}\mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P} + \boldsymbol{u}) \\ & \approx \boldsymbol{I}_1(\frac{1}{Z_1}\boldsymbol{KP}) - \boldsymbol{I}_2(\frac{1}{Z_2}\boldsymbol{K}\mathrm{exp}(\boldsymbol{\xi}^{\wedge})\boldsymbol{P}) - \frac{\partial \boldsymbol{I}_2}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} \delta \boldsymbol{\xi} \\ &= e(\boldsymbol{\xi}) - \frac{\partial \boldsymbol{I}_2}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} \delta \boldsymbol{\xi} \end{aligned}
$$

• 求解 3 项偏导数：参考 BA 问题，根据上式得出的扰动结果即可求出导数，其中上式右侧三项偏导数分别如下。
  • $ \frac{\partial \boldsymbol{I}_2}{\partial \boldsymbol{u}} $：$ \boldsymbol{u} $处的像素梯度。
  • $ \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} $：投影方程关于相机坐标系下的三维点的导数。记$ \boldsymbol{q}=[X,Y,Z]^T $，根据 BA 问题中的推导，该偏导为：

$$
\frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} = \begin{bmatrix} \frac{\partial u}{\partial X} & \frac{\partial u}{\partial X} & \frac{\partial u}{\partial X} \\ \frac{\partial u}{\partial X} & \frac{\partial u}{\partial X} & \frac{\partial u}{\partial X} \end{bmatrix} = \begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2} \\ 0 & \frac{f_y}{Z} & -\frac{f_y Y}{Z^2} \end{bmatrix}
$$

• $ \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} $：变换后的三维点对变换的导数，根据李代数扰动模型求导结果，且同 BA 问题中只取前三维，可得

$$
\frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} = [\boldsymbol{I},-\boldsymbol{q}^{\wedge}]
$$

• 3 项偏导的后两项只与三维点$ \boldsymbol{q} $有关，而与图像无关，相乘合并。得到优化问题中的雅克比矩阵。

$$
\frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}} = -\begin{bmatrix} \frac{f_x}{Z^{\prime}} & 0 & -\frac{f_x X^{\prime}}{Z^{\prime 2}} & -\frac{f_x X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_x+\frac{f_x X^2}{Z^{\prime 2}} & -\frac{f_x Y^{\prime}}{Z^{\prime}} \\ 0 & \frac{f_y}{Z^{\prime}} & -\frac{f_y Y^{\prime}}{Z^{\prime 2}} & -f_y-\frac{f_y Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_y X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_y X^{\prime}}{Z^{\prime 2}} \end{bmatrix}
$$

$$
\boldsymbol{J} = -\frac{\partial \boldsymbol{I}_2}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}
$$

• 对于 N 个点的问题，可使用上述方法得到优化问题的雅克比矩阵，然后使用 G-N 或 L-M 方法计算增量，迭代求解。

2.3.2 总结

分类：

直接法推导中$ P $是一个已知位置的空间点，。根据$ P $的来源，直接法可分为以下几类：

• 稀疏直接法：$ P $点来自稀疏关键点。通常使用数百个至上千个关键点，并且像 L-K 光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度最快，但只能计算稀疏的重构。
• 半稠密直接法：$ P $来自部分像素。如果像素梯度为零，上述雅可比就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。这称之为半稠密（Semi-Dense）的直接法，可以重构一个半稠密结构。
• 稠密直接法：$ P $为所有像素。P 为所有像素，称为稠密直接法。稠密重构需要计算所有像素（一般几十万至几百万个），因此多数不能在现有的 CPU 上实时计算，需要 GPU 的加速。但是，如前面所讨论的，梯度不明显的点，在运动估计中不会有太大贡献，在重构时也会难以估计位置

优点：

• 可以省去计算特征点、描述子的时间。
• 只要求有像素梯度即可，无须特征点。因此，直接法可以在特征缺失的场合下使用。比较极端的例子是只有渐变的一张图像。它可能无法提取角点类特征，但可以用直接法估计它的运动。
• 可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。

缺点：

• 非凸性：直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。
• 单个像素没有区分度：找一个和他像的实在太多了！于是要么计算图像块，要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致。只能少数服从多数，以数量代替质量。
• 灰度值不变是很强的假设：如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时亦会出现这种情况。特征点法对光照具有一定的容忍性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败。针对这一点，目前的直接法开始使用更细致的光度模型标定相机，以便在曝光时间变化时也能让直接法工作。