《视觉SLAM十四讲》学习笔记-光流法原理

直接法的根源

特征点法存在的问题：
- 关键点与描述子计算非常耗时；
- 忽略除特征点外的其他所有信息；
- 如何处理特征缺失的问题。

克服特征点法的几种思路：
* 保留特征点，只计算关键点，不计算描述子，用光流法跟踪特征点的运动；
* 保留特征点，只计算关键点，不计算描述子，用直接法计算特征点在下一时刻图像的位置；
* 既不计算关键点也不计算描述子，根据像素灰度的差异直接计算相机运动。

后两种为直接法.

光流法(Optical Flow)

分为两种：
- 计算部分像素运动的：稀疏光流，以Lucas-Kanade为代表。
- 计算所有像素运动的：稠密光流。

Lucas-Kanade光流原理

前提：同一个空间点的像素灰度值，在各个图像中是固定不变的(灰度不变假设).
$t$时刻位于$(x,y)$处的像素，设$t+dt$的位置为$(x+dt, y+dt)$,依假设条件有：

$$\mathbf{I}(x+dx, y+dy, t+dt) = \mathbf{I}(x, y, t)$$
对左边进行泰勒展开，有：
$$\mathbf{I}(x+dx, y+dy, t+dt) \approx \mathbf{I}(x, y, t) + \frac{\partial \mathbf{I} }{\partial x} dx + \frac{\partial \mathbf{I} }{\partial y} dy+ \frac{\partial \mathbf{I} }{\partial t} dt$$
依据假设条件，有：
$$\frac{\partial \mathbf{I} }{\partial x} dx + \frac{\partial \mathbf{I} }{\partial y} dy+ \frac{\partial \mathbf{I} }{\partial t} dt = 0$$
整理得到：
$$\frac{\partial \mathbf{I} }{\partial x} \frac{dx}{dt} + \frac{\partial \mathbf{I} }{\partial y} \frac{dy}{dt} =- \frac{\partial \mathbf{I} }{\partial t}$$
不妨记：
$$\frac{dx}{dt}=u, ~~~\frac{dy}{dt} =v, ~~~ \frac{\partial \mathbf{I} }{\partial x} = \mathbf{I}_x, ~~~\frac{\partial \mathbf{I} }{\partial y} =\mathbf{I}_y$$

写成矩阵形式有：

$$\begin{bmatrix} \mathbf{I}_x & \mathbf{I}_y \end{bmatrix} \begin{bmatrix} u \\ v \end{bmatrix} = -\mathbf{I}_t$$
这是一个带有两个变量的一次方程，只有一个点是无法计算 $u$和$v$.
LK的做法是假设某一个窗口内的像素具有相同的运动。假设窗口大小为 $w\times w$, 则有 $w^2$个像素，所以共有 $w^2$个方程：

$$\begin{bmatrix} \mathbf{I}_x & \mathbf{I}_y \end{bmatrix}_k \begin{bmatrix} u \\ v \end{bmatrix} = -\mathbf{I}_{tk}, \quad k=1,\cdots, w^2.$$

$$\mathbf{A} = \begin{bmatrix} [\mathbf{I}_x, \mathbf{I}_y]_1 \\ \vdots \\ [\mathbf{I}_x, \mathbf{I}_y]_k \end{bmatrix},\quad \mathbf{b} = \begin{bmatrix} \mathbf{I}_{t1}\\ \vdots \\ \mathbf{I}_{tk} \end{bmatrix}$$

则方程可变为：

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

这是一个超定线性方程，采用最小二乘解：

$$\begin{bmatrix} u \\ v \end{bmatrix}^* = -(\mathbf{A}^\top\mathbf{A})^{-1}\mathbf{A}^\top \mathbf{b}$$

即可得到$u, v$.

光流跟踪的特点：
- 加速基于特征点的视频里程计算法
- 需要相机运动较慢

直接法原理

设空间点$P$的世界坐标为$[X, Y, Z]$, 它在两个相机上成像的非齐次坐标为$\vec{p}_1, \vec{p}_2$。问题为计算第一个相机到第二个相机的相对位姿变换. 思路为根据当前相机的位姿估计值来寻找$\vec{p}_2$的位置。

以第一个相机为相对参考系，第二个相机的旋转和平移为$\mathbf{R},\vec{t}$(李代数为$\xi$). 另外两个相机的内参$K$相同，所以投影方程为：

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

其中$Z_1$是$P$的深度，$Z_2$是$P$在第二个相机坐标系下的深度，也是$\mathbf{RP}+\vec{t}$的第三个坐标值。

目标是最小化光度误差:

$$e = \mathbf{I}_1(\vec{p}_1) - \mathbf{I}_2(\vec{p}_2)$$
优化方程可写为：
$$\underset{\xi}{\min} J(\xi) = \|e\|^2$$
假设有 $N$个空间点$P_i$, 则整个相机位姿问题为：
$$\underset{\xi}{\min} J(\xi) = \sum_{i=1}^{N} {e}_i^\top {e}_i, ~~ e_i = \mathbf{I}_1(\vec{p}_1, i) - \mathbf{I}_2(\vec{p}_2, i)$$
这里的优化变量为相机位姿 $\xi$.使用李代数上的扰动模型，给 $\exp(\xi)$左乘一个小扰动 $\exp(\delta\xi)$:

