1. 项目概述从零构建双目视觉的“尺子”最近在做一个嵌入式项目需要在资源受限的设备上实现一个简单的测距功能。市面上的方案要么太贵要么依赖OpenCV这类“重型”库对硬件要求不低。于是我决定自己动手用纯C实现一套双目视觉的三角测量核心算法完全绕开OpenCV。这听起来有点“造轮子”但当你真正理解从两张二维图片中计算出三维坐标的每一步时那种对原理的掌控感是调用现成API无法比拟的。这个项目非常适合想深入理解计算机视觉几何基础的朋友或者需要在没有OpenCV环境的嵌入式、工控场景下实现三维感知的开发者。整个过程就像是用最基础的数学工具亲手打造一把测量三维空间的“尺子”。2. 核心原理拆解三角测量到底在算什么在开始写代码之前我们必须把双目视觉三角测量的数学原理吃透。很多人一上来就找代码结果参数调来调去都不对根本原因是对原理一知半解。2.1 从人眼到相机视差如何产生深度我们人类能用双眼判断距离核心在于“视差”。当你交替闭上一只眼睛看同一个物体时会发现它在背景上的位置发生了偏移。大脑就是根据这个偏移量视差来估算距离的。双目相机系统模仿了这一过程。假设我们有两个完全相同的相机它们的成像平面在同一平面上且光轴平行——这就是经典的双目平行光轴模型。一个三维空间点P在左相机成像平面上的投影点是$p_l (x_l, y)$在右相机上的投影点是$p_r (x_r, y)$。注意因为在理想平行配置下两个相机的Y坐标是相同的。那么视差$d$的定义就是$d x_l - x_r$。这里有一个关键点对于相机前方的物体它在右相机图像中的位置会比在左相机图像中更靠左假设左相机是基准。因此$x_r x_l$视差$d$为正值。物体越近这个位置差异视差就越大物体越远差异越小直至趋近于零。2.2 三角测量公式推导从像素到三维坐标知道了视差如何换算成真实的三维坐标呢这需要用到相似三角形的几何关系。假设两个相机的焦距都是$f$它们的光心距离基线长度为$B$。根据上图在脑海中构建或画出来的相似三角形关系我们可以得到 $ \frac{Z}{f} \frac{B}{d} $ 其中$Z$就是我们要求的深度物体到相机平面的垂直距离。由此我们得到了最核心的深度计算公式 $ Z \frac{f \cdot B}{d} $得到深度Z后三维空间点P的X和Y坐标也可以根据小孔成像模型反推出来 $ X \frac{x_l \cdot Z}{f} $ $ Y \frac{y \cdot Z}{f} $ 这里我们通常以左相机的光心为世界坐标系原点。注意这个公式成立的前提是前面提到的理想平行光轴模型并且$x_l, y$是已经转换到以图像物理中心即光轴与成像平面交点为原点的坐标系下的坐标而不是图像左上角为(0,0)的像素坐标。这是我们后续代码中必须进行的一步关键坐标转换。2.3 为何选择不用OpenCV这可能是很多人的第一个疑问。OpenCV已经提供了cv::triangulatePoints这样成熟的函数为什么还要自己实现依赖与部署OpenCV是一个庞大的库。在嵌入式设备如ARM Cortex-M系列、某些边缘计算盒子或要求极简依赖的工业软件中引入整个OpenCV可能不现实或代价高昂。纯C实现的核心算法编译后可能只有几十KB。理解与掌控调用库函数是一个黑盒。当三角测量结果出现系统性偏差时你很难定位问题是出在校准、匹配还是三角计算本身。自己实现一遍对每一个参数如焦距、主点、基线的物理意义和影响都会有刻骨铭心的理解。定制与优化你的应用场景可能非常特殊。例如只需要计算特定区域的深度或者需要对三角测量过程加入特殊的约束或滤波。自己写的代码可以随心所欲地修改和优化。学习价值这是学习计算机视觉多视图几何不可逾越的一步。自己推导并实现知识才会变成你自己的。3. 系统设计与关键模块一个完整的、不依赖OpenCV的双目三角测量系统可以分解为以下几个核心模块。我们将按照数据处理的流水线来设计。3.1 模块一相机模型与参数管理这是整个系统的基石。我们需要一个数据结构来精确描述相机的内部参数内参和两个相机之间的相对位置关系外参。内参Intrinsics描述相机自身的成像几何。焦距fx, fy以像素为单位。由于制造工艺x和y方向的焦距可能有微小差异。主点cx, cy图像物理中心光轴与成像平面交点的像素坐标。畸变系数k1, k2, p1, p2, k3...描述镜头导致的图像形变。这是最复杂的部分但在我们追求简洁和速度的初始版本中可以假设输入的图像已经过畸变校正。这是一个重要的简化但非常实用。在实际项目中校正可以在上位机用OpenCV完成然后将校正后的图像和相机内参校正后的下发给我们的C程序。外参Extrinsics描述右相机相对于左相机的位姿。旋转矩阵R3x3理想平行模型中R是单位矩阵。平移向量T3x1其中$T[0]$就是基线长度B以米为单位。在平行模型中T只有X分量有值即$T [B, 0, 0]^T$。在代码中我们可以设计一个Camera类来管理这些参数。// 示例一个简化的相机参数结构体 struct CameraIntrinsics { double fx, fy; // 焦距 (像素) double cx, cy; // 主点 (像素) // 暂不考虑畸变 }; struct CameraExtrinsics { // 假设我们已经将右相机图像旋转校正到与左相机平行 // 因此旋转矩阵R是单位阵我们只关心平移向量T double baseline; // 基线长度 (米)对应 T[0] // 更一般的情况可以存储完整的3x1平移向量 T double Tx, Ty, Tz; }; class StereoCameraSystem { public: StereoCameraSystem(const CameraIntrinsics leftIntrinsics, const CameraIntrinsics rightIntrinsics, const CameraExtrinsics extrinsics); // 核心三角测量函数 bool triangulate(double xl, double yl, // 左图像匹配点像素坐标 double xr, double yr, // 右图像匹配点像素坐标 double X, double Y, double Z) const; // 输出世界坐标(左相机坐标系) private: CameraIntrinsics leftCam_, rightCam_; CameraExtrinsics extrinsics_; // 内参矩阵的逆预先计算以提高效率 Eigen::Matrix3d leftK_inv_, rightK_inv_; };实操心得即使目标是“不用OpenCV”在开发调试阶段强烈建议用OpenCV或Matlab等工具先对相机进行高精度标定获取准确的内外参。将这些参数作为常量硬编码或配置文件读入你的纯C程序。不要试图自己写标定算法那是一个更复杂的领域。我们的目标是“使用”标定结果而不是“实现”标定过程。3.2 模块二图像匹配与视差计算这是双目视觉中最具挑战性的一环。给定左图的一个点如何在右图中找到它的对应点对于平行光轴模型由于极线是水平的我们只需要在右图的同一水平行即相同的y坐标上进行搜索即可这大大简化了问题。这就是所谓的“极线校正”的目标。匹配算法选择 对于自己实现的轻量级系统块匹配Block Matching是一个很好的起点。SAD绝对误差和计算简单速度快。cost Σ|I_left(xi, yj) - I_right(x-di, yj)|其中d是待测试的视差。SSD误差平方和对差异更敏感但计算量稍大。ZNCC零均值归一化互相关对光照变化具有较好的鲁棒性但计算最复杂。在我们的C实现中可以从SAD开始。我们需要设定一个搜索范围minDisparity到maxDisparity和一个匹配窗口大小如5x5, 7x7。// 简化的SAD块匹配函数示例 int computeDisparitySAD(const uint8_t* leftImg, const uint8_t* rightImg, int imgWidth, int imgHeight, int x, int y, int windowRadius, int minDisp, int maxDisp) { int bestDisparity 0; int minCost INT_MAX; for (int d minDisp; d maxDisp; d) { if (x - windowRadius - d 0 || x windowRadius imgWidth) { continue; // 边界检查确保窗口在图像内 } int cost 0; for (int j -windowRadius; j windowRadius; j) { for (int i -windowRadius; i windowRadius; i) { int idxLeft (y j) * imgWidth (x i); int idxRight (y j) * imgWidth (x i - d); // 注意x - d cost std::abs(leftImg[idxLeft] - rightImg[idxRight]); } } if (cost minCost) { minCost cost; bestDisparity d; } } return bestDisparity; }视差图对左图的每一个像素或感兴趣区域的像素执行上述匹配得到的结果就是一个与左图同尺寸的“视差图”Disparity Map每个像素值代表该点的视差d。踩坑记录匹配窗口大小的选择是精度和速度的权衡。窗口太小对噪声敏感匹配不稳定窗口太大在物体边缘处会模糊导致深度图边界不清晰。通常从7x7开始调试。另外必须进行严格的边界处理否则程序极易在图像边缘崩溃。3.3 模块三三角测量核心算法实现这是将我们推导的数学公式转化为代码的关键一步。输入是匹配好的像素坐标(xl, yl)和(xr, yr)以及相机参数输出是三维点(X, Y, Z)。根据之前的推导在理想平行模型下步骤是检查视差d xl - xr。如果d 0说明匹配可能出错物体应在无穷远或匹配错误返回无效值。将像素坐标转换到归一化相机坐标系去除内参影响 $ x_{norm_l} (x_l - cx) / fx $ $ y_{norm_l} (y_l - cy) / fy $ 对于右图同理但通常我们假设校正后左右相机内参一致且y坐标相同计算深度Z$ Z (f_x * B) / d $。注意这里用fx和以像素为单位的视差d计算B是物理基线长度米。f_x * B这个乘积需要根据你的标定结果来定。计算三维坐标 $ X x_{norm_l} * Z $ $ Y y_{norm_l} * Z $代码实现如下bool StereoCameraSystem::triangulate(double xl, double yl, double xr, double yr, double X, double Y, double Z) const { // 1. 计算视差 double disparity xl - xr; // 假设xr是校正后右图对应点的x坐标 if (disparity 0.0) { // 视差非正点位于无穷远或匹配无效 return false; } // 2. 像素坐标归一化 (左相机) double xn_l (xl - leftCam_.cx) / leftCam_.fx; double yn_l (yl - leftCam_.cy) / leftCam_.fy; // 3. 计算深度Z (基于平行模型简化公式) // 注意这里的 baseline 是 extrinsics_.Tx单位是米 // fx * baseline 是一个常数可以预先计算 Z (leftCam_.fx * extrinsics_.baseline) / disparity; // 4. 计算三维坐标 (左相机坐标系) X xn_l * Z; Y yn_l * Z; return true; }关键细节公式中的f和d必须单位一致。如果焦距fx是以像素为单位那么视差d也必须是以像素为单位的差值。基线B通常以米为单位那么计算出的Z也是以米为单位。确保单位统一是避免结果差几个数量级的关键。3.4 模块四后处理与优化直接由匹配和三角测量得到的三维点云通常噪声很大需要后处理。视差滤波唯一性约束在匹配时除了从左到右匹配也可以从右到左匹配。只有两者匹配结果一致的点才认为是可靠的。峰值比滤波检查匹配代价函数的最小值与次小值的比值。如果比值太小如1.5说明匹配模糊拒绝该点。一致性检查相邻像素的视差不应出现剧烈跳变可以用中值滤波或双边滤波对视差图进行平滑。深度滤波范围过滤根据应用场景设定一个有效的深度范围[Z_min, Z_max]剔除过近或过远的无效点。统计滤波计算局部邻域内深度的均值μ和标准差σ剔除那些深度值不在[μ-ασ, μασ]范围内的离群点。这对于去除飞点非常有效。// 简单的深度中值滤波示例 void medianFilterDepthMap(std::vectordouble depthMap, int width, int height, int kernelRadius) { std::vectordouble window; window.reserve((2*kernelRadius1)*(2*kernelRadius1)); std::vectordouble filtered depthMap; for (int y kernelRadius; y height - kernelRadius; y) { for (int x kernelRadius; x width - kernelRadius; x) { window.clear(); for (int j -kernelRadius; j kernelRadius; j) { for (int i -kernelRadius; i kernelRadius; i) { int idx (y j) * width (x i); if (depthMap[idx] 0) { // 只考虑有效的深度点 window.push_back(depthMap[idx]); } } } if (!window.empty()) { std::sort(window.begin(), window.end()); filtered[y * width x] window[window.size() / 2]; // 取中值 } } } depthMap.swap(filtered); }4. 完整代码实现与流程串联现在我们将所有模块串联起来形成一个完整的处理流程。假设我们有两张已经过畸变校正和极线校正的灰度图像leftImg和rightImg。// 主处理流程伪代码 int main() { // 1. 加载相机参数 (从文件或硬编码) CameraIntrinsics intri; // 假设左右相机内参相同且已校正 intri.fx 800.0; intri.fy 800.0; intri.cx 320.0; intri.cy 240.0; // 假设图像640x480 CameraExtrinsics extri; extri.baseline 0.12; // 12厘米基线 StereoCameraSystem stereoSystem(intri, intri, extri); // 2. 加载图像 (这里需要自己实现或使用其他轻量库如stb_image) int width 640, height 480; std::vectoruint8_t leftImg loadGrayImage(left.png, width, height); std::vectoruint8_t rightImg loadGrayImage(right.png, width, height); // 3. 参数设置 int windowRadius 3; // 7x7窗口 int minDisparity 5; int maxDisparity 80; // 根据场景和基线估算 // 4. 计算视差图 std::vectorint disparityMap(width * height, 0); for (int y windowRadius; y height - windowRadius; y) { for (int x windowRadius maxDisparity; x width - windowRadius; x) { int d computeDisparitySAD(leftImg.data(), rightImg.data(), width, height, x, y, windowRadius, minDisparity, maxDisparity); disparityMap[y * width x] d; } } // 5. (可选)对视差图进行滤波 // medianFilterDisparity(disparityMap, width, height, 2); // 6. 三角测量生成点云 std::vectorPoint3d pointCloud; for (int y 0; y height; y) { for (int x 0; x width; x) { int idx y * width x; int d disparityMap[idx]; if (d 0) { double xr x - d; // 平行校正下右图对应点x坐标 double X, Y, Z; if (stereoSystem.triangulate(x, y, xr, y, X, Y, Z)) { if (Z 0.3 Z 5.0) { // 深度范围过滤例如0.3米到5米 pointCloud.emplace_back(X, Y, Z); } } } } } // 7. 保存点云 (例如为PLY格式便于用MeshLab等查看) savePointCloudToPLY(output.ply, pointCloud); std::cout 三角测量完成生成 pointCloud.size() 个三维点。 std::endl; return 0; }性能提示上述双循环遍历每个像素计算视差是计算最密集的部分时间复杂度为O(widthheightdisparityRange*windowArea)。在实际应用中可以考虑以下优化1) 使用积分图像加速SAD计算2) 只对图像中感兴趣的区域如通过边缘检测得到进行计算3) 使用多线程并行处理不同的图像行4) 对于嵌入式平台可以考虑将搜索范围[minDisp, maxDisp]用固定点整数运算来模拟浮点数并利用SIMD指令集。5. 常见问题、调试技巧与效果评估自己实现算法调试是最大的挑战。以下是几个常见问题及排查思路。5.1 深度值全部异常过大或过小检查相机参数单位这是最常见的问题。确认fx, fy, cx, cy的单位是像素。基线B的单位是米。如果你的标定结果中基线是以毫米为单位的例如120mm那么代码中需要转换为0.12米。检查视差正负确保你的匹配算法得到的视差d是x_left - x_right并且对于前方的物体d应该是正数。如果反过来深度Z会变成负值。验证坐标转换在triangulate函数中打印中间变量。xn_l和yn_l应该在0附近小范围波动例如[-1, 1]。如果值非常大说明像素坐标减去主点(cx,cy)或除以焦距(fx,fy)的步骤有误。5.2 深度图充满噪声和“空洞”匹配窗口大小尝试增大匹配窗口如从5x5到9x9。更大的窗口能抑制噪声但会损失边缘细节。视差搜索范围minDisparity和maxDisparity设置是否正确它们定义了最近和最远物体的搜索范围。可以用一个已知距离的物体来反推大致的视差d ≈ fx * B / Z_known。图像质量输入图像是否模糊、光照不均或缺乏纹理双目匹配在纹理丰富的区域效果才好。对于白墙、纯色桌面等弱纹理区域匹配必然失败产生噪声或空洞。这是立体视觉的固有难题。后处理滤波务必加入中值滤波、一致性检查等后处理步骤。原始的视差图通常是不能直接用的。5.3 重建物体形状扭曲或缩放不对极线校正是否完美我们的整个推导基于完美的平行光轴模型。如果实际的双目图像没有经过严格的极线校正那么匹配点对就不满足y_left y_right的假设导致三角测量公式失效。确保输入图像是经过立体校正的。这一步可以用OpenCV的stereoRectify和initUndistortRectifyMap完成然后将校正映射表应用到图像上这个步骤可以离线进行。相机参数不准内参特别是焦距fx,fy和外参基线B的标定误差会直接导致深度和三维坐标的系统性误差。重新进行高精度标定。5.4 如何验证你的代码是正确的构造合成数据这是最可靠的调试方法。在三维软件如Blender中创建几个已知三维坐标的虚拟点用虚拟的双目相机渲染出左右两张图像。将图像和已知的、精确的相机参数输入你的程序。计算出的三维点应该与原始点几乎重合。你可以控制所有变量完美隔离算法问题。使用标准数据集使用Middlebury、KITTI等权威立体视觉数据集。它们提供了校正后的图像、标定参数和真实的视差图/深度图。你可以用它们的图像和参数运行你的代码将生成的视差图与真实值对比。分模块测试单独测试triangulate函数。手动输入几组已知的、正确的匹配点对可以通过特征点匹配算法如SIFT获得但需确保匹配准确看输出的三维坐标是否合理。5.5 效果评估指标对于视差图/深度图常用的定量评估指标有误匹配率Bad Pixel Ratio计算视差图与真实值Ground Truth差异超过一定阈值如1像素或2像素的像素比例。均方根误差RMSE视差或深度误差的均方根。平均绝对误差MAE误差绝对值的平均值。对于自己实现的项目初期更应关注定性评估重建出的点云是否大致反映了物体的形状平面物体是否重建为平面不同距离的物体是否有正确的深度层次6. 从原理到进阶还能做些什么当你成功运行起这个基础版本后可以沿着以下方向深化打造一个更鲁棒、更高效的系统。更优的匹配算法用半全局匹配SGM替代简单的局部块匹配。SGM通过在多条路径上进行动态规划能显著改善弱纹理区域和遮挡区域的匹配效果。虽然实现比SAD复杂但有很多开源的精简实现可以参考其核心思想是代价聚合。亚像素精度视差我们目前计算的视差是整数像素。通过在对匹配代价函数曲线在最佳视差附近进行二次或线性插值可以获得亚像素精度的视差从而将深度精度提升一个数量级。处理非平行配置如果相机无法被校正到理想平行比如基线不水平我们需要使用更一般的三角测量方法。这通常涉及求解一个最小二乘问题从两个相机的投影矩阵$P_l$和$P_r$出发对于匹配点对$p_l$和$p_r$求解三维点$P$使得重投影误差$||p_l - P_l P||^2 ||p_r - P_r P||^2$最小。这可以通过线性三角测量DLT或迭代优化的方法求解。集成到实时系统将算法移植到嵌入式平台如树莓派、Jetson Nano、STM32OV系列摄像头模组。重点优化内存使用和计算速度可能需要对图像进行降采样使用定点数运算甚至手写ARM NEON汇编进行加速。添加彩色信息将RGB图像的色彩信息附加到生成的三维点云上可以得到彩色的点云视觉效果和后续应用价值如分割、识别会更好。实现一个不依赖OpenCV的双目三角测量系统就像完成了一次计算机视觉的“底层修炼”。你获得的不仅仅是一个可用的测距代码更是对多视图几何、相机模型、优化算法的深刻理解。当你在项目中遇到奇怪的3D重建问题时这份从底层构建的经验会让你更快地定位到问题的根源——是标定不准是匹配出错还是三角测量公式用错了这种解决问题的能力远比单纯会调用一个库函数要宝贵得多。