>
返回

VIO 中的前端基本流程

整理一下 VIO 中视觉部分的前端流程。

简介

一个 SLAM 系统中通常可以粗略地分为两部分:前端和后端,前端负责接收传感器的所有信息并进行载体前后两帧之间运动信息的粗估计,并有选择性地将部分信息进行存储。而后端则是利用这些存储的传感器信息进行全局优化(或利用滑动窗口优化),得到位姿的精确估计。而对应地,在 VIO 中,前端则是利用相机和 IMU 的观测进行前后两帧之间载体运动的估计,在前端部分相机和 IMU 的处理流程相对独立,根据 IMU 进行运动解算可以参考:基于 IMU 的惯性导航解算及误差分析。这篇博客主要整理以下视觉部分的前端执行流程。

视觉前端基本流程

在 VO 或者 VIO 中,前端中视觉信息(图像)的处理流程流程一般是:

  • 前后两帧图像进行匹配,其中按照不同思路有三种方法:
    • 特征点法:需要进行特征点提取,然后按照描述子对前后两帧的特征点进行匹配
    • 光流法:需要进行关键点提取,然后利用光流法找到上一帧关键点在下一帧图像中的位置
    • 直接法:稠密直接法不需要进行关键点提取,半稀疏和稀疏直接发需要提取关键点
  • 根据匹配结果进行前后两帧图像之间相机的相对位姿变化,同样有几种方法:
    • 对极约束:在前后两帧均只有 2d 像素点时可以利用对极约束估计相机运动
    • PnP:如果在上一帧中已经恢复了特征点的 3d 位置,可以使用 PnP 方法估计相机运动
    • ICP:如果是双目相机的情况,本身可以通过一帧中的两个图像获得特征点的 3d 位置,此时前后两帧特征点都是 3d 的,可以利用 ICP 方法估计相机运动
    • 直接法:直接法在对前后两帧图像匹配时会同时进行相机运动的估计
  • 关键帧选择:出于计算规模和实际场景的考虑,一般两帧图像之间的运动变化不会太大,所以后端没有必要对所有帧进行优化,因此需要指定一定策略有选择性地将某些帧选为关键帧并发往后端
  • 特征点的世界坐标恢复:为了构建地图,我们需要计算图像中的特征点在世界坐标中的位置:基本的方法是三角化

图像匹配

特征点法

特征点法的思路是从两帧图像中提取出一些特征点,这些特征点通常具有:可重复性、可区别性、高效率和本地性。对每一个特征有关键点和描述子两部分:关键点指的是该点本身,而描述子则是利用方法对特征点周围的环境进行描述,以区分其他点。常见的特征点类型有:

获取了两帧图像中的特征点(包括关键点和描述子)之后,对这些特征点利用其描述子进行比较匹配即可,常见的匹配方法有:

  • 暴力匹配:对一帧图像的任意一个特征点与另一帧图像中的所有特征点进行描述子比较,选出最接近的一个
  • 快速近似最近邻(FLANN)

光流法

光流按稠密和稀疏分有 HS 光流和 LK 光流两种,这里整理以下稀疏 LK 光流的原理。LK 光流中认为图像中的每个点的灰度值是随时间改变的,以对于一个像素坐标为 $(x, y)$ 的点,其灰度值为 $\boldsymbol{I}(x, y, t)$。考虑一个空间点在 $t$ 时刻下图像的像素坐标是 $(x, y)$ 在另一帧的图像中像素位置可能会发生变化,我们想要求出其位置变化量,从而在下一帧图像中找到其对应的位置。LK 光流中假设每个空间点在不同图像中的灰度值是一样的,这个假设比较强,但是在相隔时间很短,位移不大的两帧图像中可以姑且认为是成立的。因此,相隔 $dt$ 时间后,有:

$$
\boldsymbol{I}(x + dx, y + dy, t + dt) = \boldsymbol{I}(x, y, t)
$$

我们想要求解式中的 $dx, dy$,对方程进行泰勒展开有:

$$
\boldsymbol{I}(x + dx, y + dy, t + dt) \approx \boldsymbol{I}(x, y ,t) + \frac{\partial\boldsymbol{I}}{\partial x}dx + \frac{\partial\boldsymbol{I}}{\partial y}dy + \frac{\partial\boldsymbol{I}}{\partial t}dt
$$

由于假设灰度不变,因此上式中后面微分部分之和为 0。此外,设像素点在 x 轴和 y 轴的速度分别是 $u = \frac{dx}{dt}$, $v = \frac{dy}{dt}$。则有:

