《视觉SLAM十四讲》-- 视觉里程计 1（上）

06 视觉里程计 1

一个 SLAM 系统分为前端和后端，前端也称为视觉里程计，它根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。包括特征点法和直接法两大类。

6.1 特征点法

（1）视觉 SLAM 中，将图像特征点作为路标，从而估计出相机的运动。

（2）特征点是图像中具有代表性的部分，他应该能在光照、视角发生少量变化时仍能保持一致。

• 可重复性

• 可区别性

• 高效性

• 本地性

（3）特征点由关键点（位置、大小、方向）和描述子（特征点周围的图像信息）组成。

（4）SIFT、FAST、ORB 特征，下面以 ORB 特征为例作简要介绍。

6.1.1 特征提取

（1）ORB 特征包括 Oriented FAST 关键点和 BRIEF 描述子两部分。

（2）FAST 角点主要检测局部像素灰度变化明显的地方，具体步骤为：

• 在图像中选取像素 $p$， 假设其亮度为 $I_p$；

• 设置一个阈值 $T$（比如 $I_P$ 的20%）；

• 以像素 $p$ 为中心，选取半径为 3 的圆上的 16 个像素点；

• 若圆上有连续 $N$ 个像素点的灰度值小于 $I_p-T$ 或大于 $I_p+p$ ，则认为像素 $p$ 为特征点。（$N$ 通常取12 即 FAST-12，其他还有 9 和 11。）

• 重复以上步骤，依次遍历每个像素。

（3）FAST 关键点还包括旋转信息，即特征点附近的图像灰度质心，具体算法为：

• 在一个小的图像块 B 中，定义图像的矩为

m p q = ∑ x , y ∈ B x p y q I ( x , y ) , p , q = { 0 , 1 } m_{p q}=\sum_{x, y \in B} x^{p} y^{q} I(x, y), \quad p, q=\{0,1\} mpq​=x,y∈B∑​xpyqI(x,y),p,q={0,1}

• 计算图像的质心

$$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$$

• 连接图像块 B 的几何中心 O 和 质心 C，得到向量 $\overrightarrow{OC}$，则特征点的方向可以定义为

$$\theta =\arctan (m_{01}/m_{10})$$

（4）BRIEF 是一种二进制描述子，由 0 和 1 组成。在关键点附近区域内，按照一定规则或随机选取 128 对像素点（如 $p$ 和 $q$ ），若 $p>q$ 则取 1，反之取 0，最终得到一个 128 维由 0、1 组成的向量。

6.1.2 特征匹配

（1）汉明距离：两个二进制串之间的汉明距离，指的是其不同位数的个数。

（2）假设两张图像 $I_1$ 和 $I_2$ 经上述特征点提取后，分别得到特征点 $x_1^m,m=1,2,...,M$、 $x_1^n,m=1,2,...,N$。最简单的匹配算法是暴力匹配，即计算每一个特征点 $x_1^m$ 与所有的 $x_1^n$ 的汉明距离，然后排序，取最近的一个作为匹配点。

6.1.3 计算相机运动

有了匹配好的点对，接下来就需要根据点对估计相机的运动。

（1）如果只有两个单目图像，得到 2D-2D 间的关系，用对极几何解决；

（2）如果匹配的是帧（2D）和地图（3D），则得到 3D-2D 的关系，通过 PnP 求解；

（3）如果匹配的是 RGB-D 图像，则得到 3D-3D 间的关系，用 ICP 求解。

6.2 2D-2D：对极几何

6.2.1 对极约束

如上图所示，$O_1$ 、$O_2$ 分别为相机光心，$p_1$、$p_2$ 为成像平面上的一对匹配特征点。$O_1{O}_{2}P$ 称为极平面；$O_1{O}_2$ 连线与成像平面 $I_1$ 、$I_2$ 的交点 $e_1$ 、$e_2$ 称为极点；$l_1$ 、$l_2$ 称为极线；$O_1{O}_2$ 为基线。

以第一帧图像为基础，经旋转 $\mathbf{R}$、平移 $\mathbf{t}$ 得到第二帧图像。

（1）在实践中，$p_1$ 和 $p_2$ 可通过特征匹配得到，空间点 $P$ 未知，$e_1$ 、$e_2$ 未知；我们希望求出变换矩阵 ${\mathbf{T}}_{12}$。

（2）我们直接以第一帧的坐标系为世界坐标系（这样省去了第一帧世界坐标系和相机坐标系的转换），空间点 $P$ 的坐标为 $[X,Y,Z{]}^T$，则其与 $p_1$ 、 $p_2$ 关系为

