08 计算相机运动

8.1 提取特征点

读取图像 --> 提取角点 --> 计算各角点的描述子 --> 匹配描述子（计算汉明距离）–> 根据汉明距离去除误匹配点对 --> 绘制结果

根据匹配好的像素点对估计相机的运动。

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

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

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

8.2 对极几何

8.2.1 几个矩阵

具体推导见笔记《视觉里程计 1》。

$${\mathbf{x}}_2^{\mathbf{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0$$

这就是对极约束，其中 ${\mathbf{x}}_1$ 为归一化坐标。

带入像素坐标，得

$${\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)}$$

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

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

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

同理，单应矩阵可用类似方法求解。

本质矩阵自由度为 5，理论上 5 对点即可求解，实际采用八点法；而单应矩阵 4 对点即可求解。

8.2.2 相机坐标系与归一化坐标系

相机内参：

$$\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

其中，$c_x，c_y$ 是相机光心，也就是相机的光学中心在图像平面上的 $x，y$ 坐标；$f_x，f_y$ 是焦距。

像素坐标 $(u,v)$ 到归一化坐标 $(x,y)$ 的转换：

$$\begin{gathered} x=(u-c_x)/f_x \\ y=(v-c_y)/f_y \end{gathered}$$

归一化坐标 $(x,y)$ 到像素坐标 $(u,v)$ 的转换：

$$\begin{gathered} u=(x\ast f_x)+c_x \\ v=(y\ast f_y)+c_y \end{gathered}$$

8.2.3 三角测量

即根据计算得到的 $\mathbf{R}，\mathbf{t}$ 恢复出尺度 $s$，进而求出三维空间坐标。

$$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}$$

8.3 PnP: 3D-2D

简单来说，PnP 位姿估计就是通过几个已知坐标(世界坐标)的特征点，结合他们在相机照片中的成像(像素坐标)，求解出相机所在的世界坐标以及旋转角度。

主要有三种求解方法：

• 直接线性变换（DLT）：至少需要 6 对点
• P3P：3 对点即可，当给定的配对点多于 3 对时，难以利用更多的信息。
• 非线性优化即 BA

针对 BA 求解，以 g2o 为例，以相机位姿和 3D 点坐标为顶点，以对应的像素坐标为观测值。

8.4 ICP：3D-3D

针对一组匹配好（已知对应关系）的 3D 点，可用 ICP 算法求解。与激光 SLAM 中的 ICP 不同，激光点云数据特征不够丰富，无法知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，再逐步迭代。

• SVD 方法

• 非线性优化

8.5 总结

一、基础矩阵、单应矩阵用像素坐标求解；本质矩阵、三角测量用归一化坐标求解。

单应矩阵直接描述了图像坐标之间的变换：

$${\mathbf{p}}_2=\mathbf{H}{\mathbf{p}}_1$$

二、在 ORB-SLAM 中这几种方法的顺序为：

• 提取两张图像的匹配点，利用对极几何计算 $\mathbf{H}$ 矩阵和 $\mathbf{F}$ 矩阵，从而恢复 $\mathbf{R}$、$\mathbf{t}$；

• 用三角测量计算各关键点对应的 3D 坐标；

• 跟踪丢失后，机器人就需要回到原来经过的位置找匹配帧，这个匹配帧的关键点对应的 3D 坐标是已知的，之前已经求出，利用这些 3D 点和机器人当前图像关键点像素坐标，就构成了 PnP 问题，从而可计算出当前的 $\mathbf{R}$、$\mathbf{t}$。