$$
\begin{aligned}
\frac{\partial\boldsymbol{I}}{\partial x}dx + \frac{\partial\boldsymbol{I}}{\partial y}dy + \frac{\partial\boldsymbol{I}}{\partial t}dt &= 0\\
\frac{\partial\boldsymbol{I}}{\partial x}\frac{dx}{dt} + \frac{\partial\boldsymbol{I}}{\partial y}\frac{dy}{dt} =-\frac{\partial\boldsymbol{I}}{\partial t} \\
\frac{\partial\boldsymbol{I}}{\partial x}u + \frac{\partial\boldsymbol{I}}{\partial y}v =-\frac{\partial\boldsymbol{I}}{\partial t} \\
\end{aligned}
$$

上式中,$\frac{\boldsymbol{I}}{\partial x}, \frac{\boldsymbol{I}}{\partial y}, \frac{\boldsymbol{I}}{\partial t}$ 分别是图像在 x,y 轴方向上的梯度以及对时间的变化量,是可以直接计算的。因此我们只需要从中反解出 u, v 即可知道像素点的运动情况,但是显然一个方程解不出两个未知数,所以一个很直观的思路是,假设我们关注的像素点周围一个小范围内像素的运动情况相同,因此可以构建多个这样的方程进行线性最小二乘求解,如下所示。假设在窗口大小为 w 内,构建以下方程。

$$
\begin{aligned}
\boldsymbol{A} &= \begin{bmatrix}
    \left[\boldsymbol{I}_x, \boldsymbol{I}_y\right]_1\\
    ...\\
    \left[\boldsymbol{I}_x, \boldsymbol{I}_y\right]_w\\
\end{bmatrix}\\
\boldsymbol{b} &= \begin{bmatrix}
    \boldsymbol{I}_{t1}\\
    ...\\
    \boldsymbol{I}_{tw}\\
\end{bmatrix}\\
\boldsymbol{A}\begin{bmatrix}
    u\\v
\end{bmatrix} &= -\boldsymbol{b}
\end{aligned}
$$

求解线性最小二乘问题有两种方法:

  • 解析法:利用 $\begin{bmatrix}u \v\end{bmatrix}^*=-(\boldsymbol{A}^T\boldsymbol{A})^{-1}\boldsymbol{A}^T\boldsymbol{b}$ 求解
  • 优化法:

求解的问题为 $\arg\min_{\Delta x, \Delta y}(||\boldsymbol{I}_1(x, y) - \boldsymbol{I}_2(x+\Delta x, y + \Delta y)||_2^2)$,误差函数对带估计量的雅可比根据上式中的线性化可以得到为 $\boldsymbol{I}_x, \boldsymbol{I}_y$,这里可以用第一帧图像中的梯度作为代替。

实际使用中,通常会先提取图像中的关键点,然后进行多层光流(对图像进行不同程度的下采样,在最小的图像中进行光流粗估计像素运动,然后作为初值在下一层图像光流中使用)。

直接法

光流法中,我们需要先对关键点进行追踪然后再通过像素的运动反推出相机的运动。因此相机运动估计的结果不能反向作用与像素运动估计来提到估计精度,直接法的思路将相机的运动估计也融合进像素运动估计中,达到互相反馈的效果。

思路和光流法很像,同样也是基于灰度不变假设,只是对像素运动的表示将相机空间运动考虑在内,假设一个空间坐标为 $\boldsymbol{P} = (x, y, z)$ 的点在前后两帧图像中的像素坐标分别为:$\boldsymbol{p}_1 = (u_1, v_1), \boldsymbol{p}_2 = (u_2, v_2)$。第二帧图像相对于第一帧图像的旋转和平移分别为:$\boldsymbol{R}, \boldsymbol{t}$,根据投影关系有:

$$
\begin{aligned}
    \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 + t})
\end{aligned}
$$

然后基于灰度不变假设,我们尝试优化两帧中的像素点的灰度误差,即:$e = \boldsymbol{I}_1(\boldsymbol{p}_1) - \boldsymbol{I}_2(\boldsymbol{p}_2)$

在优化时可以直接对变换矩阵 $\boldsymbol{T}$ 求导,也可以将其分解为 $\boldsymbol{R}$ 和 $\boldsymbol{t}$ 求导,结果会稍微不同,这里以分开求导为例,设 $\boldsymbol{P}' = \boldsymbol{R}\boldsymbol{P} + \boldsymbol{t}$ 为该点在第二帧相机坐标系的空间坐标,$\boldsymbol{u} = \frac{1}{z_2}\boldsymbol{KP}'$ 为点在第二帧图像中的像素位置,利用链式法则可以求解:

