第三章 特征点法前端：ORB特征提取与匹配、特征匹配优化、RANSAC剔除误匹配、PnP求解位姿
各位同学，欢迎来到视觉SLAM前端最核心的一章。说实话，特征点法前端是整个SLAM系统的“眼睛”和“大脑”——眼睛负责看，大脑负责算。今天我们就来聊聊ORB特征、匹配优化、RANSAC去噪，以及最后的PnP位姿求解。这一套流程走下来，你就能从两帧图像里算出相机的运动了。
3.1 ORB特征提取与匹配
ORB特征，全称Oriented FAST and Rotated BRIEF。说白了，它就是把FAST角点和BRIEF描述子结合，再给它们加上方向和尺度信息。我最早接触ORB时，觉得它比SIFT快太多，比SURF也轻量，非常适合嵌入式设备。
ORB的核心步骤其实就两步：
- 关键点提取：用FAST检测角点，然后计算方向（灰度质心法）
- 描述子计算：用BRIEF描述子，但加上了旋转不变性
我在项目中遇到过一个问题：ORB在纹理稀疏的场景下，特征点数量会急剧下降。这时候我通常会降低FAST阈值，或者改用自适应阈值策略。嗯，这里要注意，阈值太低会引入大量噪声点，后面匹配会很痛苦。
关键参数经验值：
- FAST阈值：默认20，纹理弱时降到10-15
- 金字塔层数：8层，尺度因子1.2
- 每层提取点数：1000-2000，太多会拖慢匹配
匹配阶段，我习惯用汉明距离做暴力匹配。为什么用汉明距离？因为BRIEF描述子是二进制串，异或运算比欧氏距离快一个数量级。代码实现大概是这样：
// ORB特征匹配示例
cv::Ptr<cv::ORB> orb = cv::ORB::create(1000);
std::vector<cv::KeyPoint> kp1, kp2;
cv::Mat desc1, desc2;
orb->detectAndCompute(img1, cv::Mat(), kp1, desc1);
orb->detectAndCompute(img2, cv::Mat(), kp2, desc2);
cv::BFMatcher matcher(cv::NORM_HAMMING);
std::vector<cv::DMatch> matches;
matcher.match(desc1, desc2, matches);3.2 特征匹配优化
直接暴力匹配的结果，你想想看，里面肯定混了大量错误匹配。我曾经在一个室外场景中，暴力匹配的正确率只有30%左右。所以我们必须做匹配优化。
我个人最常用的优化手段有三个：
- 距离阈值筛选：只保留最小距离的2-3倍以内的匹配
- 交叉验证：从img1到img2匹配，再从img2到img1匹配，只有双向一致才保留
- 最近邻与次近邻比值：如果最近距离远小于次近距离，说明匹配可靠
我的经验：交叉验证虽然增加计算量，但能显著提升匹配质量。在实时性要求高的场景，可以只用比值法，阈值设为0.7-0.8。
优化后的匹配数量通常会减少30%-50%，但正确率能提升到80%以上。这一步值得做，因为后面的RANSAC和PnP都依赖高质量的匹配点。
3.3 RANSAC剔除误匹配
即使做了匹配优化，仍然会有一些“顽固”的错误匹配。这时候RANSAC就派上用场了。RANSAC的全称是Random Sample Consensus，随机抽样一致。它的核心思想很简单：从数据中随机选一小部分，拟合模型，然后看其他点是否符合这个模型。
在视觉SLAM中，我们通常用RANSAC来求解基础矩阵或单应矩阵，然后剔除不符合该几何约束的匹配点。我习惯用基础矩阵，因为它在一般场景下更鲁棒。
避坑指南：我曾经在纯旋转运动下用基础矩阵RANSAC，结果所有匹配都被剔除了。后来才意识到，纯旋转时基础矩阵退化，应该用单应矩阵。所以，先判断运动类型再选择模型，是个好习惯。
RANSAC的迭代次数怎么设？理论上，迭代次数越多，找到正确模型的概率越大。但实际中，我一般设200-500次，内点阈值设为1-3像素。如果场景噪声大，可以适当提高阈值。
经过RANSAC后，剩下的匹配点就是“内点”了。这些内点才是我们用来求解位姿的“精兵强将”。
3.4 PnP求解位姿
PnP，Perspective-n-Point，就是已知3D点和对应的2D投影，求解相机位姿。在SLAM中，我们通常用前一帧的3D地图点和当前帧的2D特征点来求解当前帧的位姿。
PnP的求解方法有很多：
- P3P：只用3个点，速度快但容易受噪声影响
- EPnP：用4个控制点，效率高，精度也不错
- 直接线性变换（DLT）：需要6个点，简单直接
- BA优化：用所有点做非线性优化，精度最高
我个人在工程中常用EPnP + BA的组合。先用EPnP快速得到一个初始解，再用BA做精细优化。这样既保证了实时性，又保证了精度。
// OpenCV PnP求解示例
cv::Mat rvec, tvec;
std::vector<cv::Point3f> points3d; // 3D地图点
std::vector<cv::Point2f> points2d; // 2D特征点
cv::Mat K; // 相机内参
// 使用EPnP求解
cv::solvePnP(points3d, points2d, K, cv::Mat(), rvec, tvec, false, cv::SOLVEPNP_EPNP);
// 可选：BA优化
cv::solvePnPRefineLM(points3d, points2d, K, cv::Mat(), rvec, tvec);注意：PnP至少需要4个不共面的3D-2D匹配点。如果匹配点少于4个，系统就会退化。我在实际部署时，会设置一个最小内点数阈值，比如10个，低于这个数就认为跟踪丢失。
求解出的rvec和tvec就是当前帧相对于参考帧的旋转和平移。有了这个，我们就完成了两帧之间的位姿估计。整个前端流程也就闭环了。
本章知识体系
下面这张图总结了特征点法前端的完整流程，从图像输入到位姿输出，每一步都环环相扣：
从图中可以看到，整个流程是线性的，但每一步都有优化空间。我个人觉得，匹配优化和RANSAC是最容易被忽视的环节。很多初学者把精力都放在特征提取和PnP上，结果匹配质量差，位姿估计自然不准。
好了，这一章的内容就到这里。特征点法前端是SLAM的基础，也是后续后端优化的前提。希望你能亲手跑一遍代码，感受一下从图像到位姿的完整过程。