4. 特征匹配与对极几何：2D-2D位姿估计
各位同学，欢迎来到第四章。今天咱们聊聊视觉SLAM里一个非常经典的问题——2D-2D位姿估计。
说白了，就是给你两张图像，你只知道图像里的像素坐标，不知道任何3D信息，怎么把相机的运动给算出来？
嗯，这其实是很多SLAM系统的起点。我记得刚入行那会儿，第一次跑通ORB-SLAM，看到它从两张模糊的图片里算出相机转了多少度，平移了多少米，那种感觉确实挺震撼的。
4.1 特征匹配：我们得先找到“共同点”
要估计两帧之间的运动，第一步是什么？
你得先知道两张图里哪些点是同一个东西。对吧？
这就涉及到特征匹配。我个人习惯把特征匹配分成三步走：
- 特征提取：在每张图里找到那些“与众不同”的点（角点、斑点等）。
- 特征描述：给每个特征点算一个“身份证号”（描述子），比如ORB描述子就是一个256位的二进制串。
- 特征匹配：根据描述子的相似度，把两帧里的点配对起来。
核心要点：匹配的质量直接决定了位姿估计的精度。如果匹配错了，后面算出来的运动基本就是错的。
我在项目中遇到过一个问题：在高速公路上，车辆快速行驶，图像模糊得厉害。ORB特征点提取出来数量少，匹配质量也差。后来我换成了光流法（LK光流）来跟踪特征点，效果反而更好。所以，不要死磕一种方法，场景变了，策略也得变。
4.1.1 常用的特征点对比
| 特征类型 | 速度 | 旋转不变性 | 尺度不变性 | 典型应用场景 |
|---|---|---|---|---|
| SIFT | 慢 | 强 | 强 | 高精度建图、离线处理 |
| SURF | 中等 | 强 | 强 | 实时性要求不高的场景 |
| ORB | 快 | 中等 | 中等 | 实时SLAM（如ORB-SLAM） |
| AKAZE | 快 | 强 | 中等 | 非线性尺度空间、边缘场景 |
我的建议：自动驾驶场景下，优先考虑ORB或AKAZE。SIFT虽然精度高，但算力消耗太大，车载芯片扛不住。
4.2 对极几何：从2D到3D的桥梁
好，假设我们已经有了匹配好的特征点对。接下来怎么算运动？
这里就要请出对极几何了。
你想想看，两个相机在不同位置拍同一个点，这个点、两个相机的光心，这三个点天然就在一个平面上。这个平面叫极平面。
极平面与两个成像平面的交线，就是极线。
这个约束关系，用数学表达就是：
p2^T * F * p1 = 0其中，p1和p2是匹配点的归一化坐标，F就是基础矩阵。
如果我们用的是归一化坐标（即去掉了内参的影响），那公式就变成：
p2^T * E * p1 = 0这里的E就是本质矩阵。
关键点：本质矩阵E包含了相机的旋转R和平移t的信息。我们只要能解出E，就能从E里分解出R和t。
4.3 八点法：求解本质矩阵
怎么解E？最经典的方法就是八点法。
为什么叫八点法？因为E是一个3x3的矩阵，有9个元素。但尺度等价性（乘以任意非零常数不影响结果），所以实际上只有8个自由度。理论上，8对匹配点就能解出E。
代码实现其实不复杂：
// 伪代码：八点法求解本质矩阵
// 输入：匹配点对 (p1, p2)，每对点都是归一化坐标
// 输出：本质矩阵 E
Matrix3x3 computeEssentialMatrix(vector<Point2f>& p1, vector<Point2f>& p2) {
// 1. 构建线性方程组 A * e = 0
// 每对点提供一个方程
Matrix A(8, 9); // 至少8个点
for (int i = 0; i < 8; i++) {
float x1 = p1[i].x, y1 = p1[i].y;
float x2 = p2[i].x, y2 = p2[i].y;
A(i, 0) = x2 * x1;
A(i, 1) = x2 * y1;
A(i, 2) = x2;
A(i, 3) = y2 * x1;
A(i, 4) = y2 * y1;
A(i, 5) = y2;
A(i, 6) = x1;
A(i, 7) = y1;
A(i, 8) = 1.0;
}
// 2. SVD分解，取最小奇异值对应的右奇异向量
SVD svd(A);
Vector9f e_vec = svd.matrixV().col(8);
// 3. 重组成3x3矩阵
Matrix3x3 E;
E << e_vec(0), e_vec(1), e_vec(2),
e_vec(3), e_vec(4), e_vec(5),
e_vec(6), e_vec(7), e_vec(8);
// 4. 强制奇异值约束：E的奇异值应为 (sigma, sigma, 0)
SVD svd_E(E);
Matrix3x3 U = svd_E.matrixU();
Vector3f S = svd_E.singularValues();
S(0) = (S(0) + S(1)) / 2.0;
S(1) = S(0);
S(2) = 0;
Matrix3x3 Sigma = S.asDiagonal();
E = U * Sigma * svd_E.matrixV().transpose();
return E;
}注意：八点法对噪声非常敏感。我曾经在实车测试中，因为路面颠簸导致图像抖动，匹配点对里混入了几个外点，结果解出来的E完全不对，分解出的平移向量指向了天上。
所以，实际工程中一定要配合RANSAC（随机采样一致性）来使用。先随机采样8个点，算一个E，然后统计有多少内点符合这个E。重复多次，选内点最多的那个E。
4.4 从本质矩阵分解R和t
解出E之后，怎么得到R和t？
嗯，这里有个小技巧。E的SVD分解形式是：
E = U * Sigma * V^T然后，R和t有四种可能的组合：
t1 = U * (0, 0, 1)^T
t2 = -U * (0, 0, 1)^T
R1 = U * W * V^T
R2 = U * W^T * V^T
其中 W = [0 -1 0; 1 0 0; 0 0 1]四种组合里，只有一种是正确的——那就是三角化之后，点在两个相机前方的那一组。
避坑指南：我曾经在分解E之后，忘了做“正深度检验”。结果算出来的R和t看起来挺合理，但实际轨迹是反的——车在往前开，我算出来它在往后倒。后来加了一个简单的深度检查，问题就解决了。
4.5 单应矩阵：当场景近似平面时
除了本质矩阵，还有一种情况很常见：场景近似在一个平面上。
比如，自动驾驶中，车辆在平坦的道路上行驶，路面上的纹理基本都在一个平面上。这时候，用单应矩阵H来建模会更稳定。
单应矩阵描述的是两个平面之间的映射关系：
p2 = H * p1求解H只需要4对匹配点（因为H有8个自由度）。
怎么判断该用E还是H？
我个人习惯的做法是：同时计算E和H，然后看哪个模型的内点更多。如果H的内点明显多于E，说明场景近似平面，用H；否则用E。
| 场景 | 推荐模型 | 最少匹配点数 | 特点 |
|---|---|---|---|
| 一般场景（非平面） | 本质矩阵E | 8对 | 需要去畸变、归一化坐标 |
| 近似平面场景 | 单应矩阵H | 4对 | 对纯旋转场景也有效 |
| 纯旋转（无平移） | 单应矩阵H | 4对 | 本质矩阵会退化 |
4.6 本章知识体系
为了让大家更直观地理解整个流程，我画了一张流程图：
4.7 工程落地中的几个坑
最后，分享几个我在实际项目中踩过的坑：
- 尺度不确定性：单目视觉估计的平移t只有方向，没有尺度。你算出来车移动了1米，但实际可能是10米。所以单目SLAM必须配合其他传感器（如轮速计、IMU）来恢复尺度。
- 纯旋转退化：如果相机只有旋转没有平移，本质矩阵E会变成零矩阵，无法求解。这时候只能用单应矩阵H。
- 特征点分布：匹配点最好均匀分布在图像上。如果所有点都集中在图像的一个角落，解出来的E会非常不稳定。
总结一下：2D-2D位姿估计是视觉SLAM的基石。核心就是三步：特征匹配 → 求解E或H → 分解出R和t。每一步都有坑，但掌握了原理，配合RANSAC和正深度检验，基本就能稳定工作了。
一个小建议：刚开始做的时候，别急着上复杂的优化。先用八点法+RANSAC跑通一个最小系统，看看效果。等基础打牢了，再慢慢加BA（光束法平差）之类的优化。
公众号:蓝海资料掘金营，微信deep3321