$$\begin{aligned} e(\xi \oplus \delta\xi) & = \mathbf{I}_1\left( \frac{1}{Z_1}\mathbf{KP} \right) - \mathbf{I}_2\left( \frac{1}{Z_2}\mathbf{K}\exp(\delta\xi^\wedge)\exp(\xi^\wedge)\mathbf{P} \right)\\ & \approx \mathbf{I}_1\left( \frac{1}{Z_1}\mathbf{KP} \right) - \mathbf{I}_2\left( \frac{1}{Z_2}\mathbf{K}(1 + \delta\xi^\wedge)\exp(\xi^\wedge)\mathbf{P} \right)\\ &= \mathbf{I}_1\left( \frac{1}{Z_1}\mathbf{KP} \right) - \mathbf{I}_2\left( \frac{1}{Z_2}\mathbf{K}\exp(\xi^\wedge)\mathbf{P} + \frac{1}{Z_2}\mathbf{K}\delta\xi^\wedge \exp (\xi^\wedge)\mathbf{P} \right) \end{aligned}$$
为简化上式，记
$$\begin{gathered} \vec{q} = \delta\xi^\wedge\exp(\xi^\wedge)\mathbf{P}\\ \vec{u} = \frac{1}{Z_2}\mathbf{K}\vec{q} \end{gathered}$$
这里 $\vec{q}$的含义为 $P$在扰动后位于第二个相机坐标系下的坐标，而$\vec{u}$为对应的像素坐标。

对上式进行一阶泰勒展开：

$$\begin{aligned} e(\xi \oplus \delta \xi) &=\mathbf{I}_1\left( \frac{1}{Z_1}\mathbf{KP} \right) - \mathbf{I}_2\left( \frac{1}{Z_2}\mathbf{K}\exp(\xi^\wedge)\mathbf{P} + \vec{u} \right)\\ &\approx \mathbf{I}_1\left( \frac{1}{Z_1}\mathbf{KP} \right) - \mathbf{I}_2\left( \frac{1}{Z_2}\mathbf{K}\exp(\xi^\wedge)\mathbf{P} \right) - \frac{\partial \mathbf{I}_2}{\partial \vec{u}} \frac{\partial \vec{u}}{\partial \vec{q}} \frac{\partial \vec{q}}{\partial \vec{\delta\xi}}\delta\xi\\ &=e(\xi) - \frac{\partial \mathbf{I}_2}{\partial \vec{u}} \frac{\partial \vec{u}}{\partial \vec{q}} \frac{\partial \vec{q}}{\partial \vec{\delta\xi}}\delta\xi \end{aligned}$$
上式中：
- $\frac{\partial \mathbf{I}_2}{\partial \vec{u}}$为 $\vec{u}$处的像素梯度
- $\frac{\partial \vec{u}}{\partial \vec{q}}$为关于相机坐标系下的三维点的导数。根据前一节介绍, 记 $\vec{q}=[X, Y, Z]^\top$:
$$\frac{\partial \vec{u}}{\partial \vec{q}} = \begin{bmatrix} \frac{\partial \vec{u} }{\partial X} & \frac{\partial \vec{u} }{\partial Y} & \frac{\partial \vec{u} }{\partial Z} \\ \frac{\partial \vec{v} }{\partial X} & \frac{\partial \vec{v} }{\partial Y} & \frac{\partial \vec{v} }{\partial Z} \end{bmatrix}= \begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_xX}{Z^2} \\ 0 & \frac{f_y}{Z} & -\frac{f_yY}{Z^2}\end{bmatrix}$$

$\frac{\partial \vec{q}}{\partial \vec{\delta\xi}}$是变换后的三维点对变换的导数，在李代数中有详细介绍：

$$\frac{\partial \vec{q}}{\partial \vec{\delta\xi}} = [\mathbf{I}, -\vec{q}^\wedge]$$
注意到后两项只与三维点相关，而与图像无关，所以经常把它们合在一起：
$$\frac{\partial \vec{u}}{\partial \delta \xi} = \begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_xX}{Z^2} & -\frac{f_xXY}{Z^2} & f_x + \frac{f_xX^2}{Z^2} & -\frac{f_xY}{Z}\\ 0 & \frac{f_y}{Z} & -\frac{f_yY}{Z^2} & -f_y - \frac{f_yY^2}{Z^2} & \frac{f_yXY}{Z^2} & -\frac{f_yX}{Z} \end{bmatrix}$$
所以误差相对于李代数的Jacobi矩阵为：
$$\mathbf{J} = -\frac{\partial \mathbf{I}_2}{\partial \vec{u}} \frac{\partial \vec{u} }{\partial \delta \xi}$$

对于$N$个点的问题，我们可以用这方法计算优化的Jacobi矩阵，然后用G-N或L-M计算增量，迭代求解。

上面的推导中, $P$是一个已知位置的空间点，根据来源，可以分为以下几类：
- 若来自于稀疏关键点，称为稀疏直接法
- 若来自于部分像素，称为半稠密(Semi-Dense)直接法
- 若来自于所有像素，称为稠密直接法

总结

直接法总结：
优点有：
- 省去计算特征点、描述子的时间
- 有像素梯度即可，无须特征点
- 可构建稠密或稠密的地图，是特征点无法做到的

缺点：
- 非凸性
- 单个像素没有区分度
- 灰度值不变是很强的假设