$$s_1{\mathbf{p}}_1=\mathbf{KP},s_1{\mathbf{p}}_2=\mathbf{K}(\mathbf{RP}+\mathbf{t})\text{\hspace{1em}(6-1)}$$

这里 $s_1$、$s_2$ 为深度（也就是 $Z$）。为便于表达，我们将上述投影关系记为

$${\mathbf{p}}_1\simeq \mathbf{K}\mathbf{P},{\mathbf{p}}_2\simeq \mathbf{K}(\mathbf{RP}+\mathbf{t})\text{\hspace{1em}(6-2)}$$

由归一化坐标 ${\mathbf{x}}_1$、 ${\mathbf{x}}_2$ 到像素坐标 ${\mathbf{p}}_1$、 ${\mathbf{p}}_2$，有

$${\mathbf{p}}_1=\mathbf{K}{\mathbf{x}}_1，{\mathbf{p}}_2=\mathbf{K}{\mathbf{x}}_2\text{\hspace{1em}(6-3)}$$

也即
$${\mathbf{x}}_1={\mathbf{K}}^{-1}{\mathbf{p}}_1，{\mathbf{x}}_2={\mathbf{K}}^{-1}{\mathbf{p}}_2\text{\hspace{1em}(6-4)}$$

结合式（6-2）、（6-3）、（6-4），得到

$${\mathbf{x}}_2\simeq \mathbf{R}{\mathbf{x}}_1+\mathbf{t}\text{\hspace{1em}(6-5)}$$

推导过程：

$$\begin{array}{rl}{\mathbf{x}}_2& ={\mathbf{K}}^{-1}{\mathbf{p}}_2\\ \simeq & {\mathbf{K}}^{-1}\mathbf{K}(\mathbf{RP}+\mathbf{t})\\ \simeq & \mathbf{RP}+\mathbf{t}\\ \simeq & \mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1+\mathbf{t}\\ \simeq & {\mathbf{Rx}}_1+\mathbf{t}\end{array}$$

注意，这里只是尺度意义相等，因为还相差一个缩放关系。

左乘 ${\mathbf{t}}^{\wedge }$，得

