【视觉SLAM:六、视觉里程计Ⅰ：特征点法】
视觉里程计（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 必定满足对极约束：
p
2
⊤
F
p
1
=
0
\mathbf{p}_2^\top\mathbf{F}\mathbf{p}_1=0
p2⊤Fp1=0
其中 𝐹 是基础矩阵，描述两图像之间的几何关系。
本质矩阵
- 当相机的内参已知时，基础矩阵 𝐹可转化为本质矩阵 𝐸：
E
=
K
⊤
F
K
\mathbf{E}=\mathbf{K}^\top\mathbf{F}\mathbf{K}
E=K⊤FK
其中 𝐾 是相机的内参矩阵。
单应矩阵
- 对于静止的平面场景，匹配点间也可以通过单应矩阵 𝐻 建立关系： p 2 = H p 1 \mathbf{p}_2=\mathbf{H}\mathbf{p}_1 p2=Hp1
- 本质矩阵 vs 单应矩阵：
- 本质矩阵：适用于一般场景，包含视差信息。
- 单应矩阵：适用于平面场景或无视差运动，易退化。
位姿分解
通过分解本质矩阵 𝐸，可恢复相机的相对位姿（旋转矩阵 𝑅 和平移向量 𝑡）。
三角测量
三角测量是从两帧图像中匹配的特征点，恢复3D点坐标的关键技术。
原理
- 给定特征点在两帧图像中的投影位置 p 1 , p 2 \mathbf{p}_1,\mathbf{p}_2 p1,p2，以及相机的内参和位姿 𝑅,𝑡，通过以下方式恢复3D点： P = Triangulate ( p 1 , p 2 , R , t ) \mathbf{P}=\text{Triangulate}(\mathbf{p}_1,\mathbf{p}_2,\mathbf{R},\mathbf{t}) P=Triangulate(p1,p2,R,t)
- 基于几何投影模型，通过线性方程或非线性优化求解3D点坐标。
验证三角化结果
- 测量点是否位于相机前方。
- 计算重投影误差是否足够小。
3D-2D: PnP问题
PnP（Perspective-n-Point）问题是已知3D点和对应的2D投影，求解相机位姿的问题。
直接线性变换（DLT）
- 基于线性方程求解相机的位姿： p i = K [ R ∣ t ] P i \mathbf{p}_i=\mathbf{K}[\mathbf{R}|\mathbf{t}]\mathbf{P}_i pi=K[R∣t]Pi
- 通过线性求解，快速但精度不高。
P3P方法
- 给定3对3D点和2D点的匹配关系，利用几何关系直接求解位姿。
- 通常结合RANSAC算法，剔除外点，提高鲁棒性。
最小化重投影误差
通过非线性优化，最小化重投影误差以提高精度：
x
∗
=
arg
min
x
∑
i
∥
p
i
−
h
(
P
i
,
x
)
∥
2
\mathbf{x}^*=\arg\min_\mathbf{x}\sum_i\|\mathbf{p}_i-h(\mathbf{P}_i,\mathbf{x})\|^2
x∗=argxmini∑∥pi−h(Pi,x)∥2
其中 ℎ(⋅) 是投影函数。
3D-3D: ICP方法
3D-3D配准问题是已知两组3D点云，求解它们之间的刚体变换（旋转矩阵 𝑅 和平移向量 𝑡）。
SVD方法
- 基于点云的最近邻匹配，构造误差函数：
E ( R , t ) = ∑ i ∥ q i − ( R p i + t ) ∥ 2 E(\mathbf{R},\mathbf{t})=\sum_i\|\mathbf{q}_i-(\mathbf{R}\mathbf{p}_i+\mathbf{t})\|^2 E(R,t)=i∑∥qi−(Rpi+t)∥2
其中 p i \mathbf{p}_i pi和 q i \mathbf{q}_i qi 是两帧中的对应3D点。 - 使用奇异值分解（SVD）求解最优刚体变换。
非线性优化方法
- 在初始位姿的基础上，利用非线性优化方法（如高斯-牛顿或LM算法）进一步减少误差，提高精度。
- 优化目标：最小化点到点或点到平面的距离误差。
总结
视觉里程计通过特征点法提取信息，结合几何约束（2D-2D对极几何、三角测量）估计相机位姿，并通过PnP（3D-2D）和ICP（3D-3D）实现更复杂场景下的位姿求解。这些方法构成了视觉里程计的核心技术体系，为SLAM中的前端跟踪提供了坚实的数学基础和实现方案。