简介在机器人导航与自动驾驶场景中精确获取三维空间信息至关重要。激光雷达等主动光方案成本较高而无需额外光源的双目立体视觉利用左右相机的视差即可高效恢复深度。其核心原理是立体匹配通过计算同名点的像素位移结合三角测量法推断距离。相机标定与立体校正是保证匹配质量的基础直接影响视差图和深度图的生成精度。以SGBM算法为代表的匹配方案在精度与实时性之间取得了良好平衡配套OpenCV等工具链可实现从标定到点云重建的完整流程。双目方案在目标检测、障碍规避、环境建模等应用中展现出独特价值尤其适合工程化落地。本文围绕一条可跑的SGBM管线完整介绍了相机标定、立体校正、立体匹配、深度图与点云生成及目标距离测量等关键环节并总结了真实场景中的常见问题与调参经验。1. 双目立体视觉不用主动光也能让机器人看到三维世界如果你手上只有一个单目摄像头想测前方障碍物的距离基本只能靠先验尺寸估算或运动恢复结构误差大且不稳定。双目立体视觉走的是另一条路用左右两个摄像头同时拍摄同一场景通过立体匹配找到同名点利用视差算出每个像素的深度再生成深度图和点云。这套流程在机器人导航和自动驾驶环境里非常常见因为它不需要激光雷达那样昂贵的硬件一对普通工业相机就能输出稠密三维信息。标题里提到的立体匹配、视差计算、深度图生成、点云重建、相机标定与校正、目标检测与跟踪其实就是从原始图像到三维感知的完整管线。我做过的模拟项目X里用一对千元级摄像头在室内走廊跑通了这套流程最终输出每秒约15帧的深度图和带距离信息的目标检测框。整套系统的核心难点不在某个算法本身而在各环节之间的衔接和参数调试。下面按我做过的方案把这六个环节依次拆开讲。2. 相机标定与立体校正左右目对齐是后面一切计算的基石2.1 标定板选择与采集姿势双目系统里两个摄像头的光轴几乎不可能天然平行镜头畸变也真实存在。如果跳过标定直接做立体匹配左右图像的同名点不在同一水平线上视差计算会完全跑偏。标定要解决的就是两件事一是去掉镜头畸变二是算出左右相机之间的旋转矩阵和平移向量为后面的立体校正提供参数。我一般用棋盘格标定板建议10×7的棋盘也就是9×6个内角点。打印时尽量打印在平整的硬板上不要用软纸因为翘曲会让角点坐标不准确。采集时不要只拍正对相机的角度那样标定出来的内参外参不充分。至少要拍15到20张覆盖画面四个角落、不同倾斜角度、不同距离并且确保标定板在每一张图里完整可见。采集时把标定板放在光照均匀的地方避免反光导致角点检测失败。左图和右图必须用同一时刻的画面所以采集时最好固定相机用手移动标定板或者让相机和标定板都静止用软件触发同步采集。如果是普通USB摄像头没有硬件同步信号就用软件同时触发读取尽量把时间差控制在几毫秒内。2.2 双目标定与去畸变双目标定我用OpenCV的stereoCalibrate接口。标定前先用findChessboardCorners检测角点然后用cornerSubPix做亚像素细化角点精度直接影响后面立体校正的极线对齐质量。import cv2 import numpy as np # 棋盘格内角点规模10x7棋盘对应9x6内角点 pattern_size (9, 6) objp np.zeros((np.prod(pattern_size), 3), np.float32) objp[:, :2] np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) obj_points [] left_img_points [] right_img_points [] # 逐对处理标定图片替换为自己的图片路径列表 for left_path, right_path in zip(left_imgs, right_imgs): left cv2.imread(left_path) right cv2.imread(right_path) gray_l cv2.cvtColor(left, cv2.COLOR_BGR2GRAY) gray_r cv2.cvtColor(right, cv2.COLOR_BGR2GRAY) ret_l, corners_l cv2.findChessboardCorners(gray_l, pattern_size, None) ret_r, corners_r cv2.findChessboardCorners(gray_r, pattern_size, None) if ret_l and ret_r: criteria (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners_l cv2.cornerSubPix(gray_l, corners_l, (5, 5), (-1, -1), criteria) corners_r cv2.cornerSubPix(gray_r, corners_r, (5, 5), (-1, -1), criteria) obj_points.append(objp) left_img_points.append(corners_l) right_img_points.append(corners_r) # 双目标定flags里的 CALIB_FIX_INTRINSIC 表示锁定单目标定得到的内参 ret, K_l, D_l, K_r, D_r, R, T, E, F cv2.stereoCalibrate( obj_points, left_img_points, right_img_points, None, None, None, None, (gray_l.shape[1], gray_l.shape[0]), criteriacriteria, flagscv2.CALIB_FIX_INTRINSIC )每个输入参数都有讲究pattern_size要和你棋盘图完全一致写错差一格就会导致大量图片“找不到角点”。flagscv2.CALIB_FIX_INTRINSIC表示焦距和畸变系数由单目标定结果固定双目只估算外参。如果用的是带变焦的摄像头必须先固定焦距再标定中途变焦会让标定结果完全失效。2.3 立体校正与重映射标定得到R和T之后接下来做立体校正。这一步把左右图像通过stereoRectify得到重映射矩阵再用initUndistortRectifyMap生成查表映射。校正后的输出是理想状态左右画面的极线严格水平对齐同名点落在同一行上这样立体匹配只需要在水平方向搜索视差。# 分辨率与前面标定输入的尺寸一致 img_size (gray_l.shape[1], gray_l.shape[0]) # alpha0 表示校正后裁剪掉黑色空白区域alpha1 保留全部像素 R1, R2, P1, P2, Q, roi1, roi2 cv2.stereoRectify( K_l, D_l, K_r, D_r, img_size, R, T, alpha0, flagscv2.CALIB_ZERO_DISPARITY ) map1_l, map2_l cv2.initUndistortRectifyMap(K_l, D_l, R1, P1, img_size, cv2.CV_32FC1) map1_r, map2_r cv2.initUndistortRectifyMap(K_r, D_r, R2, P2, img_size, cv2.CV_32FC1) # 校正后续所有视频帧时只需 remap left_rect cv2.remap(left_raw, map1_l, map2_l, cv2.INTER_LINEAR) right_rect cv2.remap(right_raw, map1_r, map2_r, cv2.INTER_LINEAR)Q矩阵是第五步点云重建的关键参数它把校正后图像的像素坐标和视差映射到三维世界坐标。alpha0会裁剪掉边缘的非公共区域适合只关心公共视野的场景如果要保留完整图像再后来做目标检测可以调成alpha0.5左右让两张图都保留更多像素。验证校正效果的方法是直接在左右校正图上画同一行水平线观察对应物体是否落在这条线上。如果偏差超过两个像素说明标定数据有问题需要重新采集图片而不是继续往下走。3. 立体匹配与视差计算SGBM参数调试和四类坑3.1 为什么默认选SGBM而不是BM或GC立体匹配算法通常分为局部算法和全局算法两类。BMBlock Matching是局部算法速度快但视差图噪声非常大尤其在弱纹理区域会出现大量误匹配。GCGraph Cut属于全局算法精度高于BM但在CPU上跑一帧要好几秒根本不适合实时视频处理。SGBMSemi-Global Block Matching处在一个恰到好处的位置它把全局能量优化的思想用多个方向的一维路径聚合来近似精度接近全局算法速度又能靠OpenCV的SIMD优化跑出不错的帧率。我在实际落地时默认第一步都先上SGBM确认效果后再考虑是否换更重的算法。SGBM前几年OpenCV里的核心类是StereoSGBM后来新版推荐用StereoSGBM_create接口创建实例。整个匹配过程还包含预处理计算梯度或灰度变化、代价计算基于Birchfield-Tomasi或块SAD、方向聚合、视差选择、后处理等几个环节OpenCV封装好了大部分但参数要自己调。3.2 一个能跑的SGBM视差计算代码import cv2 import numpy as np # 按实际分辨率配比建议保持 numDisparities 为16的倍数 min_disparity 0 num_disparities 64 block_size 11 stereo cv2.StereoSGBM_create( minDisparitymin_disparity, numDisparitiesnum_disparities, blockSizeblock_size, P18 * 3 * block_size ** 2, P232 * 3 * block_size ** 2, disp12MaxDiff1, uniquenessRatio10, speckleWindowSize100, speckleRange32, preFilterCap63, modecv2.STEREO_SGBM_MODE_SGBM ) # 输入必须是灰度图用前面校正过的图 disp stereo.compute(left_rect, right_rect).astype(np.float32) / 16.0参数这块最常翻车的不是算法本身而是比例关系。numDisparities指要搜索的视差范围必须是16的倍数。它决定近距离可测范围数值越大能测越近的物体但计算量和虚假匹配也明显增多。minDisparity表示起始视差基线较大的双目系统里如果最近物体的视差超过当前范围可以用负值或增大numDisparities来覆盖。blockSize要求是奇数我常用7到15之间越大对噪声越鲁棒但会抹掉细小的深度边缘。P1和P2是惩罚项。P1对相邻像素视差变化为1的情况做小惩罚P2对变化大于1做较大惩罚。常见做法是把P1设为8倍通道数乘blockSize平方P2设为4倍P1。如果发现视差图在物体边缘出现断裂适当调大P2如果视差图过于平滑、细节丢失则调小P2。uniquenessRatio用于剔除低置信度的匹配通常设在5到15之间太高会导致大面积黑色的空洞。speckleWindowSize和speckleRange用来滤除小块的飞点噪声前者是连通域的像素阈值后者是允许的视差波动范围设置过大会把真实物体边缘也抹掉。调用compute后输出的视差值是定点数所以除以16才还原成实数单位。如果没除后面所有深度计算都会偏大16倍。3.3 从视差反推深度基线、焦距和三角测量立体匹配计算视差视差是左右相机同一空间点在校正后图像上的横坐标差。得到视差d后深度Z由三个几何参数决定相机焦距f单位像素、基线b单位毫米、以及视差d单位像素。公式是Z f * b / d。用这个公式时要留意单位一致性。f一般从相机内参读取单位是像素b是标定得到的平移向量T的模长。如果T的量纲是毫米算出Z单位也是毫米。我用过的一对工业镜头基线约120mm焦距约700像素对应视差为10个像素时深度约8.4米。这也是为什么远距离的深度噪声特别大视差差一个像素在近处只差几厘米在远处可能差出一两米。知道了这个原理后面做目标检测融合时就应该对远处目标弱化距离精度预期。4. 深度图生成与点云重建把像素映射到三维空间4.1 用Q矩阵把像素坐标投影到世界坐标立体校正时stereoRectify输出Q矩阵它专门用于把校正后图像上的像素坐标和视差转成三维空间点。公式是[X, Y, Z, W] Q * [x, y, disparity, 1]然后实际三维坐标是(X/W, Y/W, Z/W)。这里的Z即深度正是上一章从视差反推的深度值。OpenCV提供了一个直接完成投影的接口reprojectImageTo3D。它输入视差图、Q矩阵和handleMissingValues开关输出三通道的XYZ图每个像素存储对应空间点的三维坐标。# depth_xyz: 三通道 float32表示每个像素对应的三维坐标 depth_xyz cv2.reprojectImageTo3D(disp, Q, handleMissingValuesTrue) # 直接取Z通道作为深度图单位与标定时的T一致 depth_map depth_xyz[:, :, 2] # 手动清理无效值避免后面点云里出现飞点 depth_map[depth_map 0] 0 depth_map[depth_map 20000] 0 # 超过20米按无效处理这里有一个容易忽略的细节reprojectImageTo3D里的disp必须是除以16之前的原始float类型还是除以16之后的值取决于你传入的Q矩阵。OpenCV的stereoRectify输出Q时已经把视差的缩放因子考虑进去所以传入的视差图应该是除以16后的真实视差。如果直接传入原始整型视差转换成的浮点图深度会整体偏差16倍。我自己就用错过一次点云里所有物体都缩到了原来的十六分之一。4.2 点云生成、过滤与导出为PLY如果只做导航深度图已经够用。但如果要构建环境模型、输出给下游处理或者做三维可视化验证需要把深度图展开成点云。遍历每个像素把depth_xyz里的有效点收集起来加上颜色信息按PLY格式写出。def depth_to_pointcloud(depth_xyz, left_color, max_depth15000): h, w depth_xyz.shape[:2] points [] colors [] valid_mask (depth_xyz[:, :, 2] 0) (depth_xyz[:, :, 2] max_depth) xs, ys np.meshgrid(np.arange(w), np.arange(h)) valid_xs xs[valid_mask] valid_ys ys[valid_mask] valid_xyz depth_xyz[valid_mask] valid_colors left_color[valid_mask] for x, y, xyz, color in zip(valid_xs, valid_ys, valid_xyz, valid_colors): points.append(xyz) colors.append(color) return np.array(points), np.array(colors) # 写PLY文件 def write_ply(filepath, points, colors): with open(filepath, w) as f: f.write(ply\n) f.write(format ascii 1.0\n) f.write(felement vertex {len(points)}\n) f.write(property float x\n) f.write(property float y\n) f.write(property float z\n) f.write(property uchar red\n) f.write(property uchar green\n) f.write(property uchar blue\n) f.write(end_header\n) for p, c in zip(points, colors): f.write(f{p[0]:.4f} {p[1]:.4f} {p[2]:.4f} {int(c[2])} {int(c[1])} {int(c[0])}\n)这段代码看似简单但逐像素循环在Python里性能不行如果直接用在实时视频流上每秒只能处理不到一帧。常见做法是在离线重建时用这段逻辑实时场景则降采样到每四个像素取一个点。另外PLY文件的颜色通道顺序是R、G、B而OpenCV读入的图像是BGR写文件时必须交换通道顺序否则重建出的点云颜色偏色。4.3 深度图和点云的快速自检生成点云后不要急着往下走先用一个非常直观的方法验证。把点云按深度值映射成伪彩色图再叠回到左目原始图上。如果物体边缘出现红色和蓝色交错的硬化边说明视差在边缘处存在跳变通常是disp12MaxDiff过小或uniquenessRatio过高的原因。如果点云在平整墙面出现波纹状起伏多半是标定板的采集角度不够丰富导致立体校正矩阵不够精确。还有一类问题只会在点云里暴露相邻帧之间点云在固定位置漂移。把相机放在静止桌面上连续生成10帧点云查看墙面点的X/Y/Z坐标是否稳定在毫米级变化范围。如果变化超过几厘米说明相机本身有振动或者曝光时间太长导致图像模糊。我遇到过一台相机在自动曝光模式下帧间亮度波动让视差图跟着变关掉自动曝光、固定曝光时间后问题消失。5. 实时视频处理中的避坑清单光照、纹理、遮挡与硬件干扰5.1 大面积反光区域让视差图出现黑洞现象室内瓷砖地面或玻璃墙面前方视差图出现大片黑色无匹配区域深度信息完全缺失。原因反光区域在左右图像上亮度差异太大局部窗口内的梯度方向不一致SGBM的代价计算找不到合适的同名点。这种情况下不是算法参数没调好而是图像本身在左右视角下就不满足匹配前提。解决先尝试把图像转成灰度前做一次直方图均衡化可以改善部分亮度差异。如果仍然不行就需要在物理层面遮挡反光或者接受这块区域不可用在导航算法里把无深度区域视为未知区域而不是安全区域。总结一句话SGBM不能处理左右视角差异过大的镜面反射区域靠参数调不出来。5.2 低纹理墙面导致视差断裂成块现象纯色墙面或无纹理地面视差图上出现一整片区域视差一致边缘处有撕裂感甚至出现大面积随机匹配。原因SGBM本质依赖图像特征匹配白色墙面在窗口内几乎没有梯度任何偏移位置的匹配代价都差不多算法选择了任意一个低代价位移造成视差在该区域不稳定。解决如果场景是室内导航可以在墙上贴一些纹理标签辅助匹配。室外自动驾驶环境里沥青路面自带纹理一般问题不大。如果实在不能改变场景只能把低纹理区域的视差视为低置信度在点云生成阶段直接过滤掉。不要试图用增大blockSize解决那会让边缘更模糊也不会让平面变得更准。5.3 前景物体边缘出现前后景重叠条带现象人站在墙前面约1米处视差图上人体边缘向外膨胀出一圈暗色或亮色条带点云里出现一条前后景混合的带状飞点。原因遮挡。左右相机由于基线存在视差前景左侧的一部分背景只被左图看到右侧则相反。这些像素在右图中找不到对应点SGBM输出一个默认可匹配的错误视差。解决disp12MaxDiff用来检查左右一致性左右视差差异超过这个值的像素会被标记为无效。把值设成1能去掉一部分遮挡区域但也会把一些正确匹配的点误删。更可靠的办法是在点云生成阶段通过相邻深度突变检测边界把边缘3到5个像素的深度值用周边中值代替或者直接置零。5.4 运动物体产生拖影和滚动快门问题现象目标检测框里的行人在视差图上轮廓残缺点云位置与检测框中心偏移明显。原因普通CMOS摄像头用滚动快门左右相机如果不同步运动物体在左右图里的位置差不仅来自视差还来自时间差导致计算出的深度偏高或偏低。物体运动速度越快误差越大1米外的物体可能偏差几十厘米。解决这是硬件层面的问题软件只能缓解。把相机的曝光时间调短能减少单帧内的拖影但不能解决左右时间差。工业相机可以用硬件触发实现同步曝光USB摄像头很难做到帧级同步。我在模拟项目X里测试过静止场景精度很好但行人走动时距离误差能到15%以上这个量值在导航决策里必须纳入考虑。5.5 实时性能不达标时的排查方向现象摄像头输出30帧的双目画面但是完整管线算下来只有5帧CPU占用接近饱和。原因立体匹配和点云生成是主要瓶颈。视差范围numDisparities每增加一倍计算量近似翻倍每帧生成全分辨率的点云也非常耗时。解决先看耗时分布打开OpenCV的getTickCount分别统计remap、compute、reprojectImageTo3D和检测网络的耗时找出占比最大的环节。把输入分辨率从1080p降到720pnumDisparities从128降到64往往能换回两倍性能。还可以把SGBM模式从STEREO_SGBM_MODE_SGBM改为STEREO_SGBM_MODE_HH4后者精度略低但速度快很多适合实时场景。真正需要每像素点云时才做全分辨率重建平时只计算深度图能省掉一大半时间。6. 融合目标检测的三维感知把点云投影回检测框6.1 检测框与深度图像的坐标对齐目标检测在这套系统里的作用是把图像中的物体识别结果和深度信息绑定。检测网络在左目校正图上输出二维检测框每个框需要对应到深度图中的一块区域。这里有一个容易掉进去的坑如果检测是在原始左图上做的而深度图是校正后左图生成的两者坐标对不上。我的做法是让检测网络直接跑在left_rect上。这样检测框坐标和深度图天然对齐无需额外坐标变换。如果检测结果必须基于原始图那就需要先用initUndistortRectifyMap生成的映射关系把原始图上的检测框四角坐标映射到校正图上再从校正图上取深度值。# 假设 detector 返回的 box 坐标基于left_rect # boxes: [x1, y1, x2, y2] x1, y1, x2, y2 map(int, box) roi_depth depth_map[y1:y2, x1:x2] # 去掉无效深度后取中值比均值抗噪 valid_depths roi_depth[(roi_depth 0) (roi_depth 20000)] if len(valid_depths) 0: target_distance float(np.median(valid_depths)) else: target_distance float(nan)6.2 区域深度估计的几个技巧取深度时很多人直接算检测框内所有像素的均值。这样做的结果是检测框边界附近混入背景深度或者框内包含遮挡边缘均值被带偏。我习惯只取检测框中心区域比如以框中心点向外扩展1/2宽、1/2高的矩形在这个子区域里取中值。中值比均值对异常飞点更鲁棒单个噪点不会让距离跳变。如果检测框里同时包含人和他手里的箱子前方遮挡严重时中值可能落在人身上也可能落在箱子上这取决于哪个区域占像素更多。这时候靠单个数值不够还要看深度直方图分布。如果直方图出现双峰说明框内有两个显著深度层导航决策就要按最近距离处理。6.3 我留下的一个调试习惯保存所有中间结果这套系统最让人头疼的不是某一个算法调不好而是问题在多个环节间传导。视差图里的一小块噪声到点云里可能表现为一簇飞点再到检测框距离里变成一个跳变的数据。为了快速定位问题我习惯在调试阶段把每一帧的中间结果做成一个可回放的序列原始左图、校正左图、视差图、伪彩色深度图、目标检测画框图五个图像放在一起同步保存。发生问题时直接翻历史记录看到底是哪个环节先出问题。这个习惯帮我省了很多时间尤其在调节SGBM参数时同时查看视差图和检测画框图能立刻判断出是匹配问题还是融合问题不用反复重跑管线。项目做到后期我最大的感受是这套立体视觉系统里真正的难点不是单个算法的原理而是把匹配、几何、检测串起来时参数联动产生的微妙影响。从标定到最终输出距离一个像素的误差会被放大成厘米级的深度误差每个环节都不能省。如果你打算在这个方向投入建议先不要急着上最强的算法而是把一条最基本的SGBM管线完整跑通保存好标定参数和中间结果再逐步调优。这个基础越扎实后面换更复杂的匹配网络时越不迷茫。希望帮到你。本文还有配套的精品资源点击获取