双目测距

环境配置

---

Visual Studio 2013

---

Python

---

相机标定

---

双目测距，当然要准备两个摄像头了，可以直接在淘宝上购买，也可以根据自己的需求进行定制。将两个摄像头的相对位置固定好，本模型采用前向平行的双目立体视觉模型。即两相机的光轴相互平行。左右两个成像平面也处于同一水平面上。




因为摄像头的位置是由我们手动进行定位的，所以需要进行相机标定，即通过目标点在图像坐标系和世界坐标系中的位置来推导相机内外参数矩阵的过程。
相机标定采用张正友相机标定算法。张正友相机标定算法指的是1998年提出的单平面棋盘格的相机标定算法。这种相机标定算法介于传统标定法和自标定法之间，克服了传统标定法需要的高精度标定物的缺点，仅需使用一个打印出来的棋盘格就可以。我们只需对标定板的不同方向进行拍摄，然后对拍摄所得的图像进行处理。
本次相机标定使用MATLAB的Camera Calibration Toolbox工具箱进行标定。

图片采集

---

import cv2

cv2.namedWindow("left", 0)
cv2.namedWindow("right", 0)
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 801, 0)
cv2.resizeWindow("left", 800, 600)
cv2.resizeWindow("right", 800, 600)
left_camera = cv2.VideoCapture(0)
right_camera = cv2.VideoCapture(1)

# 设置左右摄像头的分辨率
left_camera.set(3, 800)
left_camera.set(4, 600)
right_camera.set(3, 800)
right_camera.set(4, 600)

counter = 0

# 棋盘格尺寸
pattern = (7, 5) 

# 拍照文件目录
folder = 'D:/img/img_q/' 

def shot(pos, frame):
    global counter
    path = folder + pos + "_" + str(counter) + ".bmp"

    cv2.imwrite(path, frame)
    print("snapshot saved into: " + path)

while True:
    ret, left_frame = left_camera.read()
    ret, right_frame = right_camera.read()

    cv2.imshow("left", left_frame)
    cv2.imshow("right", right_frame)

    key = cv2.waitKey(1)
    if key == ord('q'):
        break
    elif key == ord('s'):
        shot("left", left_frame)
        shot("right", right_frame)
        counter += 1

left_camera.release()
right_camera.release()
cv2.destroyWindow("left")
cv2.destroyWindow("right")

按s来采集左右相机的图片，采集到的图片如下：
left：

right：

左右相机各采集20~30张图片即可。
注意：图片采集时，要求棋盘完整出现在左右视野。不能有部分未出现情况。

左右相机标定

---

先将MATLAB标定工具箱下载下来。
MATLAB相机标定工具箱
提取码：jnme
将下载下来的工具箱解压。
将采集到的图片拷贝到下面文件夹（注意图片名要对应）。

然后将上述文件夹拷贝到MATLAB安装路径下的toolbox文件夹下。

打开MATLAB，并将工作路径修改为刚才拷贝过去的文件夹。

在命令行窗口输入calib_gui会出现下面窗口。

单击Standard出现如下窗口。

我们先对左相机的图片进行标定，点击Image names，命令行窗口会提示你输入图片的basename（一组图片的公共前缀）以及图片的格式。


点击Extract grid corners。

开始角点提取。

此处全部回车使用默认值。开始提取第一张图片。

提取完第一张后不要立即回车，在命令行输入棋盘格的实际尺寸。这里使用的棋盘格为58mm x 58mm。

第一张提取效果。


回车继续下一张。后序数据全部使用默认即可，直到left_图像全部提取完毕。

点击Calibration开始标定。




点击Save。

将保存的文件的名字进行修改。左相机结果改为Calib_Result_left.mat，右相机结果改为Calib_Result_right.mat。

同左相机的标定流程，对右相机进行标定。

立体标定

---

左右相机都标定完成后开始立体标定。
命令中输入stereo_gui启动立体标定面板。

点击Load left and right calibration files，加载左右相机的标定结果。


点击Run stereo calibration。


保存上述结果。


可以看到，两摄像头位置基本是前向平行的。

测距

---

相机标定数据的使用

---

camera_configs.py

import cv2
import numpy as np

left_camera_matrix = np.array([[406.37454, 0., 326.69505],
                               [0., 407.55708, 245.46379],
                               [0., 0., 1.]])
left_distortion = np.array([[0.02645, -0.05332, -0.00019, 0.00152, 0.00000]])

right_camera_matrix = np.array([[406.44618, 0., 340.65160],
                                [0., 407.38844, 258.01597],
                                [0., 0., 1.]])
right_distortion = np.array([[0.01690, -0.03804, 0.00077, 0.00119, 0.00000]])

# 旋转矩阵。
om = np.array([0.00733, -0.00336, -0.00375]) 
# 使用Rodrigues变换将om变换为R。
R = cv2.Rodrigues(om)[0]  
# 平移矩阵。
T = np.array([-120.25274, 0.70313, -1.53635]) 

# 图像尺寸。
size = (800, 600) 

# 进行立体矫正。
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
                                                                  right_camera_matrix, right_distortion, size, R,                                                                  T)
# 计算矫正map。
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)

代码演示

---

import numpy as np
import cv2
import camera_configs

cv2.namedWindow("left", 0)
cv2.namedWindow("right", 0)
cv2.namedWindow("depth")
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 404, 0)
cv2.resizeWindow("left", 400, 300)
cv2.resizeWindow("right", 400, 300)
cv2.createTrackbar("num", "depth", 0, 10, lambda x: None)
cv2.createTrackbar("blockSize", "depth", 15, 255, lambda x: None)

camera1 = cv2.VideoCapture(0)
camera2 = cv2.VideoCapture(1)

camera1.set(3, 800)
camera1.set(4, 600)
camera2.set(3, 800)
camera2.set(4, 600)

# 添加点击事件，打印当前点的距离。
def callbackFunc(e, x, y, f, p):
    if e == cv2.EVENT_LBUTTONDOWN:        
        print (threeD[y][x])

cv2.setMouseCallback("depth", callbackFunc, None)

while True:
    ret1, frame1 = camera1.read()
    ret2, frame2 = camera2.read()

    if not ret1 or not ret2:
        break

	# 根据更正map对图片进行重构。
    img1_rectified = cv2.remap(frame1, camera_configs.left_map1, 
                camera_configs.left_map2, cv2.INTER_LINEAR)
    img2_rectified = cv2.remap(frame2, camera_configs.right_map1, 
                camera_configs.right_map2, cv2.INTER_LINEAR)

	# 将图片转为灰度图，为StereoBM做准备。
    imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
    imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY)

	# 两个trackbar用来调节不同的参数查看效果。
    num = cv2.getTrackbarPos("num", "depth")
    blockSize = cv2.getTrackbarPos("blockSize", "depth")
    if blockSize % 2 == 0:
        blockSize += 1
    if blockSize < 5:
        blockSize = 5

	# 根据Block Maching方法生成差异图（opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试）。
    stereo = cv2.StereoBM_create(numDisparities=16*num, blockSize=blockSize)
    disparity = stereo.compute(imgL, imgR)

    disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
    # 将图片扩展至3d空间中，其z方向的值则为当前的距离
    threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32)/16., camera_configs.Q)

    cv2.imshow("left", img1_rectified)
    cv2.imshow("right", img2_rectified)
    cv2.imshow("depth", disp)

camera1.release()
camera2.release()
cv2.destroyAllWindows()

运行结果

---