双目摄像头测量距离
前言
在计算机视觉中，可以通过双目摄像头实现，常用的有BM 算法和SGBM 算法等，双目测距跟激光不同，双目测距不需要激光光源，是人眼安全的，只需要摄像头，成本非常底，也用于应用到大多数的项目中。本章我们就来介绍如何使用双目摄像头和SGBM 算法实现距离测量。
相机标定
每个种双目摄像头都不一样，他们之间的距离，畸变等其他的原因，这些都会导致他们定位算法参数的差异，所以我们通常是通过相机标定来得到他们的算法参数。标定的目的是为了消除畸变以及得到内外参数矩阵，内参数矩阵可以理解为焦距相关，它是一个从平面到像素的转换，焦距不变它就不变，所以确定以后就可以重复使用，而外参数矩阵反映的是摄像机坐标系与世界坐标系的转换，至于畸变参数，一般也包含在内参数矩阵中。从作用上来看，内参数矩阵是为了得到镜头的信息，并消除畸变，使得到的图像更为准确，外参数矩阵是为了得到相机相对于世界坐标的联系，是为了最终的测距。
拍摄标定图像
我们需要通过摄像头拍摄标定图片，拍摄得到的是左目摄像头和右目摄像头的图像，笔者一般是拍摄16张左右。通常双目摄像头拍摄得到的图像是左目摄像头拍摄的在第一位，然后是右目摄像头，使用OpenCV拍摄的图像，可以通过裁剪的方式把他们分开分别存储。以下是笔者提供的拍摄标定图像的Python代码，通过按回车键保存图像。注意在拍摄前需要调整好摄像头的焦距，调整之后就不要再动了。
import cv2
imageWidth = 1280
imageHeight = 720
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, imageWidth * 2)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, imageHeight)
i = 0
while True:
# 从摄像头读取图片
success, img = cap.read()
if success:
# 获取左右摄像头的图像
rgbImageL = img[:, 0:imageWidth, :]
rgbImageR = img[:, imageWidth:imageWidth * 2, :]
cv2.imshow('Left', rgbImageL)
cv2.imshow('Right', rgbImageR)
# 按“回车”保存图片
c = cv2.waitKey(1) & 0xff
if c == 13:
cv2.imwrite('Left%d.bmp' % i, rgbImageL)
cv2.imwrite('Right%d.bmp' % i, rgbImageR)
print("Save %d image" % i)
i += 1
cap.release()
以下图像是相机标定是所需的棋盘，可以使用A3纸打印出来，使用木板等固定好，不要弯曲，最好是使用专业棋盘。摄像头拍摄的棋盘应该占拍摄区域的三分之一以上。
图像标定
拍摄完成图像之后，使用MATLAB对其进行标定。笔者使用的是MATLAB R2016a，其他的版本应该也可以。
打开MATLAB R2016a，添加TOOLBOX_calib的路径，TOOLBOX_calib下载地址：https://resource.doiduoyi.com/#w0w0sko，下载之后把它解压到D盘的根目录，如下下图所示：
然后在MATLAB R2016a命令区输入：cd d:\calib_example打开标定图片的文件夹，如果读者保存的图片是其他路径，就打开对应的路径。
还是在刚才的命令区，输入命令：calib_gui，打开标定工具，即可启动标定工具，界面如下，然后点击Standard(all the images are stored in memory)按钮。
执行上一步之后会弹出以下界面，然后点击Image names按钮，这个主要是为了通过图像的名称来列出所需的标定图像。
点击Image names按钮之后，会列出当前目录的图像，首先我们标定左目摄像头拍摄的图像。我们在拍摄图像保存时，保存的名字变化主要在名字最后的数字，这样我们就可以通过固定图像名称的前半部分和后缀名来列出将要标定的图像。
输入如下，指定名称为Left，后缀名为bmp，这就可以把左目摄像头的图像都加载进来了。
加载完成之后会弹以下窗口，这些就是将要标定的图像。
然后回到标定工具的功能选择界面，点击Extract grid corners按钮，开始标定。
执行上一步之后，需要回到命令区，需要对接下来的标定做好配置。其他基本可以默认，直接回车就好，有两个参数需要更加推荐输入，读者可能跟我的不一样。
执行完上面之后，会弹出一个标定图像的窗口，标定时需要按照左上、右上、右下、左下顺序点击 4 个边界角点，如图所示。
回到命令区，标定第一张图像时，需要输入棋盘中每个格子的大小，笔者使用A3纸打印的，每个格子大概是28.8mm，这个需要读者去测量自己其他每个格子的大小。
输入上面的数据之后，会在每个格子的对角都标注，这样对畸变的图像进一步调整的，如果使用的是无畸变的图像，那到这一步基本上是完成了。
如果是无畸变的摄像头，在命令去看到以下输出时，直接回车就可以开始标注下一张图像了。如果是有畸变的摄像头，需要输入1，然后根据提示输入调整参数，参数范围在[-1, 1]，通过调整使得红色的标记都在每个格子的对角上。
全部标记完成之后，再次回到标定功能选择菜单上，然后点击Calibration按钮。
点击上面按钮之后，会输入类似以下的信息。
最后点击保存标注信息，文件会被保存为Calib_Results.mat，我们需要将标定结果文件重命名为： Calib_Results_left.mat，这是为了修改成下一步默认路径。左目摄像头标定完成后，按照同样的方法标定右目摄像头，将标定结果文件重命名为：Calib_Results_right.mat，之后可以进行双目标定。
上面都完成之后，应该有Calib_Results_left.mat和Calib_Results_right.mat这两个文件。然后在命令区输入命令：stereo_gui，会弹出以下界面，点击Load left and right calibration files按钮。
执行上一步之后会输入以下信息，这次产生就是我们测距算法所需的参数，不同我们还可以对这些参数进行优化。
点击Run stereo calibration按钮对结果进行优化。
最后输出的是经过优化的参数，输出如下，这些参数非常重要。
以下为每个参数值对应的双目测距的参数。他们的介绍如下：
fc_left：左目镜头像素级焦距值，fc_left × 像元尺寸 = 左目镜头物理焦距值，像
元尺寸见双目相机产品参数表。
cc_left：左目光心位置坐标。
kc_left：左目镜头畸变系数。
alpha_c_left: 偏斜系数。
om：旋转向量。
T：平移向量。T_01，双目间距（即：双目基线）
Focal Length: fc_left = [ 781.69191 781.93358 ] [ 3.14543 3.14792 ]
【fc_left_x fc_left_y】 【误差1， 误差2】
Principal point: cc_left = [ 319.50000 239.50000 ] [ 0.00000 0.00000 ]
【cc_left_x cc_left_y】 【误差1 误差2】
Skew: alpha_c_left = [ 0.00000 ] [ 0.00000 ] => angle of pixel axes = 90.00000 0.00000 degrees
Distortion: kc_left = [ 0.02704 0.10758 -0.00408 -0.01769 0.00000 ] [ 0.01402 0.07915 0.00047 0.00072 0.00000 ]
【kc_left_01, kc_left_02, kc_left_03, kc_left_04, kc_left_05】【误差1 误差2 误差3 误差4 误差5】
Intrinsic parameters of right camera:
Focal Length: fc_right = [ 781.98159 784.43429 ] [ 3.23167 3.20482 ]
【fc_right_x fc_right_y】 【误差1， 误差2】
Principal point: cc_right = [ 319.50000 239.50000 ] [ 0.00000 0.00000 ]
【cc_right_x cc_right_y】 【误差1 误差2】
Skew: alpha_c_right = [ 0.00000 ] [ 0.00000 ] => angle of pixel axes = 90.00000 0.00000 degrees
Distortion: kc_right = [ 0.00679 0.18063 -0.00633 -0.00190 0.00000 ] [ 0.01585 0.12209 0.00054 0.00056 0.00000 ]
【kc_right_01, kc_right_02, kc_right_03, kc_right_04, kc_right_05】【误差1 误差2 误差3 误差4 误差5】
Extrinsic parameters (position of right camera wrt left camera):
【rec旋转向量】
Rotation vector: om = [ -0.01044 -0.04553 -0.00143 ] [ 0.00026 0.00033 0.00018 ]
【T平移向量】
Translation vector: T = [ -60.87137 0.15622 0.01502 ] [ 0.17816 0.16723 1.12660 ]
距离测量
本章教程我们使用的是SGBM算法，SGBM算法作为一种全局匹配算法，立体匹配的效果明显好于局部匹配算法，但是同时复杂度上也要远远大于局部匹配算法。
SGBM算法在OpenCV中已经开源，该算法的函数为cv2.StereoSGBM_create。下面我们就使用Python实现这个双目测距的程序，为了简单，该程序只是使用本地保存的左目图像和右目图像，如何读者想使用摄像头拍摄，可以参考文章开头提供的拍照代码，两者结合，实时检测距离。
这个SGBM算法的实现和相关函数都可以通过OpenCV完成，imageWidth是单目摄像头拍摄的宽度。
import cv2
import numpy as np
imageWidth = 1280
imageHeight = 720
imageSize = (imageWidth, imageHeight)
以下是相机标定的参数，按照相机标定生成的参数对应修改这些参数值。
'''左目相机标定参数
fc_left_x 0 cc_left_x
0 fc_left_y cc_left_y
0 0 1
'''
cameraMatrixL = np.array([[849.38718, 0, 720.28472],
[0, 850.60613, 373.88887],
[0, 0, 1]])
# [kc_left_01, kc_left_02, kc_left_03, kc_left_04, kc_left_05]
distCoeffL = np.array([0.01053, 0.02881, 0.00144, 0.00192, 0.00000])
'''右目相机标定参数
fc_right_x 0 cc_right_x
0 fc_right_y cc_right_y
0 0 1
'''
cameraMatrixR = np.array([[847.54814, 0, 664.36648],
[0, 847.75828, 368.46946],
[0, 0, 1]])
# kc_right_01, kc_right_02, kc_right_03, kc_right_04, kc_right_05
distCoeffR = np.array([0.00905, 0.02094, 0.00082, 0.00183, 0.00000])
# T平移向量
T = np.array([-59.32102, 0.27563, -0.79807])
# rec旋转向量
rec = np.array([-0.00927, -0.00228, -0.00070])
以下就是使用左右目摄像头拍摄到的两张图像，利用SGBM算法技术图像中物体距离摄像头的距离。最后输出的xyz是图像中的三维坐标，通过这个结果可以获取图像中每个点的三维坐标。
# 立体校正
R = cv2.Rodrigues(rec)[0]
Rl, Rr, Pl, Pr, Q, validROIL, validROIR = cv2.stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR,
imageSize, R, T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0,
newImageSize=imageSize)
# 计算更正map
mapLx, mapLy = cv2.initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, cv2.CV_32FC1)
mapRx, mapRy = cv2.initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, cv2.CV_32FC1)
# 读取图片
rgbImageL = cv2.imread("Left3.bmp")
grayImageL = cv2.cvtColor(rgbImageL, cv2.COLOR_BGR2GRAY)
rgbImageR = cv2.imread("Right3.bmp")
grayImageR = cv2.cvtColor(rgbImageR, cv2.COLOR_BGR2GRAY)
# 经过remap之后，左右相机的图像已经共面并且行对齐
rectifyImageL = cv2.remap(grayImageL, mapLx, mapLy, cv2.INTER_LINEAR)
rectifyImageR = cv2.remap(grayImageR, mapRx, mapRy, cv2.INTER_LINEAR)
# SGBM算法重要的参数
mindisparity = 32
SADWindowSize = 16
ndisparities = 176
# 惩罚系数
P1 = 4 * 1 * SADWindowSize * SADWindowSize
P2 = 32 * 1 * SADWindowSize * SADWindowSize
# BM算法
sgbm = cv2.StereoSGBM_create(mindisparity, ndisparities, SADWindowSize)
sgbm.setP1(P1)
sgbm.setP2(P2)
sgbm.setPreFilterCap(60)
sgbm.setUniquenessRatio(30)
sgbm.setSpeckleRange(2)
sgbm.setSpeckleWindowSize(200)
sgbm.setDisp12MaxDiff(1)
disp = sgbm.compute(rectifyImageL, rectifyImageR)
# 在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16
xyz = cv2.reprojectImageTo3D(disp, Q, handleMissingValues=True)
xyz = xyz * 16
xyz是一个矩阵，不能直观体现图像中物体的距离，所以我们可以把他们转换成一张灰度图，更加颜色的深浅就知道他们的大概情况了。然后我们还可以添加一个鼠标点击事件，这样通过点击图像上的点，直接输出该点的三维坐标。
# 用于显示处理
disp = disp.astype(np.float32) / 16.0
disp8U = cv2.normalize(disp, disp, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
disp8U = cv2.medianBlur(disp8U, 9)
# 鼠标点击事件
def onMouse(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
print('点 (%d, %d) 的三维坐标 (%f, %f, %f)' % (x, y, xyz[y, x, 0], xyz[y, x, 1], xyz[y, x, 2]))
# 显示图片
cv2.imshow("disparity", disp8U)
cv2.setMouseCallback("disparity", onMouse, 0)
cv2.waitKey(0)
cv2.destroyAllWindows()
通过xyz最终生成的图像类似如下，全黑的区域是无法测量的距离或者过远的距离。直接使用鼠标点击图像的位置就可以输出该点的三维坐标了。
点 (777, 331) 的三维坐标 (57.852905, -23.240721, 519.846985)
点 (553, 383) 的三维坐标 (-70.191200, 6.370971, 525.825806)
点 (573, 305) 的三维坐标 (-58.206013, -38.124424, 521.407104)
点 (1012, 694) 的三维坐标 (96.540405, 92.311638, 262.277863)
以上的源码笔者提供了下载，包括测试图像。
源码下载地址： https://resource.doiduoyi.com/#ckg1m1u