前言
首先进行双目定标，获取双目摄像头内部的参数后，进行测距。本次的双目视觉测距，基于BM算法。
注意：双目定标的效果会影响测距的精准度，建议大家在做双目定标时，做好一些（尽量让误差小）
如果不太了解双目视觉原理，建议先看看这篇文章：一篇文章认识《双目立体视觉》
一、双目测距 效果
基于BM算法，生成视差图的效果
用鼠标点击视差图，程序会自动计算该点的世界坐标、距离，输出信息如下：
像素坐标 x = 470, y = 163
世界坐标xyz 是： 0.22539872741699218 -0.1110642318725586 0.6529436645507812 m
距离是： 0.6996250988920024 m
这里的距离是双目相机中心（左右相机中心）到物体的实际距离，如上面的是以米为单位。
二、双目测距 流程思路
程序流程图 如下
三、双目测距 前提准备
1）打开双目摄像头；
2）双目摄像头标定；获取的参数：
左相机内参、左相机畸变系数:[k1, k2, p1, p2, k3]
右相机内参、右相机畸变系数:[k1, k2, p1, p2, k3]
左右相机之间的旋转矩阵、平移向量。并命名为：camera_config.py，下面测距需要用到的。
import cv2 import numpy as np # 左相机内参 left_camera_matrix = np.array([[416.841180253704, 0.0, 338.485167779639], [0., 416.465934495134, 230.419201769346], [0., 0., 1.]]) # 左相机畸变系数:[k1, k2, p1, p2, k3] left_distortion = np.array([[-0.0170280933781798, 0.0643596519467521, -0.00161785356900972, -0.00330684695473645, 0]]) # 右相机内参 right_camera_matrix = np.array([[417.765094485395, 0.0, 315.061245379892], [0., 417.845058291483, 238.181766936442], [0., 0., 1.]]) # 右相机畸变系数:[k1, k2, p1, p2, k3] right_distortion = np.array([[-0.0394089328586398, 0.131112076868352, -0.00133793245429668, -0.00188957913931929, 0]]) # om = np.array([-0.00009, 0.02300, -0.00372]) # R = cv2.Rodrigues(om)[0] # 旋转矩阵 R = np.array([[0.999962872853149, 0.00187779299260463, -0.00840992323112715], [ -0.0018408858041373, 0.999988651353238, 0.00439412154902114], [ 0.00841807904053251, -0.00437847669953504, 0.999954981430194]]) # 平移向量 T = np.array([[-120.326603502087], [0.199732192805711], [-0.203594457929446]]) size = (640, 480) R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion, right_camera_matrix, right_distortion, size, R, T) left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2) right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
双目定标可以参考：双目视觉 定标+矫正 （基于MATLAB）
双目数据转化可以参考：双目视觉 三维重建、测距 ---准备工作（数据转化）
四、双目测试 实现
完整代码 主要包括main.py、camera_config.py两个文件的代码；main.py是主函数，实现双目视觉测距。相机参数用 camera_config.py表示。
main.py代码如下：
# -*- coding: utf-8 -*- import numpy as np import cv2 import camera_config import random import math cap = cv2.VideoCapture(0) cap.set(3, 1280) cap.set(4, 480) #打开并设置摄像头 # 鼠标回调函数 def onmouse_pick_points(event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: threeD = param print('\n像素坐标 x = %d, y = %d' % (x, y)) # print("世界坐标是：", threeD[y][x][0], threeD[y][x][1], threeD[y][x][2], "mm") print("世界坐标xyz 是：", threeD[y][x][0]/ 1000.0 , threeD[y][x][1]/ 1000.0 , threeD[y][x][2]/ 1000.0 , "m") distance = math.sqrt( threeD[y][x][0] **2 + threeD[y][x][1] **2 + threeD[y][x][2] **2 ) distance = distance / 1000.0 # mm -> m print("距离是：", distance, "m") WIN_NAME = 'Deep disp' cv2.namedWindow(WIN_NAME, cv2.WINDOW_AUTOSIZE) while True: ret, frame = cap.read() frame1 = frame[0:480, 0:640] frame2 = frame[0:480, 640:1280] #割开双目图像 imgL = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY) # 将BGR格式转换成灰度图片 imgR = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY) # cv2.remap 重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。 # 依据MATLAB测量数据重建无畸变图片 img1_rectified = cv2.remap(imgL, camera_config.left_map1, camera_config.left_map2, cv2.INTER_LINEAR) img2_rectified = cv2.remap(imgR, camera_config.right_map1, camera_config.right_map2, cv2.INTER_LINEAR) imageL = cv2.cvtColor(img1_rectified, cv2.COLOR_GRAY2BGR) imageR = cv2.cvtColor(img2_rectified, cv2.COLOR_GRAY2BGR) # BM numberOfDisparities = ((640 // 8) + 15) & -16 # 640对应是分辨率的宽 stereo = cv2.StereoBM_create(numDisparities=16, blockSize=9) #立体匹配 stereo.setROI1(camera_config.validPixROI1) stereo.setROI2(camera_config.validPixROI2) stereo.setPreFilterCap(31) stereo.setBlockSize(15) stereo.setMinDisparity(0) stereo.setNumDisparities(numberOfDisparities) stereo.setTextureThreshold(10) stereo.setUniquenessRatio(15) stereo.setSpeckleWindowSize(100) stereo.setSpeckleRange(32) stereo.setDisp12MaxDiff(1) disparity = stereo.compute(img1_rectified, img2_rectified) # 计算视差 disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) #归一化函数算法 threeD = cv2.reprojectImageTo3D(disparity, camera_config.Q, handleMissingValues=True) #计算三维坐标数据值 threeD = threeD * 16 # threeD[y][x] x:0~640; y:0~480; !!!!!!!!!! cv2.setMouseCallback(WIN_NAME, onmouse_pick_points, threeD) cv2.imshow("left", frame1) # cv2.imshow("right", frame2) # cv2.imshow("left_r", imgL) # cv2.imshow("right_r", imgR) cv2.imshow(WIN_NAME, disp) #显示深度图的双目画面 key = cv2.waitKey(1) if key == ord("q"): break cap.release() cv2.destroyAllWindows()
五、BM算法
BM，全称Bidirectional Matching，一种匹配算法。优点就是快，缺点是深度图的效果不是很好。
它是进行双向匹配的，首先通过匹配代价在右图中计算得出匹配点。然后相同的原理及计算在左图中的匹配点。比较找到的左匹配点和源匹配点是否一致，如果是，则匹配成功。
原理：将两个摄像头的的帧分成很多的小方块来机型匹配，通过移动小方块来匹配另一个图中的小方块，通过发现不同小方块在另一个图像中的像素点位置在结合两个摄像头的关系数据（标定的参数中的translate 和rotation矩阵）来计算出物体的实际深度从而生成相应的深度图。
下面将一些实用性的，如何调整BM中参数，达到不同环境有好的效果。
OpenCV中创建BM函数：
参数含义:
numDisparities 数量差异 |
视差搜索范围。对于每个像素算法都会找到从 0（默认最小视差）到 numDisparities 的最佳视差。然后可以通过更改最小视差来移动搜索范围。 |
blockSize 块大小 |
算法比较的块的线性大小。大小应该是奇数（因为块以当前像素为中心）。更大的块大小意味着更平滑但不太准确的视差图。较小的块大小提供更详细的视差图，但算法找到错误对应关系的机会更高。 |
该函数创建StereoBM对象。然后调用StereoBM::compute()来计算特定立体对的视差。
还想设置其他一些参数，部分如下：
详细参考官方的：OpenCV: cv::StereoBM Class Reference
BM算法示例：
numberOfDisparities = ((640 // 8) + 15) & -16 # 640对应是分辨率的宽 stereo = cv2.StereoBM_create(numDisparities=16, blockSize=9) #立体匹配 stereo.setROI1(camera_config.validPixROI1) stereo.setROI2(camera_config.validPixROI2) stereo.setPreFilterCap(31) stereo.setBlockSize(15) stereo.setMinDisparity(0) stereo.setNumDisparities(numberOfDisparities) stereo.setTextureThreshold(10) stereo.setUniquenessRatio(15) stereo.setSpeckleWindowSize(100) stereo.setSpeckleRange(32) stereo.setDisp12MaxDiff(1) disparity = stereo.compute(img1_rectified, img2_rectified) # 计算视差
小结
视差图效果：SGBM 好于 BM。速度：BM 快于 SGBM
通常双目视觉测距可以结合目标检测，首先用YOLO、SSD等目标检测算法把物体框出来；然后计算物体的中心或质点，并在附近选取一点计算三维坐标和距离。
参考文献
欢迎交流；
