【视觉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 必定满足对极约束： $\mathbf{p}_2^\top\mathbf{F}\mathbf{p}_1=0$
其中 𝐹 是基础矩阵，描述两图像之间的几何关系。

本质矩阵

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

单应矩阵

对于静止的平面场景，匹配点间也可以通过单应矩阵 𝐻 建立关系： $\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}^*=\arg\min_\mathbf{x}\sum_i\|\mathbf{p}_i-h(\mathbf{P}_i,\mathbf{x})\|^2$
其中 ℎ(⋅) 是投影函数。

3D-3D: ICP方法

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

SVD方法

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