视觉SLAM第7讲｜特征点法、对极几何与位姿求解全梳理-平芜编程栈

视觉SLAM第7讲｜视觉里程计(上)：特征点法、对极几何与位姿求解全梳理

如果说前面几讲的刚体运动、李群李代数是SLAM的「数学工具包」，那从这一讲开始，我们就正式进入视觉SLAM的核心前端模块——视觉里程计（Visual Odometry, VO）。

简单说，VO的任务就是：输入相邻两帧图像，输出这两帧之间相机的运动。第7讲聚焦最经典、落地性最强的特征点法VO，从特征提取匹配，到对极几何、PnP、ICP，把两帧图像求解相机运动的主流方案一次性讲透。

一、特征点法：先找「标志性像素」再算运动

直接拿整幅图像算运动？像素量太大、光照变化、物体形变都会让结果崩掉。工程界的成熟做法是：先从图像里挑一批「辨识度高、不容易随光照视角变化」的点，靠这些点来算运动。这些点就叫特征点。

1.1 特征点的两个核心部分

关键点（KeyPoint）：这个点在图像里的位置、尺度、方向。相当于「地标在哪」。
描述子（Descriptor）：这个点周围像素的特征编码，相当于这个地标的「身份证」。两个点描述子越相似，是同一个空间点的概率就越高。

💡通俗解释：就像你在城市里导航，不靠看整条街的每一块砖，只靠几个标志性路口、大楼就能判断自己的位置和运动。特征点就是图像里的「标志性建筑」，大幅压缩计算量，还提升了鲁棒性。

1.2 ORB特征：速度与平衡的王者

ORB全称Oriented FAST and Rotated BRIEF，是目前SLAM领域最常用的实时特征，没有之一。

FAST角点检测
原理非常直观：比较中心点和周围一圈像素的灰度差，如果有足够多的像素和中心点差值超过阈值，就判定为角点。
最大优点：极快，适合实时场景；缺点：原生FAST没有方向、没有尺度不变性。

图1：FAST特征点

ORB的改进
用灰度质心法给每个角点计算主方向，再用图像金字塔做多尺度检测，让ORB具备了旋转不变性和尺度不变性。

图2：使用金字塔可以匹配不同缩放倍率下的图像

BRIEF二进制描述子
用256位0/1二进制串描述一个特征点。匹配时直接算汉明距离（不同位的数量），速度比浮点描述子快一个数量级。

💡经验之谈：ORB不是精度最高的特征，但一定是同算力下性价比最高的。ORB-SLAM系列能稳定实时运行，很大程度上就是选对了ORB这个特征。

1.3 特征匹配：把两帧的同一个点对上

图3：两帧图像间的特征匹配

匹配就是在两帧特征点里，找出对应同一个空间点的点对。

暴力匹配：逐个计算描述子距离，找最近的那个。精度高但速度慢，特征点少的时候用。
FLANN快速匹配：近似最近邻算法，特征点数量大的时候速度优势明显。
匹配筛选：这步比匹配本身还重要。常用比率测试（最近距离/次近邻距离 < 阈值）、交叉匹配，剔除大量误匹配。

💡踩坑提示：匹配不是越多越好。一堆错误的匹配对，会把后面的运动求解直接带偏。优质的几十对匹配，远好于劣质的几百对。

1.4 实践环节

本讲代码演示了两种实现：

调用OpenCV内置API提取ORB特征、完成匹配；
手写实现FAST角点检测和BRIEF描述子，从底层理解特征是怎么来的。

二、2D-2D 对极几何：单目两帧算运动的核心

当我们只有两帧单目图像、没有任何先验三维信息时，靠对极几何就能算出相机的相对运动。这是单目VO最底层的理论基础。

2.1 对极约束：五点共面的几何本质

两个相机光心O₁、O₂，空间点P，两帧成像点x₁、x₂，这五个点一定在同一个平面（对极平面）上。这个约束就是对极约束。

图4：对极几何约束

写成矩阵形式就是：
x₂的转置 · E · x₁ = 0

其中：

x₁, x₂：两帧点的归一化坐标（像素坐标去掉内参，投影到z=1归一化平面）

💡通俗解释：归一化坐标，就是把像素坐标换算到相机前方1米虚拟平面上的坐标，消除相机镜头焦距的影响。

E：本质矩阵（Essential Matrix），只和相机的相对位姿R（旋转）、t（平移）有关。

2.2 本质矩阵：性质与求解

本质矩阵是3×3矩阵，秩为2，共5个自由度（旋转3个+平移3个，减去1个尺度不确定性）。

💡通俗解释：自由度代表未知量个数。旋转有3个自由度，平移有3个自由度，但单目相机只能判断平移方向，无法判断距离长短，所以减掉1个自由度。

