做视觉SLAM和三维重建的朋友应该都有过这种经历手上有两路图像匹配点也提出来了但在获取两相机之间的旋转矩阵R和平移向量t时总是绕不开基础矩阵F、本征矩阵E、单应矩阵H这三个名字。教科书里的推导看着严谨真到写代码时又不知道该调哪个接口算出来的结果还经常莫名奇妙地翻车。这篇文章就把这一整套东西串起来讲清楚从几何原理到C实现再到那些容易踩的坑一次性梳理明白。这篇文章主要面向正在做双目标定、视觉里程计或者三维重建的读者也适合刚接触多视图几何、对三个矩阵概念模糊的初学者。看完之后你不仅能理解对极几何是怎么约束两幅图像上的匹配点的还能直接照着文中的C代码在OpenCV环境里把相对位姿估计、三角化这些流程跑通。我会尽量用工程视角来写少讲虚的多讲怎么用、为什么这样用。1. 两视图几何与相机姿态估计的核心问题在拿到两个相机拍摄的同一场景图像时我们最想知道的一件事是第二个相机相对于第一个相机到底转了多大角度、平移了多少距离。这个相对位姿是后续一切三维重建计算的基石不管是双目立体匹配、运动恢复结构SfM还是视觉SLAM第一步都在解这个参数。1.1 对极几何约束的直观理解我们对极几何做一个直观理解。假设空间里有一个三维点P它同时被左右两个相机看到在左右图像上分别成像为点x1和x2。如果只知道x1的像素位置却不知道点P的深度那么P在空间中可能出现的所有位置构成了从左相机光心出发的一条射线。这条射线投影到右相机图像平面上会形成一条直线x2一定位于这条直线上。这条直线被称为对极线。这个关系的奇妙之处在于它完全由两个相机的内参和相对位姿决定跟场景内容无关。不管前面是一面白墙还是密密麻麻的纹理只要相机不动对极线就不变。这种“一个点约束另一个点在一条线上”的几何关系就是整个对极几何的核心基础矩阵F正是把这个约束代数化的工具。1.2 相机模型与坐标系预处理在深入矩阵之前必须把坐标系和相机模型说清楚。一般我们用针孔相机模型空间点P在相机坐标系下的三维坐标为(X, Y, Z)投影到像素坐标系下的坐标为(u, v)。这个过程可以写成齐次坐标形式λ * [u, v, 1]^T K * [R | t] * [X, Y, Z, 1]^T内参矩阵K包含了焦距fx、fy和主点cx、cy外参R、t描述相机在世界坐标系下的位姿。如果把空间点转换到相机坐标系得到归一化坐标(x_n, y_n, 1)那么像素坐标和归一化坐标之间的关系就是[u, v, 1]^T K * [x_n, y_n, 1]^T这个归一化坐标非常关键因为本征矩阵E就是在归一化坐标系下定义的而基础矩阵F是在像素坐标系下定义的。很多同学算出来的结果不对往往就是没有分清当前坐标到底是像素坐标还是归一化坐标。2. 基础矩阵、本征矩阵、单应矩阵的数学本质与分工F、E、H这三个矩阵看起来都是3×3矩阵但它们描述的关系完全不同适用的场景也完全不同。这里我会把每个矩阵的定义、自由度、物理意义讲透并给出选型建议。2.1 基础矩阵F像素坐标系下的点线约束基础矩阵F的约束关系可以写成如下形式x2^T * F * x1 0这里x1是左图像素坐标的齐次形式x2是右图像素坐标的齐次形式。这个式子表示的是x2必须位于由x1决定的对极线上而对极线的方程就是F * x1。F的维度是3×3但它有尺度无关性同时本身是奇异矩阵秩为2所以实际只有7个自由度。F最明显的工程特点是它直接用像素坐标计算完全不需要知道相机的内参。这个特性让它在早年未标定时代特别有吸引力也给一些纯像素层面的应用比如基础矩阵引导的特征匹配提供了便利。但代价是F里面混着内参和外参的信息你没办法直接从F里把R和t干净地拆出来。如果要得到相对位姿还需要借助内参K把F转换到E。2.2 本征矩阵E去掉内参之后的位姿密码本征矩阵和基础矩阵的关系非常直接E K2^T * F * K1这里的K1、K2分别是左右相机的内参矩阵。如果是同一个相机或者两个完全相同的相机K1和K2可以都用K。E同样是一个3×3矩阵但它只包含旋转和平移信息不包含内参。E的自由度是5旋转3个自由度平移3个自由度但因为平移的全局尺度无法从单目图像中恢复所以整体减去1个自由度。E有一个重要的数学特性它的奇异值是两个相等的非零值加上一个零值。这个性质可以用来检验E的估计结果是不是合理的。我在调试时会把E做SVD分解看一眼奇异值分布是否满足这个规律如果差得很远一般说明输入匹配点或者内参K出了问题。E到R、t的分解是我在实际项目中用到最频繁的环节。分解过程基于SVD技巧可以得到四组可能的(R, t)解其中只有一组是物理上正确的。判断标准是把空间点三角化之后点必须同时在两个相机的前方。我后面会在C实现部分详细给出这段筛选逻辑。2.3 单应矩阵H平面场景中的点对点映射单应矩阵H和前两个矩阵有一个本质区别F和E描述的是点到线的约束而H描述的是点到点的约束。如果场景中的空间点都落在同一个平面上或者相机只做纯旋转而没有平移那么同一个空间点在两幅图像上的投影之间就存在一个直接的线性映射x2 H * x1H的自由度是8。因为3×3矩阵有9个元素去掉一个全局尺度因子正好是8个自由度。估计H至少需要4对不共线的匹配点这比估计F的7对或者8对更省。H在工程里有三个典型应用场景。第一是全景图像拼接因为拼接时相机通常只做旋转两幅图之间正好用单应矩阵变换对齐。第二是纯旋转场景下的位姿估计因为这种场景下F和E都会退化只有H依然稳定。第三是平面特征的视觉测量比如识别地面上的二维码或者平面标记通过H来估计相机相对于该平面的姿态。最近关于单应性矩阵变换的讨论热度也一直在涨很多在做平面AR、平面替换、影像矫正的开发者都在用这一套方法。3. C与OpenCV环境下的矩阵估计完整流程这一章是实操重点。我会按照实际工程的顺序从特征匹配一路写到姿态分解给出可以直接参考的代码片段和设计思路。3.1 特征提取与匹配点质量保障所有矩阵估计的输入都是匹配点对所以匹配质量决定了矩阵估计的上限。这里我强烈建议不要直接在原始匹配结果上估计矩阵而是先做一轮清洗。我通常使用的流程是这样的首先用ORB或SIFT提取特征点。ORB适合实时性要求高的场景缺点是特征描述子的区分度不如SIFTSIFT在重复纹理和光照变化下的稳定性更好但计算代价大。对离线标定场景我一般选SIFT对实时SLAM我会选ORB并配合金字塔层数调优。匹配阶段我习惯先用BFMatcher做暴力匹配然后开启crossCheck。crossCheck的含义是只有左图最近的匹配点和右图再做一次最近邻查询后仍然互为最优时才保留该匹配这能滤掉大量不对称的误匹配。紧接着做Lowes ratio test对每个特征点取最近邻距离和次近邻距离的比值如果比值大于0.8就认为区分度不足直接丢弃。这两个步骤下来匹配准确率通常能从60%提升到90%以上。3.2 基础矩阵F的估计代码与分析基础矩阵的估计在OpenCV里是一行函数调用std::vectorcv::Point2f points1, points2; // 已按顺序对齐的匹配点 // 使用RANSAC配合8点法估计基础矩阵重投影误差阈值为1.0像素 cv::Mat F cv::findFundamentalMat( points1, points2, cv::FM_RANSAC, 1.0, 0.99 );这里有几个参数值得展开说说。FM_RANSAC表示使用RANSAC鲁棒估计本质是反复随机抽取8对点计算候选F然后统计所有匹配点在F约束下的误差保留内点数最多的模型。误差阈值1.0表示对极距离的像素误差容忍度。在1080P图像上1.0是一个比较合理的中位值如果图像噪声大、特征定位精度差可以适当放宽到1.5甚至2.0但建议不要超过3.0否则内点会包含很多质量差的点反而降低估计精度。调试时有一个很容易被忽略的点points1和points2的坐标必须是像素坐标数据类型可以先用float。匹配点顺序必须严格对应不能在中间做任何排序或者删除操作后忘记同步。我在项目里遇到过不少次因为对匹配点做了按x坐标排序结果F矩阵彻底算成垃圾值。3.3 本征矩阵E与R,t分解实现如果相机内参K已知可以直接从匹配点估计E。OpenCV提供的接口如下cv::Mat E cv::findEssentialMat( points1, points2, cameraMatrix, // 单目或双目共用内参 cv::RANSAC, 0.99, 1.0 ); cv::Mat R1, R2, t; cv::decomposeEssentialMat(E, R1, R2, t);从E分解得到的R1、R2和t可以组合出四组解(R1, t)、(R1, -t)、(R2, t)、(R2, -t)。为了选出正确的解必须对每一组都做三角化然后统计落在两个相机前方的三维点数。下面是我常用的筛选代码框架int bestCount -1; cv::Mat bestR, bestT; for (int i 0; i 4; i) { cv::Mat R (i % 2 0) ? R1 : R2; cv::Mat tVec (i 2) ? t : -t; // 构造左右相机的投影矩阵 cv::Mat P1(3, 4, CV_64F); cv::Mat P2(3, 4, CV_64F); R1_eye.copyTo(P1(cv::Rect(0, 0, 3, 3))); // P1 [I | 0] R.copyTo(P2(cv::Rect(0, 0, 3, 3))); tVec.copyTo(P2(cv::Rect(3, 0, 1, 3))); // P2 [R | t] // 用K乘投影矩阵转到像素坐标 cv::Mat K cameraMatrix; P1 K * P1; P2 K * P2; cv::Mat points4D; cv::triangulatePoints(P1, P2, points1, points2, points4D); int count 0; for (int j 0; j points4D.cols; j) { cv::Mat X points4D.col(j); X / X.atdouble(3, 0); // 齐次坐标转欧氏坐标 double x X.atdouble(0, 0); double y X.atdouble(1, 0); double z X.atdouble(2, 0); // 判定是否在相机前方用相机坐标系的Z0来判断 if (z 0) count; } if (count bestCount) { bestCount count; bestR R.clone(); bestT tVec.clone(); } }这里有一个关键思路三角化出来的是空间点的相机坐标系坐标判断Z是否大于0就等价于点是否在相机前方。如果正确的那组解正点率明显偏低我一般会回头检查特征匹配质量或者基线长度而不是盲目加大RANSAC迭代次数。4. 单应矩阵H的估计与工程应用单应矩阵在视觉姿态估计里经常被误解成“只是拼接用的”但它在退化场景中反而是救命的工具。4.1 单应矩阵的数学模型与DLT原理单应矩阵描述的是两个平面之间的射影变换。假设空间点P落在某个三维平面上它在左图上的投影为x1在右图上的投影为x2那么x2和x1之间存在线性映射。展开后也就是说[x2, y2, 1]^T ≈ H * [x1, y1, 1]^T这里的“≈”表示在尺度意义下相等。矩阵H有8个自由度求解它的经典方法是直接线性变换DLT。DLT的核心思想是把每个点对的投影约束转化为两个线性方程然后用SVD求解最小二乘问题。理论上4对匹配点就可以求解但工程中为了抗噪声通常使用8对以上的点做最小二乘估计再用RANSAC剔除脏点。4.2 单应矩阵与基础矩阵的关系分析单应矩阵和基础矩阵并不是两个孤立的概念。当相机只做纯旋转时基础矩阵F会退化到无法估计因为此时对极线退化成了单个点原来的点到线约束坍缩成了点到点约束这个点对点的映射正好就是单应矩阵。同样当场景恰好是平面时所有匹配点的对极线变成了同一条线F虽然理论上还存在但数值估计会非常不稳定H反而成为更可靠的选择。这就是为什么我在实际项目中会使用这样一个判断流程先看相机是否基本没有平移如果是就切换到单应矩阵估计再看场景主结构是不是近似平面比如地面、墙面、桌面如果是也优先用单应矩阵只有当场景有明显的深度变化且相机有足够的平移时才放心使用基础矩阵或本征矩阵。4.3 使用cv::findHomography估计单应矩阵OpenCV中估计H的接口如下cv::Mat H cv::findHomography( points1, points2, cv::RANSAC, 3.0 );这里的3.0是RANSAC的重投影误差阈值表示在像素坐标下的最大允许误差。还有一个重要的参数变体是cv::LMEDS即最小中值法它对噪声更鲁棒但在某些情况下收敛速度不如RANSAC。我一般默认使用RANSAC当图像质量特别差、脏点比例特别高时会尝试LMEDS对比结果。需要特别注意的一点是findHomography默认的输入输出都是像素坐标。如果你要在相机归一化坐标系下使用单应矩阵需要自行转换公式是H_normalized K^{-1} * H * K。这个转换在把H用于姿态恢复时特别重要否则算出来的旋转矩阵会带有内参的畸变。我自己做过一个基于地面特征的单目视觉定位项目相机安装在小车顶部主视角朝向前下方地面基本占据图像下半部分。这个场景非常适合单应矩阵变换每一帧用地面特征点估计H再分解H得到小车相对于地面的姿态变化整体运行稳定。如果这个场景改用基础矩阵一方面地面纹理较弱导致内点率低另一方面地面近似平面容易导致对极约束退化结果会很糟糕。5. 工程实战中的问题排查与避坑技巧理论知识再多不如实战中踩一次坑记忆深刻。这一章我把常见的故障现象、原因和对策整理出来希望能帮你少走弯路。5.1 场景退化与矩阵选型速查表我把场景条件和推荐的矩阵选择总结成一张表方便在实际项目里快速决策场景条件运动/结构特征推荐选择不推荐原因纯旋转无平移只转头单应矩阵HF/E退化对极线坍缩平面场景地面、墙面等单应矩阵HH更稳F数值不稳定一般三维场景有平移深度变化本征矩阵EF可做中间量但最终要E相机位置固定完全静止无解或退化为零基线无视差不满足估计前提在使用这张表时你需要注意真实场景往往不是绝对的某一类。例如小车在地面上运动既有旋转又有平移但地面占画面比例很大这时F和H都能算但H通常更稳定。我建议的做法是同时计算F和H比较两者在RANSAC下的内点率谁的内点率高就用谁这比引一堆复杂的自适应选择算法更实用。5.2 R和t计算结果的常见错误源我遇到最多的“R和t算错”的问题基本都来自几个固定原因。第一个是坐标类型与内参不匹配。E的估计本质是在归一化坐标下完成的如果你传入的是像素坐标但内参K是旧标定参数结果会带有系统性偏差。遇到这种情况回归标定过程确认fx、fy、cx、cy精度尤其是主点偏移对E的估计影响很大。第二个是匹配点分布偏置。所有匹配点集中在图像的一个局部区域时矩阵估计的病态性会急剧增加。解决方法是使用特征点网格化均匀采样例如把图像分成8×8网格每个网格最多保留5个匹配点这样能让参与估计的点分布更均匀。第三个是退化场景未识别。前面提过纯旋转会让F僵尸化但它的残差还可能看起来很小导致你误判结果没问题。我的检测办法是看恢复的对极线是否近乎水平且排列密集如果是基本可以判定场景退化。另一个更直接的指标是RANSAC内点率如果F的内点率很高但分解出来的t的模长非常小就要怀疑纯旋转退化。5.3 三角化与尺度问题的经验分享三角化是位姿估计后的下一环但同样坑很多。cv::triangulatePoints要求输入的是4×4的投影矩阵P1、P2以及左右匹配点坐标。我特别想提醒的是类型问题在部分OpenCV版本里P矩阵必须使用CV_64F类型而points1、points2可以是float类型。如果把P矩阵误设为CV_32F结果会出现时好时坏的随机错误而且很难定位。尺度方面单目相机的E分解靠SVD只能得到归一化的平移方向t的模长通常被认为是1。这意味着你拿到的三维重建结果是在一个任意尺度下的无法反映真实的米、厘米。如果需要真实尺度必须引入其他信息源比如已知尺寸的标定物、IMU数据、轮式里程计或者双目相机的固定基线长度。5.4 OpenCV版本差异与C工程经验最后聊一下工程环境。OpenCV 4.x和3.x在特征相关接口上变动很大特别是findEssentialMat、decomposeEssentialMat这些函数的默认参数有差异可能导致新旧代码跑出不同结果。我建议团队项目统一锁定OpenCV版本尽量不要只升级系统库而不升级代码。还有一个很隐晦的坑是EIGEN和OpenCV混用时的内存布局问题。EIGEN默认矩阵存储是列主序OpenCV的cv::Mat是行主序直接对内存地址做memcpy会导致数据错乱。正确做法是使用cv::eigen2cv和cv::cv2eigen做显式转换虽然会引入一次拷贝开销但能避开大量难以察觉的内存错误。6. 一点个人心得对极几何、基础矩阵、本征矩阵、单应矩阵这套知识刚接触的人会被公式唬住觉得特别“数学”、特别难。但真正做工程后你会发现核心其实就三件事搞明白每个矩阵描述的是哪种几何关系选对适合当前场景的求解方法以及写代码时盯紧坐标类型和矩阵维度。把这三点做好双目标定、相对位姿估计、简易三维重建这些需求基本都能解决。我个人特别受益的一个习惯是每换一个数据集或者每切换一次相机都会先用可视化工具把对极线画出来检查一遍。别看这个操作简单它能直观地暴露出矩阵估计是整体正确还是局部错误比盯着数值残差高效太多。如果你连这一步都懒得做后面再精致的算法优化也可能是在错误的地基上盖房子。