前言¶
在計算機視覺中，可以通過雙目攝像頭實現，常用的有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)
以上的源碼筆者提供了下載，包括測試圖像。
源碼下載地址： 點擊下載