视觉里程计（Visual Odometry, VO）是通过处理图像序列，估计摄像头在时间上的相对位姿变化的技术。它是视觉SLAM的重要组成部分之一，主要通过提取图像中的信息（如特征点或直接像素强度）来实现相机运动估计。以下从特征点法、2D-2D对极几何、三角测量、3D-2D的PnP方法、3D-3D的ICP方法介绍视觉里程计的核心内容。

特征点法

特征点法是视觉里程计的经典方法，通过提取图像中的显著特征点，计算特征点之间的匹配关系，进而估计相机的运动。

特征点法简介

• 原理：提取图像中显著的特征点，并通过特征点的匹配关系推断相机运动。
• 流程：
  • 提取图像特征点。
  • 描述特征点并计算特征点匹配关系。
  • 通过特征匹配估计相机运动（后续结合几何关系）。

ORB特征

• ORB（Oriented FAST and Rotated BRIEF）是一种高效的特征点提取和描述方法，常用于特征点法中。
  • ORB特征点提取：基于FAST角点，结合图像金字塔提取多尺度特征点。
  • ORB描述子：使用BRIEF描述子（特征点局部的二值编码）加快特征匹配。
  • 优势：ORB特征兼具高效性和鲁棒性，且能处理旋转和尺度变化。

特征匹配

• 常用匹配算法：
  • 暴力匹配：直接计算每对特征点描述子的距离（如欧氏距离、汉明距离），效率低。
  • KNN匹配：为每个特征点找到最近的几个邻居，并通过比值测试筛选最佳匹配。
• 关键点均匀化：通过八叉树等方法均匀分布特征点，避免密集区域的冗余。

2D-2D对极几何

在特征点匹配基础上，2D-2D对极几何用于计算相机间的相对位姿。

对极约束

• 定义：在两幅图像中，某点 𝑝1 的匹配点 𝑝2 必定满足对极约束：$${\mathbf{p}}_2^{\top }\mathbf{F}{\mathbf{p}}_1=0$$
  其中 𝐹 是基础矩阵，描述两图像之间的几何关系。

本质矩阵

• 当相机的内参已知时，基础矩阵 𝐹可转化为本质矩阵 𝐸：$$\mathbf{E}={\mathbf{K}}^{\top }\mathbf{FK}$$
  其中 𝐾 是相机的内参矩阵。

单应矩阵

• 对于静止的平面场景，匹配点间也可以通过单应矩阵 𝐻 建立关系：$${\mathbf{p}}_2=\mathbf{H}{\mathbf{p}}_1$$
• 本质矩阵 vs 单应矩阵：
  • 本质矩阵：适用于一般场景，包含视差信息。
  • 单应矩阵：适用于平面场景或无视差运动，易退化。

位姿分解

通过分解本质矩阵 𝐸，可恢复相机的相对位姿（旋转矩阵 𝑅 和平移向量 𝑡）。

三角测量

三角测量是从两帧图像中匹配的特征点，恢复3D点坐标的关键技术。

原理

• 给定特征点在两帧图像中的投影位置${\mathbf{p}}_1,{\mathbf{p}}_2$，以及相机的内参和位姿 𝑅,𝑡，通过以下方式恢复3D点：$$\mathbf{P}=\text{Triangulate}({\mathbf{p}}_1,{\mathbf{p}}_2,\mathbf{R},\mathbf{t})$$
• 基于几何投影模型，通过线性方程或非线性优化求解3D点坐标。

验证三角化结果

• 测量点是否位于相机前方。
• 计算重投影误差是否足够小。

3D-2D: PnP问题

PnP（Perspective-n-Point）问题是已知3D点和对应的2D投影，求解相机位姿的问题。

直接线性变换（DLT）

• 基于线性方程求解相机的位姿：$${\mathbf{p}}_i=\mathbf{K}[\mathbf{R}|\mathbf{t}]{\mathbf{P}}_i$$
• 通过线性求解，快速但精度不高。

P3P方法

• 给定3对3D点和2D点的匹配关系，利用几何关系直接求解位姿。
• 通常结合RANSAC算法，剔除外点，提高鲁棒性。

最小化重投影误差

通过非线性优化，最小化重投影误差以提高精度：$${\mathbf{x}}^{\ast }=\arg \underset{\mathbf{x}}{\min }\sum _i\Vert {\mathbf{p}}_i-h({\mathbf{P}}_i,\mathbf{x}){\Vert }^2$$
其中 ℎ(⋅) 是投影函数。

3D-3D: ICP方法

3D-3D配准问题是已知两组3D点云，求解它们之间的刚体变换（旋转矩阵 𝑅 和平移向量 𝑡）。

SVD方法

• 基于点云的最近邻匹配，构造误差函数：
  $$E(\mathbf{R},\mathbf{t})=\sum _i\Vert {\mathbf{q}}_i-(\mathbf{R}{\mathbf{p}}_i+\mathbf{t}){\Vert }^2$$
  其中${\mathbf{p}}_i$和${\mathbf{q}}_i$ 是两帧中的对应3D点。
• 使用奇异值分解（SVD）求解最优刚体变换。

非线性优化方法

• 在初始位姿的基础上，利用非线性优化方法（如高斯-牛顿或LM算法）进一步减少误差，提高精度。
• 优化目标：最小化点到点或点到平面的距离误差。

总结

视觉里程计通过特征点法提取信息，结合几何约束（2D-2D对极几何、三角测量）估计相机位姿，并通过PnP（3D-2D）和ICP（3D-3D）实现更复杂场景下的位姿求解。这些方法构成了视觉里程计的核心技术体系，为SLAM中的前端跟踪提供了坚实的数学基础和实现方案。