求解方法：八点法，用8对匹配点就可以线性解出本质矩阵。

💡通俗解释：凑齐8组匹配好的特征点，列方程组直接解出E矩阵。

分解：从本质矩阵里可以分解出相机的旋转R和平移t，一共4组解，只有一组深度全为正，就是正确解。

💡经典问题：单目的尺度不确定性
本质矩阵只能确定平移的方向，不能确定长度。你把轨迹和地图同时放大2倍，对极约束依然成立。这是单目天生的问题，所以实际系统里要么用双目/深度相机定尺度，要么用IMU融合恢复尺度。
💡通俗解释：单目相机分不清，是物体离得远尺寸大，还是物体离得近尺寸小，没法直接知道真实距离。

2.3 单应矩阵：平面场景的专属变换

当场景中大部分点都落在同一个平面上（比如地面、墙面），两帧之间的像素变换可以用单应矩阵（Homography）H描述：
x₂ = H · x₁

单应矩阵也是3×3，8个自由度，用四点法即可求解。

💡应用场景：
纯旋转运动、或者场景是平面时，用单应分解位姿比本质矩阵更稳；
工程里通常E和H同时算，最后选重投影误差小的那个结果。

2.4 实践：对极约束求解相机运动

代码中演示了两种实现：

调用OpenCV的findEssentialMat + recoverPose一键求解；
手写八点法求解本质矩阵，再手动分解出R和t，验证结果。

三、三角测量：算出空间点的三维坐标

算出了相机相对运动，怎么得到特征点的三维坐标？靠三角测量。

原理很直观：两个相机光心分别向各自的成像点作射线，两条射线的交点就是空间点的三维位置。
理想情况下两条射线严格相交；实际有噪声，两条线不相交，就用最小二乘法求解最近点。

💡关键结论：三角测量的精度和基线长度正相关。
基线越长（两帧相机离得越远），深度估计越准；基线极短的时候，深度误差会爆炸。这也是为什么单目启动时深度不准，走一段基线拉开之后精度就上来了。
💡通俗解释：就像人两只眼睛，左右眼分开距离越大，判断物体远近越准；两眼挨在一起，很难判断距离。

实践环节中，会用对极几何算出的位姿，三角化出所有匹配点的深度，验证结果的合理性。

四、3D-2D PnP：SLAM最常用的位姿求解

如果我们已经有了地图点的三维坐标，来了一帧新的2D图像，匹配上之后求当前相机位姿——这就是PnP（Perspective-n-Point）问题。

这是SLAM里最常见的位姿求解场景：地图已经有了，新来一帧，算自己在哪。比2D-2D对极几何用得还要频繁。

4.1 求解方法

直接线性变换（DLT）
6对点即可线性求解，直接得到代数解。优点是快，缺点是没有利用几何约束，精度一般。
最小化重投影误差（主流方案）
先用DLT给初值，再以重投影误差为目标函数，用非线性优化迭代求精。

💡重投影误差通俗解释：把3D地图点，按照估算出来的相机位置，反向投影到图像上，得到预测像素坐标；拿预测坐标和图像上真实检测到的像素坐标做差值，就是重投影误差。SLAM优化就是不断缩小这个误差。

工程上还有EPnP、UPnP等更高效的解法，OpenCV的solvePnP系列函数都有实现。

4.2 实践：求解位姿

代码演示了两种实现：

调用OpenCV的solvePnP一键求解；
手写高斯牛顿法，优化重投影误差求解位姿，从底层理解优化过程。

五、3D-3D ICP：点云匹配求变换

如果两帧都有三维点云（比如双目相机、RGB-D相机、激光雷达），两组3D点之间求刚体变换，就是ICP（Iterative Closest Point）。

5.1 求解方法

SVD分解法
当两组点已经一一对应好了，直接用SVD分解就能解析解出R和t，一步到位。
非线性优化法
点对应不精确的时候，用迭代优化的方式，最小化点到点的距离误差。

💡踩坑提示：ICP的命门是「点对应」。
如果点和点没对应对，ICP会直接收敛到错误的结果。所以实际工程中，一般先靠特征匹配做点对应，再用ICP精修；或者用点到面ICP等改进版本。

5.2 实践：求解ICP

代码分别实现了SVD解析解和非线性优化迭代解，对比两种方法的结果。

六、方法选择指南

一张表搞懂什么时候用什么方案：

场景	首选方法	核心输入
两帧都是2D单目，无先验3D	对极几何+三角测量	像素匹配对
已有3D地图点，新帧是2D	PnP	3D点+对应2D像素
两帧都是3D点云	ICP	两组3D点对