$$
\begin{aligned}
    \frac{\partial\boldsymbol{e}}{\partial\boldsymbol{R}} &= \frac{\partial\boldsymbol{e}}{\partial\boldsymbol{u}}\frac{\partial\boldsymbol{u}}{\partial\boldsymbol{P}'}\frac{\partial\boldsymbol{P}'}{\partial\boldsymbol{R}}\\
    \frac{\partial\boldsymbol{e}}{\partial\boldsymbol{t}} &= \frac{\partial\boldsymbol{e}}{\partial\boldsymbol{u}}\frac{\partial\boldsymbol{u}}{\partial\boldsymbol{P}'}\frac{\partial\boldsymbol{P}'}{\partial\boldsymbol{t}}
\end{aligned}
$$

其中前两部分求解为:

$$
\begin{aligned}
    \frac{\partial\boldsymbol{e}}{\partial\boldsymbol{u}} &= \begin{bmatrix}
        \boldsymbol{I}_x & \boldsymbol{I}_y
    \end{bmatrix}\\
    \frac{\partial\boldsymbol{u}}{\partial\boldsymbol{P}'} &= \frac{\partial\frac{1}{z_2}\boldsymbol{KP}'}{\partial\boldsymbol{P}'}\\
    &=\begin{bmatrix}
        \frac{\partial u}{\partial x_2} & \frac{\partial u}{\partial y_2} & \frac{\partial u}{\partial z_2} \\
        \frac{\partial v}{\partial x_2} & \frac{\partial v}{\partial y_2} & \frac{\partial v}{\partial z_2}
    \end{bmatrix}\\
    &= \begin{bmatrix}
        \frac{f_x}{z_2} & 0 & \frac{f_xx_2}{z^2_2}\\
        0 & \frac{f_y}{z_2} & \frac{f_yy_2}{z^2_2}\\
    \end{bmatrix}
\end{aligned}
$$

最后一部分求解可以参考:一些常见的关于旋转的函数对旋转求导过程的推导,这里直接给出结果:

$$
\begin{aligned}
    \frac{\partial\boldsymbol{P}'}{\partial\boldsymbol{R}} &= -\boldsymbol{RP}^\wedge\\
    \frac{\partial\boldsymbol{P}'}{\partial\boldsymbol{t}} &= \boldsymbol{I}
\end{aligned}
$$

同样取多个点计算雅可比矩阵构建信息矩阵和优化方程,利用高斯牛顿法或者 LM 法求解。

在实际使用中,根据取点的多少可以分为几种方法:

  • 稀疏直接法:像光流一样,提取关键点,不必计算描述子,这种方法可以快速进行匹配求估计相机运动
  • 半稀疏直接法:由于像素值为 0 的点对信息矩阵的贡献是 0,可以取像素值非 0 的点
  • 稠密直接法:取所有点,这种方法可以构建稠密地图

相机运动估计

根据两个匹配点的不同情况,可以分为以下三种方式:

2D-2D:对极几何

在单目相机的情况下,我们通常只能得到一对匹配好的 2d 特征点,这个时候需要利用对极几何来求解相机运动。考虑以下场景:$\boldsymbol{P}$ 为空间中的一个点,坐标为 $(x, y, z)$,它在两帧图像 $I_1, I_2$ 中的投影点分别为 $\boldsymbol{p}_1, \boldsymbol{p}_2$, $O_1, O_2$ 分别为相机中心。$\boldsymbol{P}, O_1, O_2$ 构成一个三角形,称为极平面,$e_1, e_2$ 分别是 $O_1O_2$ 在 $I_1, I_2$ 中的交点,称为极点。$l_1, l_2$ 是极平面和两帧图像平面的交线,称为极线

先看第一帧,在不知道 $\boldsymbol{P}$ 的具体位置时,射线 $\overrightarrow{O_1p_1}$ 是它可能出现的空间位置。同样的道理看第二帧,射线 $\overrightarrow{e_2p_2}$ 也是 $\boldsymbol{P}$ 在第二帧上可能出现的投影位置,即 $p_2$ 的像素位置。通过特征点匹配或者光流法我们已经确定了 $p_2$ 的像素位置,因此可以利用几何关系分析两帧图像的相对变换。

假设前后两帧之间相机的旋转和平移分别是 $\boldsymbol{R}, \boldsymbol{t}$,这里假设第一帧图像的相机坐标系就是参考坐标系,设$\boldsymbol{K}$ 是相机的内参矩阵,则 $p_1, p_2$ 的齐次像素坐标分别是:

$$
\begin{aligned}
    s_1\boldsymbol{p}_1 &= \boldsymbol{KP}\\
    s_2\boldsymbol{p}_2 &= \boldsymbol{K}(\boldsymbol{RP} + \boldsymbol{t})
\end{aligned}
$$

这里 $s$ 为比例因子(实际上 $s = z$),对于一个齐次坐标,向量和其自身乘上一个非零常数称为尺度意义下相等,即:

$$
s\boldsymbol{p} \approxeq \boldsymbol{p}
$$

因此有:

$$
\begin{aligned}
    \boldsymbol{p}_1 &\approxeq \boldsymbol{KP}\\
    \boldsymbol{p}_2 &\approxeq \boldsymbol{K}(\boldsymbol{RP} + \boldsymbol{t})
\end{aligned}
$$

设 $\boldsymbol{x}_1, \boldsymbol{x}_2$ 为该点在两图像上像素点的归一化平面坐标,有:

$$
\begin{aligned}
    \boldsymbol{x}_1 &= \boldsymbol{K}^{-1}\boldsymbol{p}_1\\
    \boldsymbol{x}_2 &= \boldsymbol{K}^{-1}\boldsymbol{p}_2\\
\end{aligned}
$$

代入上式有:

$$
\boldsymbol{x}_2 \approxeq \boldsymbol{R}\boldsymbol{x}_1 + \boldsymbol{t}
$$

等式两侧同时乘上 $\boldsymbol{t}^\wedge$(即对 $\boldsymbol{t}$ 做叉积),可以消掉右侧的 $\boldsymbol{t}$,有:

$$
\boldsymbol{t}^\wedge\boldsymbol{x}_2 \approxeq \boldsymbol{t}^\wedge\boldsymbol{Rx}_1
$$

然后同时左乘 $\boldsymbol{x}_2^T$:

$$
\boldsymbol{x}_2^T\boldsymbol{t}^\wedge\boldsymbol{x}_2 \approxeq \boldsymbol{x}_2^T\boldsymbol{t}^\wedge\boldsymbol{Rx}_1
$$

观察左侧式子,$\boldsymbol{t}^\wedge\boldsymbol{x}_2$ 和 $\boldsymbol{t}, \boldsymbol{x}_2$ 都垂直(叉积性质),因此和 $\boldsymbol{x}_2^T$ 求内积恒等于 0,因此对右侧式子有:

$$
\boldsymbol{x}_2^T\boldsymbol{t}^\wedge\boldsymbol{Rx}_1 = 0
$$

代入 $\boldsymbol{p}_1, \boldsymbol{p}_2$ 有:

$$
\boldsymbol{p}_2^T\boldsymbol{K}^{-T}\boldsymbol{t}^\wedge\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{p}_1 = 0
$$

这两个式子称为对极约束,表示了一个 3d 点在两帧图像投影点之间的来自两帧图像位姿变化的约束关系,为了简化表示,做以下处理:

$$
\begin{aligned}
    \boldsymbol{E} &= \boldsymbol{t}^\wedge\boldsymbol{R}\\
    \boldsymbol{F} &= \boldsymbol{K}^{-T}\boldsymbol{E}\boldsymbol{K}^{-1}\\
\end{aligned}
$$

其中,$\boldsymbol{E}, \boldsymbol{F}$ 分别称为本质矩阵和基础矩阵,对极约束可以表示为:

$$
\boldsymbol{x}_2^T\boldsymbol{Ex}_1 = \boldsymbol{p}_2^T\boldsymbol{Fp}_1 = 0
$$

通过对极约束,估计相机运动的过程可以分成两部:

  • 根据配对像素点求 $\boldsymbol{E}$ 或者 $\boldsymbol{F}$
  • 根据 $\boldsymbol{E}$ 或者 $\boldsymbol{F}$ 分解出 $\boldsymbol{R}, \boldsymbol{t}$

E 的求解

由于内参矩阵 $\boldsymbol{K}$ 一般是已知的,所以一般求更简洁的本质矩阵 $\boldsymbol{E}$,它具有以下性质:

  • 由于对极约束是等式为 0 的约束,因此对 $\boldsymbol{E}$ 乘上任何非零常数对极约束依然满足,因此 $\boldsymbol{E}$ 在不同尺度下是等价的,其内 9 个参数中自由度为 8
  • 根据 $\boldsymbol{E} = \boldsymbol{t}^\wedge\boldsymbol{R}$ 可以证明它的奇异值形式为:$[\sigma, \sigma, 0]^T$,这称为本质矩阵的内在性质
  • 由于平移和旋转各有三个自由度,因此本质矩阵最多只有 6 个自由度,又由于本质矩阵的尺度等价性,实际上只有 5 个自由度

理论上,我们可以只用 5 个点求解本质矩阵,但是 $\boldsymbol{E}$ 的内在性质会给估计带来麻烦,因此只考虑尺度等价性,即考虑其有 8 个自由度,利用 8 个点可以构成方程组。设每个点的归一化坐标为 $[u, v, 1]^T$,有:

$$
\begin{bmatrix}
    u_2 & v_2 & 1
\end{bmatrix}
\begin{bmatrix}
    e_1 & e_2 & e_3 \\
    e_4 & e_5 & e_6 \\
    e_7 & e_8 & e_9
\end{bmatrix}
\begin{bmatrix}
    u_1\\v_1\\1
\end{bmatrix} = 0
$$

将 $\boldsymbol{R}$ 改为向量形式,改写成线性形式有:

$$
\begin{bmatrix}
    u_2u_1 & u_2v_1 & u_2 & v_2u_1 & v_2v_1 & v_2 & u_1 & v_1 & 1
\end{bmatrix}\boldsymbol{e} = 0
$$

取 8 个点构成方程组,有:

$$
\begin{bmatrix}
    u^1_2u^1_1 & u^1_2v^1_1 & u^1_2 & v^1_2u^1_1 & v^1_2v^1_1 & v^1_2 & u^1_1 & v^1_1 & 1 \\
    u^2_2u^2_1 & u^2_2v^2_1 & u^2_2 & v^2_2u^2_1 & v^2_2v^2_1 & v^2_2 & u^2_1 & v^2_1 & 1 \\
    ...\\
    u^8_2u^8_1 & u^8_2v^8_1 & u^8_2 & v^8_2u^8_1 & v^8_2v^8_1 & v^8_2 & u^8_1 & v^8_1 & 1
\end{bmatrix}
\begin{bmatrix}
    e_1 \\ e_2 \\ e_3 \\e_4 \\ e_5 \\e_6 \\e_7 \\e_8 \\e_9
\end{bmatrix}  = 0
$$

只要系数矩阵为 8 即可直接求解得出 $\boldsymbol{E}$

R, t 的求解

利用 SVD 分解 $\boldsymbol{E} = \boldsymbol{U\Sigma V}^T$ 可以恢复 $\boldsymbol{R}, \boldsymbol{t}$,一共可以得到 4 个解,需要根据将点的空间坐标带入通过判断深度是否为正数来判断解的合法性。过程可以参考这篇博客:本质矩阵E求解及运动状态恢复

3D-2D:PnP

如果我们已知了一些特征点在空间中的 3d 坐标以及投影位置,需要求解相机位姿,这个问题叫 PnP 问题。有几种方法:如 P3P,EPnP,DLT 以及最直观的优化方法。

3D-3D:ICP

如果我们已知特征点在两帧相机坐标系下的坐标,要求相机之间的位姿变化,可以采用 ICP 方法求解。ICP 方法应用比较广泛,且不局限于视觉 SLAM,求解方法同样有利用线性代数的解析方法以及利用非线性优化方式的方法求解。

特征点坐标恢复

如果我们知道了一个特征点在两帧图像的投影位置,以及相机的运动估计。我们可以利用三角化来恢复该点的空间坐标。同样考虑下图所示场景:

以 $I_1$ 时相机的坐标系为参考坐标系,第二帧相机的旋转和平移分别是 $\boldsymbol{R}, \boldsymbol{t}$。设 $\boldsymbol{x}_1, \boldsymbol{x}_2$ 为该特征点在两时刻下相机坐标系下的归一化坐标,则有:

$$
s_2\boldsymbol{x}_2 = s_1\boldsymbol{Rx}_1 + \boldsymbol{t}
$$

这里 $s_1, s_2$ 实际上是特征点在两个相机坐标系下的深度,即 $z$。如果我们能求出这个参数,则可以通过反投影计算出点的空间坐标。比较简单的方式是左乘一个 $\boldsymbol{x}_2^\wedge$,构建构建方程如下:

$$
s_2\boldsymbol{x}_2^\wedge\boldsymbol{x}_2 = 0 = s_1\boldsymbol{x}_2^\wedge\boldsymbol{Rx}_1 + \boldsymbol{x}_2^\wedge\boldsymbol{t}
$$

上式右侧只有 $s_2$ 一个未知数,可以直接求解,实际使用中由于 $\boldsymbol{R}, \boldsymbol{t}$ 的估计误差可能导致等式实际上不为 0,因此一般会求最小二乘解。

参考资料

Built with Hugo
Theme Stack designed by Jimmy