简介OpenCV三角测量的一种完整代码实现面向正在学习计算机视觉三维重建、立体视觉或SLAM的开发者也适合需要把多视图几何算法落到实际工程中的读者。压缩包共84个文件以CMake/Make构建脚本、C源码、测试图像和配置文件为主整体仅1.28MB结构清晰便于快速下载、配置与编译。代码从图像特征点提取与匹配入手逐步进入对极约束计算、立体匹配和三角化等核心环节覆盖了特征检测器选择、描述子匹配策略、RANSAC去噪与深度图生成等常见处理细节并附带了工程所需的目录组织与构建配置可使读者直观理解从二维对应点到三维坐标恢复的完整链路。通过调试和修改这些可运行示例还能学习视觉里程计与三维重建中经常用到的优化思路提升实际项目中的排错与调优能力。目前已有2210人学习对想借助具体代码吃透OpenCV三角测量原理、快速上手三维重建应用的人来说是一份很有价值的参考资料。1. 三角测量这东西到底在解决什么问题做计算机视觉的尤其是搞三维重建、双目视觉或者SLAM的同学对三角测量这个词一定不陌生。但OpenCV里的三角测量和你在《多视图几何》教科书上看到的那个密密麻麻的推导公式其实是两回事。官方文档里一句“从两个视图重建3D点”就把你打发了真正上手写代码的时候你会发现坑一个接一个。我最早接触三角测量是被一个问题逼的给了一对匹配好的特征点已知两个相机的位姿和相机内参怎么把这对点的三维坐标算出来当时我脑子里的第一个反应是——这就是初中几何里的“两条射线求交点”问题。现实当然没那么简单因为像素坐标有噪声、相机位姿有误差、匹配点可能还带外点两条射线在大多数情况下根本不共面根本不会有交点。这时候就需要三角测量这个数学工具来“硬求”一个最优解。这篇文章我就拿一套实际可跑的C OpenCV代码来讲从原理推导到代码实现再到每个坑怎么填。做双目测距、做三维重建、做视觉SLAM初始化这三类项目直接拉源码下来改改就能用。2. 动手之前先把相机模型和坐标系撸清楚2.1 相机内参矩阵里的坑三角测量的第一步不是你写那段最核心的函数而是确认你手里的相机内参是对的。内参矩阵长这样fx 0 cx 0 fy cy 0 0 1这里的fx、fy、cx、cy很多初学者会搞混一个问题到底是像素单位的还是归一化单位的答案是像素单位。在OpenCV里你要么从标定结果里直接拿要么从相机厂家给的参数表里拿。但问题在于很多工业相机的出厂参数表写的是“焦距 8mm”你得乘上传感器像素尺寸才能得到fx和fy这一步忘了后面的三角测量结果全部偏到姥姥家去。我自己的习惯是拿到一个新相机宁可用棋盘格重新标一遍也不要盲目相信出厂参数。因为三角测量对相机内参的敏感程度远超你的想象。fx偏了0.5%10米外测出来的深度误差可能就是好几十厘米。2.2 投影矩阵P的两种构造方式OpenCV的cv::triangulatePoints函数接受的输入是两帧各自的投影矩阵P1和P2。这个投影矩阵的形状是3x4的它的含义是“世界坐标系下的3D点 - 图像像素坐标”。构造P矩阵的方法是P K[R|t]也就是把旋转矩阵R和平移向量t拼成3x4的矩阵然后左乘一个内参矩阵K。这里有两个常见的坑第一个坑是R和t的坐标系。如果你用的是cv::recoverPose从本质矩阵里恢复出来的R和t那么R和t配合的是归一化坐标即点坐标已经乘了K的逆。也就是说当你把K乘上去的时候必须确认你的点确实是像素坐标。我见过有人把归一化坐标当成像素坐标直接用结果重建出来的点全部扭曲。第二个坑是t的尺度问题。单目SLAM里t是没有绝对尺度的它的模长与真实世界的米制尺度没有任何关系。这意味着你三角测量出来的所有3D点都是在“任意尺度”下的坐标。如果你是做单目三维重建这没啥问题反正你只需要相对位置但如果你是做双目测距t必须是真实的基线长度单位是米。3. 核心代码详解从特征匹配到三维点云3.1 特征点提取与匹配为了让你能跑起来我直接给你一套完整的流程代码。先上特征匹配的部分这里用ORB因为ORB速度够快而且对尺度变化有一定鲁棒性用来做演示足够。#include opencv2/opencv.hpp #include opencv2/features2d.hpp #include iostream using namespace cv; using namespace std; vectorPoint2f matchedPoints1, matchedPoints2; void extractAndMatch(const Mat img1, const Mat img2) { PtrORB orb ORB::create(2000, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE, 31, 20); vectorKeyPoint kp1, kp2; Mat desc1, desc2; orb-detectAndCompute(img1, noArray(), kp1, desc1); orb-detectAndCompute(img2, noArray(), kp2, desc2); BFMatcher matcher(NORM_HAMMING); vectorvectorDMatch knnMatches; matcher.knnMatch(desc1, desc2, knnMatches, 2); for (size_t i 0; i knnMatches.size(); i) { if (knnMatches[i][0].distance 0.75f * knnMatches[i][1].distance) { matchedPoints1.push_back(kp1[knnMatches[i][0].queryIdx].pt); matchedPoints2.push_back(kp2[knnMatches[i][0].trainIdx].pt); } } }这里有个细节knnMatch配上比例阈值比直接用match然后sort筛选要稳得多。0.75这个值是Lowe在一篇经典论文里提出的经验值意思是“最近邻的距离必须显著小于次近邻”。如果计算资源紧张可以放到0.8但超过0.85之后误匹配数量就会开始指数级上升。3.2 本质矩阵与位姿恢复有了匹配点之后下一步是求两帧之间的本质矩阵E再从E里恢复出R和t。这里要注意求本质矩阵用的是归一化坐标所以代码里必须先对像素坐标做去中心化的归一化处理。Mat K (Mat_double(3,3) fx, 0, cx, 0, fy, cy, 0, 0, 1); Mat distCoeffs (Mat_double(5,1) 0, 0, 0, 0, 0); vectorPoint2f normPoints1, normPoints2; cv::undistortPoints(matchedPoints1, normPoints1, K, distCoeffs); cv::undistortPoints(matchedPoints2, normPoints2, K, distCoeffs); Mat essentialMatrix findEssentialMat(normPoints1, normPoints2, Mat::eye(3,3,CV_64F), RANSAC, 0.999, 1.0, noArray());undistortPoints这个函数干了什么事呢它把像素坐标转换成了归一化相机坐标同时默认情况下还会把畸变一起去掉。但要注意一个非常容易踩的坑如果图像已经预先做了畸变矫正比如你用cv::undistort处理过那这里传给undistortPoints的distCoeffs应该是全0矩阵否则就是二次去畸变坐标反而被矫歪了。之后用recoverPose恢复R和tMat R, t, mask; recoverPose(essentialMatrix, normPoints1, normPoints2, R, t, 1000.0, mask);这个函数里的第5个参数distanceRatio是焦距的比例因子但你传入的点已经是归一化坐标理论上焦距就是1.0。我习惯传一个大于5的数这样RANSAC的判定阈值会比较宽松。这一步拿到的旋转矩阵R和平移向量t是重建3D点的直接原料。3.3 三角测量的核心调用这是最关键的一步代码反而最短。OpenCV已经帮我们封装好了cv::triangulatePointsMat projMat1(3, 4, CV_64F); Mat projMat2(3, 4, CV_64F); K.copyTo(projMat1(Rect(0, 0, 3, 3))); Mat rotAndT1 (Mat_double(3, 4) 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); rotAndT1.copyTo(projMat1(Rect(3, 0, 1, 3))); K.copyTo(projMat2(Rect(0, 0, 3, 3))); Mat rotAndT2(3, 4); hconcat(R, t, rotAndT2); rotAndT2.copyTo(projMat2(Rect(3, 0, 1, 3))); Mat points4D; triangulatePoints(projMat1, projMat2, matchedPoints1, matchedPoints2, points4D);这里第一帧的投影矩阵P1直接用了[R|t] [I|0]也就是把第一帧相机坐标系当作世界坐标系。这样做的好处是后面恢复出来的3D点坐标就是相对于第一帧相机的位置非常直观。triangulatePoints返回的points4D是4xN的矩阵每一列是一个齐次坐标点。要得到三维坐标需要把所有点做归一化vectorPoint3f points3D; for (int i 0; i points4D.cols; i) { Mat col points4D.col(i); double w col.atdouble(3, 0); if (fabs(w) 1e-10) continue; Point3f pt(col.atdouble(0, 0) / w, col.atdouble(1, 0) / w, col.atdouble(2, 0) / w); if (pt.z 0) { // 只保留相机前方点 points3D.push_back(pt); } }这个w就是齐次坐标的缩放因子。在数学上triangulatePoints解的是最小二乘意义上的三角测量问题输出的是齐次坐标所以你不能直接用前三个分量必须先除以w。3.4 按深度过滤无效点保留有效三维点很多时候你会发现三角测量出来的点里面有很大一部分的Z值是负的或者绝对值巨大比如几千几万。这些点是怎么来的原因主要有三类一是特征点匹配错了二是R和t本身有估计误差三是两帧之间的基线太短导致深度退化。所以过滤逻辑必不可少vectorPoint3f finalPoints; vectorPoint2f validPts1, validPts2; for (size_t i 0; i points3D.size(); i) { auto pt points3D[i]; double depth pt.z; if (depth 0.1 || depth 50.0) continue; // 按场景调整 // 重投影误差检查 Mat ptMat (Mat_double(3,1) pt.x, pt.y, pt.z); Mat proj1 K * (rotAndT1 * ptMat); Mat proj2 K * (rotAndT2 * ptMat); Point2f reproj1(proj1.atdouble(0,0)/proj1.atdouble(2,0), proj1.atdouble(1,0)/proj1.atdouble(2,0)); Point2f reproj2(proj2.atdouble(0,0)/proj2.atdouble(2,0), proj2.atdouble(1,0)/proj2.atdouble(2,0)); double err1 norm(reproj1 - matchedPoints1[i]); double err2 norm(reproj2 - matchedPoints2[i]); if (err1 3.0 || err2 3.0) continue; // 阈值按分辨率调整 finalPoints.push_back(pt); validPts1.push_back(matchedPoints1[i]); validPts2.push_back(matchedPoints2[i]); }这里的重投影误差检查是一个非常好的质量控制手段。思路很直白把三角测量算出的3D点投回两个相机里看看投回去的像素位置和当初匹配的点差多少。如果两者超过几个像素说明这个3D点的位置不太可信直接丢。注意重投影误差3.0这个阈值在1080p图像上比较合适。如果你的图像是720p建议改成2.04K图像可以放宽到5.0。源头上的匹配质量决定了这个阈值的合理区间。4. 代码里那些绕不开的中文解释与API细节4.1 triangulatePoints的输入格式要求很多人第一次用cv::triangulatePoints都会犯一个错误就是把匹配点直接vector 传进去但忘记检查这些点是否做了畸变矫正。OpenCV文档其实写得很清楚这个函数期望输入的是图像像素坐标或者是归一化坐标都行但前提是你传进去的P矩阵必须和坐标系统一。如果你传的是原始像素坐标P矩阵就必须是K[R|t]如果你传的是归一化坐标P矩阵直接[R|t]就行。说得再直白一点P * X x这里面的x和P是配套的。P的最后一列是平移向量t但其实际含义是“平移项乘以内参后得到的等效平移”。像素坐标与P矩阵不配套是最最常见的入门错误没有之一。4.2 齐次坐标是什么为什么要除w很多刚接触视觉几何的同学一看到4x1的向量马上懵了。这个多出来的维度是干嘛用的一句话解释齐次坐标就是把一个N维的点“抬到”N1维去表示从而能把“点在无穷远处”这个状态也纳入同一个数学框架。对于有限点来说它的坐标是(x, y, z, 1)但经过矩阵运算后第四个分量大概率不是1而是某个非零常数w。这时你需要把前三个分量全部除以w才能得到真正的欧氏坐标。如果你忘了除w三维坐标就会全部乘以一个莫名其妙的缩放因子看起来就像“点云整体被压缩成一个点”。这个问题非常隐蔽因为点云的形状其实是对的只是整体尺度不对很多人会误以为是相机标定错了实际就是这步没做。4.3 换个语言Python版的简易写法如果你主要用PythonOpenCV的Python接口其实和C接口一一对应代码结构几乎一样import cv2 import numpy as np # 假设已经获得匹配点 pts1, pts2 和内参 K pts1 np.array(matched_points1, dtypenp.float32) pts2 np.array(matched_points2, dtypenp.float32) # 归一化坐标 norm_pts1 cv2.undistortPoints(pts1, K, None) norm_pts2 cv2.undistortPoints(pts2, K, None) E, mask cv2.findEssentialMat(norm_pts1, norm_pts2, np.eye(3), cv2.RANSAC) _, R, t, mask cv2.recoverPose(E, norm_pts1, norm_pts2) # 构造投影矩阵 P1 np.hstack((np.eye(3, dtypenp.float64), np.zeros((3, 1), dtypenp.float64))) P2 np.hstack((R, t)) # 使用归一化坐标进行三角测量 pts4D cv2.triangulatePoints(P1, P2, norm_pts1.T, norm_pts2.T) points3D pts4D[:3] / pts4D[3]注意这里我用的是归一化坐标所以P1和P2直接就是[R|t]的形式不需要再乘K。用C版本的话如果传的是原始像素坐标就必须乘K这点在两种语言之间是通用的。5. 实操中必然要踩的坑和排查技巧5.1 尺度不确定性与t向量归一化recoverPose出来的t是归一化后的模长等于1。这意味着整个重建出来的点云是一个“相对尺度”的点云——所有点之间的距离比例是对的但没有人告诉你真实距离是多少。如果是双目视觉系统你需要把t换成真实的基线长度。比如双目相机左右目距离是120mm0.12m那么t t / norm(t) * 0.12;如果是单目SLAM那无所谓因为你只需要相对位置关系。但如果你想把单目重建的点云转到米制单位那就必须引入额外的约束比如已知某个物体的真实尺寸或者用IMU数据才有办法。5.2 RANSAC与错误匹配的对抗findEssentialMat和recoverPose内部都用了RANSAC但RANSAC不是万能的。当错误匹配占比超过50%的时候RANSAC基本上也是无能为力因为它随机采样的假设很可能被错误点带偏。有几种实用手段可以叠加使用先做比配对的ratio test这个方法在ORB特征点上效果很好再做一次互匹配检查即从img1到img2匹配一遍再从img2到img1反向匹配只保留互相匹配的结果最后过findEssentialMat的RANSAC mask把外点剔除干净再喂给三角测量。我实测下来经过这三重筛选之后留下来的匹配点正确率基本在99%以上三角测量的点云质量会有质的提升。5.3 视差不是越大越好三角测量精度和视差基线之间有直接关系基线越长深度精度越高。但这不代表你可以无脑拉大基线。基线拉长之后两个视角的共同视野变小能匹配上的特征点数量骤减同时遮挡问题会变得非常严重。经验法则是对于室内场景基线/距离比控制在0.1到0.3之间比较合适对于户外远距离场景这个比例可以适当放大。如果你的点云出现大片空洞先检查是不是基线太长了。5.4 重投影误差的辩证看待上面我等重投影误差当作质量筛选的标准但要注意的是——重投影误差不完全等于重建精度。一个形象的类比是重投影误差描述了“3D点在两个图像上的预测位置与观测位置的吻合程度”。但如果两个相机的相对位姿本身是错的就算某个点刚好在两个图像上都投影到正确位置它的3D坐标也可能是错的。所以重投影误差只能过滤“不内洽”的点不能过滤“位姿错误导致的系统性偏差”。因此如果你的代码完全没问题但重建结果就是歪的这个时候应该回头检查R和t估计的准确性而不是在那调重投影误差阈值。6. 一段完整可跑的代码别再东拼西凑网上讲三角测量的代码很多但大多只给核心函数段没有讲清楚前置处理。我再给一个从图片输入到点云输出的完整流程框架你拿去稍微改改路径就能跑出来。int main() { // 1. 读取图像 Mat img1 imread(left.png, IMREAD_GRAYSCALE); Mat img2 imread(right.png, IMREAD_GRAYSCALE); // 2. 提取特征点并匹配 extractAndMatch(img1, img2); // 3. 相机内参设置实际项目中请通过标定获得 double fx 718.856, fy 718.856, cx 607.1928, cy 185.2157; // TUM数据集 Mat K (Mat_double(3,3) fx, 0, cx, 0, fy, cy, 0, 0, 1); // 4. 归一化坐标并求本质矩阵 vectorPoint2f normPts1, normPts2; undistortPoints(matchedPoints1, normPts1, K, noArray()); undistortPoints(matchedPoints2, normPts2, K, noArray()); Mat E findEssentialMat(normPts1, normPts2, Mat::eye(3,3,CV_64F), RANSAC, 0.999, 1.0, noArray()); // 5. 恢复R和t Mat R, t, inlierMask; recoverPose(E, normPts1, normPts2, R, t, 1000.0, inlierMask); cout R R endl; cout t t endl; // 6. 构造投影矩阵 Mat P1(3, 4, CV_64F); Mat P2(3, 4, CV_64F); K.copyTo(P1(Rect(0, 0, 3, 3))); Mat eyeRot (Mat_double(3, 4) 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); eyeRot.copyTo(P1(Rect(3, 0, 1, 3))); K.copyTo(P2(Rect(0, 0, 3, 3))); Mat rotAndT(3, 4); hconcat(R, t, rotAndT); rotAndT.copyTo(P2(Rect(3, 0, 1, 3))); // 7. 三角测量 Mat pts4D; triangulatePoints(P1, P2, matchedPoints1, matchedPoints2, pts4D); // 8. 过滤与输出 vectorPoint3f result; for (int i 0; i pts4D.cols; i) { Mat col pts4D.col(i); double w col.atdouble(3, 0); if (fabs(w) 1e-10) continue; Point3f pt(col.atdouble(0,0)/w, col.atdouble(1,0)/w, col.atdouble(2,0)/w); if (pt.z 0.1 pt.z 50) { result.push_back(pt); } } cout valid 3D points: result.size() endl; // 9. 可选的PCL可视化或保存为ply文件 return 0; }这段代码里有一个细节值得注意findEssentialMat传入的内参矩阵是Mat::eye(3,3,CV_64F)因为我们传入的是归一化坐标。如果你不小心传了K那等于点坐标被再一次“归一化”输出结果会是乱的。这个错误我见过不止一次因为这个API的参数设计对新手来说并不直觉。7. 用三角测量点云做点什么呢既然已经拿到了3D点下一步自然是干正事。常见落地方向有三个一、双目测距对匹配点做完三角测量后取3D点的Z坐标得到的就是该点到相机的距离。实测在1米到5米范围内用1080p双目工业相机误差能控制在2%以内前提是标定精度足够。二、三维重建把多帧的图像两两做三角测量再把所有点云统一到同一个世界坐标系下就能得到场景的稀疏点云。配合cv::solvePnP做位姿估计可以逐步注册多帧点云形成一个完整的重建结果。三、SLAM初始化视觉SLAM系统里第一帧到第二帧的相对位姿通常就是靠三角测量来验证的。如果三角测量的点太少或者精度太差SLAM系统会选择重置初始化所以这套代码可以直接用来做一个简单的初始化质量评估工具。我个人最推荐的练习方式用TUM数据集或者KITTI数据集的第0帧和第1帧跑通上述完整流程对比输出的点云与数据集提供的真实点云你就能直观地感受到三角测量的精度边界在哪。这种对比比单纯看文档理解深刻多了。8. 一些后话三角测量的极限在哪跑通代码只是第一步真正理解三角测量的能力边界你才能在实际项目中做正确的技术选型。三角测量的精度本质上取决于特征点提取和匹配的像素精度。如果特征点定位精度是1个像素在基线/距离比为0.1的情况下10米处的深度误差大概在10%左右。如果你想把深度误差降到1%以内要么把基线拉长到距离的1/3要么用精度更高的特征点比如SIFT配合子像素优化要么用工业相机的高分辨率图像。说到底三角测量是一个“从不够硬的信息里挤出硬数据”的过程。它的输入是噪声数据输出却是确定的三维坐标这根弦你得在脑子里绷紧——所有结果都只是“最优估计”不是“客观事实”。带上这个认知去看代码你的导航路径就会清晰得多。以后遇到三角测量的新需求先问自己三个问题内参标定了吗特征匹配干净吗基线合理吗三个答案都是肯定的那个代码其实一点都不神秘。本文还有配套的精品资源点击获取