$${\mathbf{t}}^{\wedge }{\mathbf{x}}_2\simeq {\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1\text{\hspace{1em}(6-6)}$$

再同时左乘 ${\mathbf{x}}_2^{\mathbf{T}}$，即

$${\mathbf{x}}_2^{\mathbf{T}}{\mathbf{t}}^{\wedge }{\mathbf{x}}_2\simeq {\mathbf{x}}_2^{\mathbf{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1\text{\hspace{1em}(6-7)}$$
观察左侧，${\mathbf{t}}^{\wedge }{\mathbf{x}}_2$ 是一个与 $\mathbf{t}$ 和 ${\mathbf{x}}_2$ 都垂直的向量（相当于叉乘），它再和 ${\mathbf{x}}_2$ 做内积（点乘）时，结果为零，也就是说

$${\mathbf{x}}_2^{\mathbf{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0\text{\hspace{1em}(6-8)}$$

这就是对极约束。

将式（6-4）代入上式

$${\mathbf{p}}_2^{\mathbf{T}}{\mathbf{K}}^{-\mathbf{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1=0\text{\hspace{1em}(6-9)}$$

至此，容易看出，我们只需要知道两张图的像素坐标以及相机内参即可求出相机运动 $\mathbf{R}$、$\mathbf{t}$。

将中间部分分别记为：基础矩阵 $\mathbf{F}$ 和本质矩阵 $\mathbf{E}$，即

$$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$$
$$\mathbf{F}={\mathbf{K}}^{-\mathbf{T}}\mathbf{E}{\mathbf{K}}^{-1}$$
$${\mathbf{x}}_2^{\mathbf{T}}\mathbf{E}{\mathbf{x}}_1={\mathbf{p}}_2^{\mathbf{T}}\mathbf{F}{\mathbf{p}}_1=0\text{\hspace{1em}(6-10)}$$

（3）根据以上推导，相机位姿估计问题简化为以下两步：

• 根据匹配点的像素坐标和相机内参求出本质矩阵 $\mathbf{E}$；

• 由本质矩阵求出 $\mathbf{R}$ 和 $\mathbf{t}$。

6.2.2 本质矩阵

（1）已知旋转和平移各有 3 个自由度，但由于尺度等价性（$\mathbf{E}$ 任意缩放，对极几何均成立），$\mathbf{E}$ 有 5 个自由度。

（2）理论上最少 5 对点即可求解，但由于其非线性性质，一般将它当做普通矩阵，那么就有 8 个自由度，因此采用 八点法 求解。

（3）假设一对匹配点的 归一化坐标 为 $[u_1,v_1,1{]}^T$、$[u_1,v_1,1{]}^T$，根据对极约束，有

$$\left[u_2,v_2,1\right]\left[\begin{array}{lll}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0\text{\hspace{1em}(6-11)}$$

把 $\mathbf{E}$ 展开，写成列向量的形式

$$\mathbf{e}=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$$

将式（6-11）展开，并写成以下形式

$$[u_2{u}_1,u_2{v}_1,u_2,v_2{u}_1,v_2{v}_1,v_2,u_1,1]\cdot \mathbf{e}=0\text{\hspace{1em}(6-12)}$$

将所有 8 个点表达式联立为线性方程组

$$\left(\begin{array}{ccccccccc}{u}_2^1{u}_1^1& u_2^1{v}_1^1& u_2^1& v_2^1{u}_1^1& v_2^1{v}_1^1& v_2^1& u_1^1& v_1^1& 1\\ u_2^2{u}_1^2& u_2^2{v}_1^2& u_2^2& v_2^2{u}_1^2& v_2^2{v}_1^2& v_2^2& u_1^2& v_1^2& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& \\ u_2^8{u}_1^8& u_2^8{v}_1^8& u_2^8& v_2^8{u}_1^8& v_2^8{v}_1^8& v_2^8& u_1^8& v_1^8& 1\end{array}\right)\left(\begin{array}{l}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right)=0\text{\hspace{1em}(6-13)}$$

（4）$\mathbf{E}$ 解出来以后，用奇异值分解恢复出矩阵 $\mathbf{R}$ 和 $\mathbf{t}$。设 $\mathbf{E}$ 的奇异值分解为

$$\mathbf{E}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathbf{T}}$$

其中，$\mathbf{U}$ 和 $\mathbf{V}$ 是正交矩阵，$\mathbf{\Sigma }$ 是奇异值矩阵。有四种可能的解

$$\begin{array}{rl}{\mathbf{t}}_1^{\wedge }& =\mathbf{U}{\mathbf{R}}_Z\left(\frac{\pi }{2}\right)\mathbf{\Sigma }{\mathbf{U}}^{\mathrm{T}},{\mathbf{R}}_1=\mathbf{U}{\mathbf{R}}_Z^{\mathrm{T}}\left(\frac{\pi }{2}\right){\mathbf{V}}^{\mathrm{T}}\\ {\mathbf{t}}_2^{\wedge }& =\mathbf{U}{\mathbf{R}}_Z\left(-\frac{\pi }{2}\right)\mathbf{\Sigma }{\mathbf{U}}^{\mathrm{T}},{\mathbf{R}}_2=\mathbf{U}{\mathbf{R}}_Z^{\mathrm{T}}\left(-\frac{\pi }{2}\right){\mathbf{V}}^{\mathrm{T}}\end{array}\text{\hspace{1em}(6-14)}$$

其中，${\mathbf{R}}_Z(\frac{\pi }{2})$ 表示沿 $Z$ 轴旋转得到的旋转矩阵。

图中，蓝色线为相机，红色点为空间点 $P$ 的投影，在其相对位置不变的情况下，有四种可能的情况，但只有第一种，相机有正的深度。因此，只要将任意一点代入四个解中，检测该点在两个相机下的深度，就可确定正确的解。

（5）上面我们是将 $\mathbf{E}$ 看做普通矩阵进行求解并 SVD 分解，那么，由于没有考虑其内在约束关系，分解出来的奇异值矩阵可能并不是 $[\sigma ,\sigma ,0{]}^T$ ，而是 $[{\sigma }_1,{\sigma }_2,{\sigma }_3{]}^T$ ，需要将其调整为以下形式：

$$\mathbf{E}=\mathbf{U}\mathrm{diag}\left(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0\right){\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(6-15)}$$

当然，由于 $\mathbf{E}$ 具有尺度不变性，可将奇异值矩阵直接取为 $diag(1,1,0)$。

（6）八点法的讨论

• 用于单目 SLAM 的初始化（必须有平移，否则 $\mathbf{E}$ 等于零）

• 对于纯旋转问题，无法求解

6.2.3 单应矩阵

若场景中的特征点都位于同一平面上（墙、地面等），本质矩阵 $\mathbf{E}$ 便会退化，此时可通过单应矩阵 $\mathbf{H}$ 进行运动估计。

在相机坐标系下（光心为原点），假设平面 单位法向量 为 $\mathbf{n}$， 且平面到原点的距离为 $d$，$\mathbf{P}$ 为平面上某点。则平面可写成（回忆一下点到平面的距离及其表达）：

$${\mathbf{n}}^{\mathbf{T}}\mathbf{P}+d=0\text{\hspace{1em}(6-16)}$$

整理，得

$$-\frac{{\mathbf{n}}^{\mathbf{T}}\mathbf{P}}{d}=1\text{\hspace{1em}(6-17)}$$

结合式（6-2），并将上式代入

$$\begin{array}{rl}{\mathbf{p}}_2& \simeq \mathbf{K}(\mathbf{RP}+\mathbf{t})\\ & \simeq \mathbf{K}\left(\mathbf{RP}+\mathbf{t}\cdot \left(-\frac{{\mathbf{n}}^{\mathrm{T}}\mathbf{P}}{d}\right)\right)\\ & \simeq \mathbf{K}\left(\mathbf{R}-\frac{\mathbf{t}{\mathbf{n}}^{\mathrm{T}}}{d}\right)\mathbf{P}\\ & \simeq \mathbf{K}\left(\mathbf{R}-\frac{\mathbf{t}{\mathbf{n}}^{\mathrm{T}}}{d}\right){\mathbf{K}}^{-1}{\mathbf{p}}_1\end{array}\text{\hspace{1em}(6-18)}$$

至此，我们得到了两帧图像特征点 ${\mathbf{p}}_1$、 ${\mathbf{p}}_2$ 之间的对应关系，将中间部分记为 $\mathbf{H}$，则

$${\mathbf{p}}_2\simeq \mathbf{H}{\mathbf{p}}_1\text{\hspace{1em}(6-19)}$$

（2）类似本质矩阵求解，将上式写成下面的形式

$$\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]\simeq \left[\begin{array}{lll}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]\text{\hspace{1em}(6-20)}$$

解得

$$u_2=\frac{h_1{u}_1+h_2{v}_1+h_3}{h_7{u}_1+h_8{v}_1+h_9}$$
$$v_2=\frac{h_4{u}_1+h_5{v}_1+h_6}{h_7{u}_1+h_8{v}_1+h_9}$$

实际处理中，可以令 $h_9=1$ 。整理，得

$$h_1{u}_1+h_2{v}_1+h_3-h_7{u}_1{u}_2-h_8{v}_1{u}_2=u_2$$
$$h_4{u}_1+h_5{v}_1+h_6-h_7{u}_1{v}_2-h_8{v}_1{v}_2=v_2\text{\hspace{1em}(6-21)}$$

这样一对匹配点可以构建两个约束，而单应矩阵有 8 个自由度（由于尺度不变性，可任意缩放，少一个自由度），故至少需要 4 对匹配点才可解出。类似 $\mathbf{E}$ 的求解过程：

$$\left[\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{v}_2^1& -v_1^1{v}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{v}_2^2& -v_1^2{v}_2^2\\ u_1^3& v_1^3& 1& 0& 0& 0& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ 0& 0& 0& u_1^3& v_1^3& 1& -u_1^3{v}_2^3& -v_1^3{v}_2^3\\ u_1^4& v_1^4& 1& 0& 0& 0& -u_1^4{u}_2^4& -v_1^4{u}_2^4\\ 0& 0& 0& u_1^4& v_1^4& 1& -u_1^4{v}_2^4& -v_1^4{v}_2^4\end{array}\right]\left[\begin{array}{l}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right]=\left[\begin{array}{c}{u}_2^1\\ v_2^1\\ u_2^2\\ v_2^2\\ u_2^3\\ v_2^3\\ u_2^4\\ v_2^4\end{array}\right]\text{\hspace{1em}(6-22)}$$

解出来以后，再恢复出 $\mathbf{R}$、$\mathbf{t}$、$\mathbf{n}$、$d$，常用的方法有数值法和解析法，同样会出现四组可能的解，需要一一排除，只剩下唯一解。

6.2.4 小结

（1）当特征点 共面 或 相机纯旋转 时，使用单应矩阵求解效果更好。实践中，会同时计算出单应矩阵和本质矩阵，选择重投影误差最小的那个作为运动估计矩阵。

（2）$\mathbf{E}$ 具有尺度等价性，分解得到的 $\mathbf{R}$ 和 $\mathbf{t}$ 也有尺度等价性，但由于 $\mathbf{R}$ 本身有内在约束，我们认为 $\mathbf{t}$ 有一个尺度，也就是说，无法确定 $\mathbf{t}$ 的实际大小，它乘以任意倍数，分解都是成立的。因此，我们经常将 $\mathbf{t}$ 归一化，让它的长度为 1。

（3）对 $\mathbf{t}$ 归一化相当于固定了尺度。以这时的 $\mathbf{t}$ 为单位 1，计算相机运动和特征点的 3D 位置，这个过程称为单目 SLAM 初始化。初始化以后，就可以用 3D-2D 计算相机运动了。

（4）单目初始化不能只有纯旋转，必须有一定程度的平移。单目 SLAM 实践中，可以让相机进行左右平移就可实现初始化（这样计算更简单）。

（5）当匹配点多于 8 对时，我们可以计算最小二乘解。对于式（6-13），将左侧系数矩阵记为 $\mathbf{A}$：

$$\mathbf{Ae}=0$$

可以通过最小化一个二次型来求（求出使$\mathbf{Ae}$ 值最小的 $\mathbf{e}$）：

$$\underset{\mathbf{e}}{\min }\Vert \mathbf{A}\mathbf{e}{\Vert }_2^2=\underset{\mathbf{e}}{\min }{\mathbf{e}}^{\mathrm{T}}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}\mathbf{e}$$

当可能存在误匹配（错误数据）时，常使用随机采样一致性（RANSAC）来求，而不是最小二乘。

6.3 三角测量

前面我们通过匹配特征点估计出了相机运动，下面还需要根据相机运动估计出特征点的空间位置。在单目 SLAM 中，仅通过单张图像，无法得到深度信息。我们通过 三角测量 估计出点的深度。

（1）以下各点所表达的含义与上面类似：

理论上 $O_1{p}_1$ 与 $O_2{p}_2$ 会相交于一点 $P$，但实际上由于噪声的存在，二者无法相交。因此可通过最小二乘法求解。

（2）以左图为参考，经变换 $\mathbf{T}$ 得到右图。假设 ${\mathbf{x}}_1$ 和 ${\mathbf{x}}_2$ 为两个特征点的归一化坐标，$\mathbf{P}$ 为第一张图像相机坐标系下的空间坐标，有

$$s_2{\mathbf{x}}_2=\mathbf{R}\mathbf{P}+\mathbf{t}=\mathbf{R}(s_1{\mathbf{x}}_1)+\mathbf{t}$$

也就得到了书中（这里不太好理解）：

$$s_2{\mathbf{x}}_2=s_1\mathbf{R}{\mathbf{x}}_1+\mathbf{t}\text{\hspace{1em}(6-23)}$$

其中， $s_1$ 和 $s_2$ 为深度。先计算 $s_2$ ，将上式两侧同乘 ${{\mathbf{x}}_2}^{\wedge }$（相当于叉乘），

$$s_2{{\mathbf{x}}_2}^{\wedge }{\mathbf{x}}_2=0=s_1{{\mathbf{x}}_2}^{\wedge }\mathbf{R}{\mathbf{x}}_1+{{\mathbf{x}}_2}^{\wedge }\mathbf{t}\text{\hspace{1em}(6-24)}$$

右侧可看成关于 $s_1$ 的方程。我们已知相机内参、两个特征点的像素坐标以及变换矩阵，由式（6-4）容易得到归一化坐标，从而解出 $s_1$，再由式（6-23）得到 $s_2$。

实际上由于噪声的存在，式（6-24）右侧并不会严格等于零，所以一般情况下都是采用最小二乘法而非直接求解。

（3）三角测量是 平移 得到的（有平移才会有三角形），纯旋转无法得到三角测量。

（4）三角测量的矛盾：如下图所示，当平移很小时，深度（$\delta d$）不确定性较大；也就是说，当平移较大时，三角测量更加精确。

要提高三角化的精度：

• 提高特征点的提取精度，也就是提高图片分辨率，但这样图片会变得很大，增加计算成本；

• 增大平移量，但这样有时图像的外观会发生明显变化（原先被遮挡的部分显现出来） ，导致特征提取和匹配困难。

6.4 3D-2D: PnP

（1）PnP 是求解 3D 到 2D 点对运动的方法，当知道了 $n$ 个 3D 空间点及其投影位置时，便可估计出相机位姿。

（2）对极几何至少需要 8 对点才能求解，且存在初始化、纯旋转的问题；而如果两帧图像中一对特征点的 3D 位置已知，最少就只需要 3 对点便可求解。

（3）特征点的 3D 位置可由 RGB-D 相机或三角测量得到。因此，在双目或 RGB-D 视觉里程计中，可直接用 PnP 估计相机运动；在单目视觉里程计中，先用对极几何和三角测量估计出特征点的 3D 位置（也就是初始化），再使用 PnP。

（4）常用求解方法有：P3P、直接线性变换、EPnP、UPnP、光束法平差（BA）。