[部分二]:https://blog.csdn.net/Thomson617/article/details/103961274
第七章.相機標定與3D重構
42.攝像頭標定
在圖像測量過程以及機器視覺應用中，爲確定空間物體表面某點的三維幾何位置與其在圖像中對應點之間的相互關係，必須建立相機成像的幾何模型，這些幾何模型參數就是相機參數。在大多數條件下這些參數必須通過實驗與計算才能得到，這個求解參數（內參、外參、畸變參數）的過程就稱之爲相機標定（或攝像機標定）。無論是在圖像測量或者機器視覺應用中，相機參數的標定都是非常關鍵的環節，其標定結果的精度及算法的穩定性直接影響相機工作產生結果的準確性。因此，做好相機標定是做好後續工作的前提，提高標定精度是科研工作的重點所在。
本節中的函數使用了所謂的針孔相機模型。在該模型中，利用透視變換將三維點投射到圖像平面上，形成場景視圖。
或者:
(X,Y,Z)是三維點在世界座標空間中的座標.
(u,v)爲投影點的座標，單位爲像素.
A是一個相機矩陣，或者說是一個固有參數矩陣.
(cx,cy)是通常位於圖像中心的一個主要點.
fx,fy是用像素單位表示的焦距。
因此，如果相機中的圖像按比例縮放一個因子，所有這些參數都應該按相同的比例縮放(分別乘以/除以)。本徵參數矩陣不依賴於所觀察的場景。因此，一旦確定，它可以重複使用，只要焦距是固定的(變焦鏡頭)。旋轉矩陣-平移矩陣[R|t]稱爲外部參數矩陣。它被用來描述相機在靜態場景中的運動，反之亦然，靜止鏡頭前物體的剛體運動。也就是說，[R|t]將一個點(X,Y,Z)的座標轉換成一個相對於攝像機固定的座標系。當z≠0時，上述轉換等價如下:
下圖爲針孔相機模型:
補充說明: fx、fy、cx、cy只與攝像機內部參數有關，故稱爲內參數矩陣。
其中fx ＝ f/dX ,fy = f/dY ,分別稱爲u軸和v軸上的歸一化焦距；f是相機的焦距，dX和dY分別表示傳感器u軸和v軸上單位像素的尺寸大小。cx和cy則表示的是光學中心，即攝像機光軸與圖像平面的交點，通常位於圖像中心處，故其值常取分辨率的一半。
真實的鏡頭通常會有一些畸變，主要是徑向畸變和輕微的切向畸變。因此，將上述模型擴展爲:
k1、k2、k3、k4、k5和k6是徑向畸變係數。
p1和p2是切向畸變係數。
s1 s2 s3 s4是薄棱鏡畸變係數。
在OpenCV中沒有考慮高階係數。
下圖顯示了兩種常見的徑向畸變:桶形畸變(通常爲k1<0)和針形畸變(通常爲k1>0)。
在某些情況下，圖像傳感器可能是傾斜的，以聚焦相機前面的斜面(Scheimpfug條件)。這可以用於粒子圖像測速(PIV)或激光風扇三角測量。傾斜會導致x "和y "的透視變形。這種扭曲可以用以下方式建模，見例[126]。
矩陣R(τx,τy)被定義爲兩個旋轉角參數τx和τy
在下面的函數中，係數作爲參數傳遞或返回向量:
也就是說,如果向量包含四個元素，那麼k3=0。畸變係數不依賴於所看到的場景。因此，它們也屬於相機的固有參數。不管捕獲的圖像分辨率如何，它們都保持不變。例如，如果一臺相機在320 x 240分辨率的圖像上進行了校準，那麼同樣的畸變係數可以用於同一臺相機的640 x 480圖像，而fx、fy、cx和cy則需要進行適當的縮放。
外部參數包括旋轉矩陣和平移向量,它可以將 3D 座標轉換到座標系統中。
在 3D 相關應用中,必須要先校正這些畸變。爲了找到這些參數,我們必須要提供一些包含明顯圖案模式的樣本圖片（比如說棋盤(8*5)）:
或9*6棋盤格:
我們可以在上面找到一些特殊點（如棋盤的四個角點）。我們起到這些特殊點在圖片中的位置以及它們的真是位置。有了這些信息,我們就可以使用數學方法求解畸變係數。爲了得到更好的結果,我們至少需要 20 個這樣的圖案模式。
如上所述,我們至少需要 20 圖案模式來進行攝像機標定。OpenCV 自帶 了一些棋盤圖像（/sample/cpp/left001.jpg--left14.jpg）, 所以我們可以使 用它們。爲了便於理解,我們可以認爲僅有一張棋盤圖像。重要的是在進行攝 像機標定時我們要輸入一組 3D 真實世界中的點以及與它們對應 2D 圖像中的 點。2D 圖像的點可以在圖像中很容易的找到。（這些點在圖像中的位置是棋盤 上兩個黑色方塊相互接觸的地方）那麼真實世界中的 3D 的點呢？這些圖像來源與靜態攝像機和棋盤不同 的擺放位置和朝向。所以我們需要知道（X,Y,Z）的值。但是爲了簡單,我 們可以說棋盤在 XY 平面是靜止的,（所以 Z 總是等於 0）攝像機在圍着棋 盤移動。這種假設讓我們只需要知道 X,Y 的值就可以了。現在爲了求 X, Y 的值,我們只需要傳入這些點（0,0）,（1,0）,（2,0）...,它們代表了點的 位置。在這個例子中,我們的結果的單位就是棋盤（單個）方塊的大小。但 是如果我們知道單個方塊的大小（比如30mm）,我們輸入的值就可以是（0,0）,（30,0）,（60,0）...,結果的單位就是mm。
3D 點被稱爲對象點,2D 圖像點被稱爲圖像點。
42.1角點提取
爲了找到棋盤的圖案,我們要使用函數 cv2.findChessboardCorners()。 我們還需要傳入圖案的類型,比如說 9x6 的格子或 5x5 的格子等。在本例中我們使用的是 8x5 的格子（通常情況下棋盤都是 9x6 或者 8x5）。它會返回角點,如果得到圖像的話返回值類型（Retval）就會是 True。這些角點會按順序排列（從左到右,從上到下）。
其它：這個函數可能不會找出所有圖像中應有的圖案。所以一個好的方法是編 寫代碼,啓動攝像機並在每一幀中檢查是否有應有的圖案。在我們獲得圖案之 後我們要找到角點並把它們保存成一個列表。在讀取下一幀圖像之前要設置一定的間隔,這樣我們就有足夠的時間調整棋盤的方向。繼續這個過程直到我們 得到足夠多好的圖案。就算是我們舉得這個例子,在所有的 14 幅圖像中也不 知道有幾幅是好的。所以我們要讀取每一張圖像從其中找到好的能用的。
其它：除了使用棋盤之外,我們還可以使用環形格子,但是要使用函數cv2.findCirclesGrid()來找圖案。據說使用環形格子只需要很少的圖像就可以了。
在找到這些角點之後我們可以使用函數cv2.cornerSubPix() 增加準確度。我們使用函數 cv2.drawChessboardCorners() 繪製圖案。所有的這些步驟都被包含在下面的代碼中了：
import numpy as np
import cv2
import glob
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((8*5,3), np.float32)
objp[:,:2] = np.mgrid[0:8,0:5].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob('*.jpg')
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (8,5),None)
# If found, add object points, image points (after refining them)
if ret == True: objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray,corners,(4,4),(-1,-1),criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (8,5), corners2,ret)
cv2.imshow('img',img)
cv2.waitKey(2000)
cv2.destroyAllWindows()
一副圖像和被繪製在上邊的圖案：
42.2相機標定
(1).普通鏡頭的單目標定
在得到這些對象點和圖像點之後,開始做攝像機標定。 我們要使用的函數是 cv2.calibrateCamera()。它會返回攝像機矩陣,畸變係數,旋轉和平移向量等。
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs[, rvecs[, tvecs[, flags[, criteria]]]])
retval, cameraMatrix, distCoeffs, rvecs, tvecs,stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors = cv2.calibrateCameraExtended(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, [, rvecs[, tvecs[, stdDeviationsIntrinsics[, stdDeviationsExtrinsics[, perViewErrors[, flags[, criteria]]]]]]])
. objectPoints 校準模式點在校準模式座標空間中的向量.
. imagePoints 校正模式點的投影向量。對於每個i, imagePoints.size()和objectPoints.size()以及imagePoints[i].size()必須等於objectPoints[i].size().
. imageSize 圖像的大小隻用於初始化相機的固有矩陣.
. cameraMatrix 輸出3x3浮點相機矩陣{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}。如果指定了fisheye::CALIB_USE_INTRINSIC_GUESS/，則必須在調用函數之前初始化部分或全部fx、fy、cx、cy.
. distCoeffs 輸出向量:畸變係數(k_1, k_2, p_1,p_2,k_3).
. rvecs 爲每個模式視圖估計旋轉向量的輸出向量(參見Rodrigues)。即,每個k旋轉矢量與相應的k翻譯(見下一個輸出參數描述)將校準模式從模型座標空間(對象指定點)向世界座標空間,也就是說,一個真正的位置校準模式在k模式視圖(k = 0 . .* M * 1).
. tvecs 每個模式視圖估計的平移向量的輸出向量.
. stdDeviationsIntrinsics輸出向量的標準差估計的內在參數。偏差值的順序:(fx,fy,cx,cy,k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,τx,τy)如果參數之一沒有估計,偏差等於零 .
. stdDeviationsExtrinsics輸出向量的標準偏差估計的外部參數。偏差值的順序:(R1,T1，…，RM,TM)其中M爲模式視圖的數量，Ri,Ti爲串聯的1x3向量.
. perViewErrors 爲每個模式視圖估計的RMS重投影誤差的輸出向量.
. flags 可能爲零或下列值的組合的不同標誌:
CALIB_USE_INTRINSIC_GUESS cameraMatrix包含有效的初始值fx, fy, cx, cy進一步優化。否則，(cx, cy)初始設置爲圖像中心(使用imageSize)，並以最小二乘方式計算焦距。注意，如果已知內部參數，則不需要使用此函數來估計外部參數。而不是使用solvePnP.
CALIB_FIX_PRINCIPAL_POINT 在全局優化過程中，主點沒有改變。當CALIB_USE_INTRINSIC_GUESS也被設置時，它會停留在指定的中心或不同的位置.
CALIB_FIX_ASPECT_RATIO 這些函數只將fy作爲一個自由參數。fx/fy的比例與輸入的camera amatrix保持一致。當CALIB_USE_INTRINSIC_GUESS未設置時，fx和fy的實際輸入值被忽略，只計算它們的比值並進一步使用.
CALIB_ZERO_TANGENT_DIST 切向畸變係數(p1,p2)設置爲零並保持爲零.
CALIB_FIX_K1,...,CALIB_FIX_K6 優化過程中不改變相應的徑向畸變係數。如果設置CALIB_USE_INTRINSIC_GUESS，則使用提供的distCoeffs矩陣的係數。否則設置爲0.
CALIB_RATIONAL_MODEL 啓用係數k4、k5和k6。爲了提供向後兼容性，應該顯式地指定這個額外的標誌，以使校準函數使用rational模型並返回8個係數。如果沒有設置該標誌，該函數計算並只返回5個失畸變數.
CALIB_THIN_PRISM_MODEL 啓用了係數s1、s2、s3和s4。爲了提供向後兼容性，應該明確指定這個額外的標記，使校準功能使用瘦棱鏡模型並返回12個係數。如果沒有設置該標誌，該函數計算並只返回5個畸變係數.
CALIB_FIX_S1_S2_S3_S4 優化過程中不改變薄棱鏡畸變係數。如果設置CALIB_USE_INTRINSIC_GUESS,則使用提供的distCoeffs矩陣的係數。否則設置爲0.
CALIB_TILTED_MODEL 啓用了係數tauX和tauY。爲了提供向後兼容性，應該明確指定這個額外的標誌，使校準功能使用傾斜的傳感器模型並返回14個係數。如果沒有設置該標誌，該函數計算並只返回5個畸變係數.
CALIB_FIX_TAUX_TAUY 優化過程中不改變傾斜傳感器模型的係數。如果設置CALIB_USE_INTRINSIC_GUESS，則使用提供的distCoeffs矩陣的係數。否則設置爲0.
. criteria 迭代優化算法的終止條件.
(2).廣角/魚眼鏡頭標定
定義:設P爲世界座標系中三維座標X中的一個點(存儲在矩陣X中)攝像機座標系中P的座標向量爲:Xc=RX+T。
其中R爲旋轉向量om所對應的旋轉矩陣:R = rodrigues(om);稱x、y、z爲Xc的三個座標:
P的針孔投影座標爲[a;b]
魚眼畸變:
畸變點座標爲[x';y']
最後，轉換爲像素座標:最終像素座標向量[u;v]地點:
廣角/魚眼鏡頭標定使用的函數是 cv2.fisheye.calibrate()。它會返回攝像機矩陣,畸變係數,旋轉和平移向量等。
retval, K, D, rvecs, tvecs =cv2.fisheye.calibrate(objectPoints, imagePoints, image_size, K, D[, rvecs[, tvecs[, flags[, criteria]]]])
參數:
. K 輸出3x3浮點相機矩陣{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}.
. D 輸出向量:畸變係數(k_1, k_2, k_3, k_4).
. flags 可能爲零或下列值的組合的不同標誌:
- **fisheye::CALIB_USE_INTRINSIC_GUESS** cameraMatrix包含有效的初始值fx, fy, cx, cy進一步優化。否則，(cx, cy)初始設置爲圖像中心(使用imageSize)，並以最小二乘方式計算焦距.
- **fisheye::CALIB_RECOMPUTE_EXTRINSIC** 外部的將重新計算後，每次迭代的內在優化.
- **fisheye::CALIB_CHECK_COND** 函數將檢查條件數的有效性.
- **fisheye::CALIB_FIX_SKEW** 傾斜係數(alpha)設置爲0並保持爲0.
- **fisheye::CALIB_FIX_K1..fisheye::CALIB_FIX_K4** 選定的畸變係數設置爲零，並保持零.
- **fisheye::CALIB_FIX_PRINCIPAL_POINT** 在全局優化過程中，主點沒有改變。當CALIB_USE_INTRINSIC_GUESS也被設置時，它會停留在指定的中心或不同的位置.
(3).雙目標定
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize[, R[, T[, E[, F[, flags[, criteria]]]]]])
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F, perViewErrors = cv2.stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize[, R[, T[, E[, F[, perViewErrors[, flags[, criteria]]]]])
retval, K1, D1, K2, D2, R, T = cv2.fisheye.stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize[, R[, T[, flags[, criteria]]]])
該函數估計兩個攝像機之間的轉換，形成一個立體對。如果你有一個相對位置和方向是固定的兩個攝像頭的立體相機,如果你分別(這可以用solvePnP)計算了一個對象的相對於第一個相機和第二個相機的姿態,(R1, T1)和(R2, T2),那麼這些姿勢肯定是相互關聯的。這意味着，給定(R1, T1)計算(R2, T2)是可能的。你只需要知道第二個相機相對於第一個相機的位置和方向。這就是所描述的函數的作用。它計算(R, T)使得:
可選地，它計算基本矩陣E:
其中Ti是平移向量T的分量:T=[T0, T1, T2]^T。這個函數也可以計算基本矩陣F:
除了與立體相關的信息外，該函數還可以對每臺攝像機進行完整的標定。然而，由於參數空間的高維性和輸入數據中的噪聲，函數可能偏離正確的解。如果每個相機的固有參數都能得到高精度的估計(例如，使用calibrateCamera)，建議您這樣做，然後將CALIB_FIX_INTRINSIC標誌和計算得到的固有參數傳遞給函數。否則，如果一次性估計所有參數，則有必要限制某些參數，例如傳遞CALIB_SAME_FOCAL_LENGTH和CALIB_ZERO_TANGENT_DIST標誌，這通常是一個合理的假設。
與calibrateCamera類似，該函數最小化了兩個相機所有可用視圖中所有點的總重投影誤差。函數返回重新投影錯誤的最終值。
42.3畸變校正
如今的低價單孔攝像機（照相機）會給圖像帶來很多畸變。畸變主要有兩種：徑向畸變和切向畸變。如下圖所示,用紅色直線將棋盤的兩個邊標註出來:
但是你會發現棋盤的邊界並不和紅線重合。所有我們認爲應該是直線的也都凸出來了。你可以通過訪問Distortion(optics)獲得更多相關細節。
這種畸變可以通過下面的方程組進行糾正：
與此相似,另外一個畸變是切向畸變,這是由於透鏡與成像平面不可能絕對平行造成的。這種畸變會造成圖像中的某些點看上去的位置會比我們認爲的位置要近一些。它可以通過下列方程組進行校正：
簡單來說,如果我們想對畸變的圖像進行校正就必須找到五個造成畸變的係數：
Distortion cofficients = ( k1 , k2 , p1 , p2 , k3 )
畸變（distortion）是對直線投影（rectilinear projection）的一種偏移。簡單來說直線投影是場景內的一條直線投影到圖片上也保持爲一條直線。畸變簡單來說就是一條直線投影到圖片上不能保持爲一條直線了，這是一種光學畸變，可能由於攝像機鏡頭的原因。鏡頭畸變係數一般有徑向畸變和切向畸變兩種參數組成:
徑向畸變有：k1,k2,k3,[k4,k5,k6]
切向畸變有：p1,p2
精確標定時也就用到這麼五個參數。
具體得到畸變係數的大小跟相機鏡頭的物理屬性還有你選擇的畸變模型有關。
通常針對消費級相機鏡頭採用多項式畸變模型得到的畸變係數都不超過1。但是如果你對魚眼鏡頭採用只含有一個徑向畸變係數k1的畸變模型進行標定，這種情況下得到的畸變係數可能會大於1。
OpenCV 提供了兩種方法。
不過在那之前我們可以使用函數 cv2.getOptimalNewCameraMatrix() (fisheye中的方法爲estimateNewCameraMatrixForUndistortRectify())得到自由縮放係數進而對攝像機矩陣進行優化,獲取優化後的內置參數。如果縮放係數 alpha = 0,返回的非畸變圖像會帶有最少量的不想要的像素。它甚至有可能在圖像角點去除一些像素。如果 alpha = 1,所有的像素都會被返回,還有一些黑圖像。它還會返回一個 ROI 圖像,我們可以 用來對結果進行裁剪。
我們讀取一個新的圖像（left2.ipg）
img = cv2.imread('left2.jpg')
h, w = img.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))
方式一: 使用 cv2.undistort() 。
只需使用這個函數和上邊得到 的 ROI 對結果進行裁剪。
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('calibresult.png',dst)
方式二: 使用 cv2.remap() 。
首先我們要找到從畸變圖像到 非畸變圖像的映射方程。再使用重映射方程。
# undistort
mapx,mapy = cv2.initUndistortRectifyMap(mtx,dist,None,newcameramtx,(w,h),5)
dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR)
# crop the image
x,y,w,h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('calibresult.png',dst)
這兩中方法給出的結果是相同的。結果如下所示：
你會發現結果圖像中所有的邊界都變直了。
現在我們可以使用 Numpy 提供寫函數（np.savez,np.savetxt 等） 將攝像機矩陣和畸變係數保存以便以後使用。
42.4反向投影誤差
我們可以利用反向投影誤差對我們找到的參數的準確性進行估計。得到的 結果越接近 0 越好。有了內部參數,畸變參數和旋轉變換矩陣,我們就可以使 用 cv2.projectPoints() 將對象點轉換到圖像點。然後就可以計算變換得到 圖像與角點檢測算法的絕對差了。然後我們計算所有標定圖像的誤差平均值。
mean_error = 0
for i in xrange(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
tot_error += error
print ("total error: ", mean_error/len(objpoints))
42.6標定完整代碼
(單目,雙目,廣角,魚眼...)參見我的另一篇博文:https://blog.csdn.net/Thomson617/article/details/103506391
43.姿勢估計
在上一節的攝像機標定中,我們已經得到了攝像機矩陣,畸變係數等。有 了這些信息我們就可以估計圖像中圖案的姿勢,比如目標對象是如何擺放,如何旋轉等。對一個平面對象來說,我們可以假設 Z=0,這樣問題就轉化成攝像機在空間中是如何擺放（然後拍攝）的。所以,如果我們知道對象在空間中的姿勢,我們就可以在圖像中繪製一些 2D 的線條來產生 3D 的效果。我們來看 一下怎麼做吧。
我們的問題是:在棋盤的第一個角點繪製 3D 座標軸（X,Y,Z 軸）。X 軸爲藍色,Y 軸爲綠色,Z 軸爲紅色。在視覺效果上來看,Z 軸應該是垂直與 棋盤平面的。
首先我們要加載前面結果中攝像機矩陣和畸變係數。
import cv2
import numpy as np
import glob
# Load previously saved data
with np.load('B.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
現在我們來創建一個函數：draw, 它的參數有棋盤上的角點（使用
cv2.findChessboardCorners() 得到）和要繪製的 3D 座標軸上的點。
def draw(img, corners, imgpts):
corner = tuple(corners[0].ravel())
img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)
img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)
img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)
return img
和前面一樣,我們要設置終止條件,對象點（棋盤上的 3D 角點）和座標軸點。3D 空間中的座標軸點是爲了繪製座標軸。我們繪製的座標軸的長度爲 3。所以 X 軸從（0,0,0）繪製到（3,0,0）;Y 軸也是;Z 軸從（0,0,0）繪製到（0,0,-3）。負值表示它是朝着（垂直於）攝像機方向。
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((9*6,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
加載圖像去搜尋9x6 的格子,如果發現,我們就把它優化到亞像素級。然後使用函數: cv2.solvePnPRansac() 來計算旋轉和變 換。但我們有了變換矩陣之後,我們就可以利用它們將這些座標軸點映射到圖 像平面中去。簡單來說,我們在圖像平面上找到了與3D 空間中的點(3,0,0),(0,3,0),(0,0,3) 相對應的點。然後就可以使用函數 draw() 從圖像上的第一個角點開始繪製連接這些點的直線了！
for fname in glob.glob('left*.jpg'):
img = cv2.imread(fname)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9,6),None)
if ret == True:
corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
# Find the rotation and translation vectors.
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img,corners2,imgpts)
cv2.imshow('img',img)
k = cv2.waitKey(0) & 0xff
if k == 's':
cv2.imwrite(fname[:6]+'.png', img)
cv2.destroyAllWindows()
結果如下,看到了嗎,每條座標軸的長度都是 3 個格子的長度。
渲染一個立方體:
如果想繪製一個立方體,要對 draw() 函數進行如下修改：修改後的draw()函數：
def draw(img, corners, imgpts):
imgpts = np.int32(imgpts).reshape(-1,2)
# draw ground floor in green
img = cv2.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3)
# draw pillars in blue color
for i,j in zip(range(4),range(4,8)):
img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3)
# draw top layer in red color
img = cv2.drawContours(img, [imgpts[4:]],-1,(0,0,255),3)
return img
修改後的座標軸點。它們是 3D 空間中的一個立方體的 8 個角點：
結果如下：
如果你對計算機圖形學感興趣的話,爲了增加圖像的真實性,你可以使用OpenGL (開放圖形庫)來渲染更復雜的圖形。
44.對極幾何（Epipolar Geometry）
在使用針孔相機時,我們會丟失大量重要的信息,比如說圖像的深度, 或者說圖像上的點和攝像機的距離,因爲這是一個從 3D 到 2D 的轉換。因此一 個重要的問題就是如何使用這樣的攝像機計算出深度信息？答案就是使用多個相機。我們的眼睛就是這樣工作的,使用兩個攝像機（兩個眼睛）, 這被稱爲立體視覺。我們來看看 OpenCV 在這方面給我們都提供了什麼吧。（《學習 OpenCV》一書有大量相關知識） 在進入深度圖像之前,我們要先掌握一些多視角幾何的基本概念。在本節中我們要處理對極幾何。下圖爲使用兩臺攝像機同時對同一個場景進行拍攝的示意圖:
如果只用一臺攝像機我們不可能知道 3D 空間中的 X 點到圖像平面的距離,因爲 OX 連線上的每個點投影到圖像平面上的點都是相同的。但是如果我 們也考慮上右側圖像的話,直線 OX 上的點將投影到右側圖像上的不同位置。 所以根據這兩幅圖像,我們就可以使用三角測量計算出 3D 空間中的點到攝像 機的距離（深度）。這就是整個思路。
直線 OX 上的不同點投射到右側圖像上形成的線 l′ 被稱爲與 x 點對應的極線。也就是說,我們可以在右側圖像中沿着這條極線找到 x 點。它可能在這條 直線上某個位置（這意味着對兩幅圖像間匹配特徵的二維搜索就轉變成了沿着 極線的一維搜索。這不僅節省了大量的計算,還允許我們排除許多導致虛假匹 配的點）。這被稱爲對極約束。與此相同,所有的點在其它圖像中都有與之對應的極線。平面 XOO' 被稱爲對極平面。
O 和 O' 是攝像機的中心。從上面的示意圖可以看出,右側攝像機的中心 O' 投影到左側圖像平面的 e 點,這個點就被稱爲極點。極點就是攝像機中心連 線與圖像平面的交點。因此點 e' 是左側攝像機的極點。有些情況下,我們可能 不會在圖像中找到極點,它們可能落在了圖像之外（這說明這兩個攝像機不能 拍攝到彼此）。
所有的極線都要經過極點。所以爲了找到極點的位置,我們可以先找到多條極線,這些極線的交點就是極點。
本節我們的重點就是找到極線和極點。爲了找到它們,我們還需要兩個元素,本徵矩陣（E）和基礎矩陣（F）。本徵矩陣包含了物理空間中兩個攝像機相關的旋轉和平移信息。如下圖所示（本圖來源自：學習OpenCV）:
基礎矩陣 F 除了包含 E 的信息外還包含了兩個攝像機的內參數。由於 F 包含了這些內參數,因此它可以它在像素座標系將兩臺攝像機關聯起來。（如果使用是校正之後的圖像並通過除以焦距進行了歸一化,F=E）。簡單來說,基 礎矩陣 F 將一副圖像中的點映射到另一幅圖像中的線（極線）上。這是通過匹 配兩幅圖像上的點來實現的。要計算基礎矩陣至少需要 8 個點（使用 8 點算 法）。點越多越好,可以使用 RANSAC 算法得到更加穩定的結果。
爲了得到基礎矩陣我們應該在兩幅圖像中找到儘量多的匹配點。我們可以使用 SIFT 描述符,FLANN 匹配器和比值檢測。
import cv2
import numpy as np
from matplotlib import pyplot as plt
img1 = cv2.imread('myleft.jpg',0) #queryimage # left image
img2 = cv2.imread('myright.jpg',0) #trainimage # right image
sift = cv2.SIFT()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1,None)
kp2, des2 = sift.detectAndCompute(img2,None)
# FLANN parameters
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1,des2,k=2)
good = []
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.8*n.distance: good.append(m)
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
現在得到了一個匹配點列表,我們就可以使用它來計算基礎矩陣了。
pts1 = np.int32(pts1) pts2 = np.int32(pts2)
'''
retval, mask=cv2.findFundamentalMat(points1, points2, method, ransacReprojThreshold, confidence, mask)
--points1：從第一張圖片開始的N個點的數組。點座標應該是浮點數(單精度或雙精度)。
--points2：與點1大小和格式相同的第二圖像點的數組。
--method：計算基本矩陣的方法。
* cv2.FM_7POINT for a 7-point algorithm. N=7
* cv2.FM_8POINT for an 8-point algorithm. N≥8
* cv2.FM_RANSAC （默認） for the RANSAC algorithm. N≥8
* cv2.FM_LMEDS for the LMedS algorithm. N≥8
--ransacReprojThreshold：僅用於RANSAC方法的參數，默認3。它是一個點到極線的最大距離(以像素爲單位)，超過這個點就被認爲是一個離羣點，不用於計算最終的基本矩陣。根據點定位、圖像分辨率和圖像噪聲的準確性，可以將其設置爲1-3左右。
--confidence：僅用於RANSAC和LMedS方法的參數，默認0.99。它指定了一個理想的置信水平(概率)，即估計矩陣是正確的。
--mask：輸出
'''
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_LMEDS)
# We select only inlier points
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]
下一步我們要找到極線。我們會得到一個包含很多線的數組。所以我們要定義一個新的函數將這些線繪製到圖像中。
def drawlines(img1,img2,lines,pts1,pts2):
''' img1 - image on which we draw the epilines for the points in img2
lines - corresponding epilines
'''
r,c = img1.shape
img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
for r,pt1,pt2 in zip(lines,pts1,pts2):
color = tuple(np.random.randint(0,255,3).tolist())
x0,y0 = map(int, [0, -r[2]/r[1] ])
x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])
img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1)
img1 = cv2.circle(img1,tuple(pt1),5,color,-1)
img2 = cv2.circle(img2,tuple(pt2),5,color,-1)
return img1,img2
現在在兩幅圖像中計算並繪製極線。
# 找出對應於右圖像(第二幅圖像)中各點的後記，並在左圖像上畫出相應的線
'''
lines = cv.computeCorrespondEpilines(points, whichImage, F, lines)
-- points：輸入點。類型爲CV_32FC2N×1或1×N矩陣。
--whichImage：包含點的圖像(1或2)的索引。
--F：基本矩陣，可使用findFundamentalMat或stereoRectify 進行估計。
--lines：對應於另一幅圖像中點的極線的輸出向量（a，b，c）表示直線ax+by+c=0
'''
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F)
lines1 = lines1.reshape(-1,3)
img5,img6 = drawlines(img1,img2,lines1,pts1,pts2)
# 找出對應於左圖像(第一幅圖像)中的點的後記，並在右圖像上畫出相應的線
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F)
lines2 = lines2.reshape(-1,3)
img3,img4 = drawlines(img2,img1,lines2,pts2,pts1)
plt.subplot(121),plt.imshow(img5)
plt.subplot(122),plt.imshow(img3)
plt.show()
結果如下：
從上圖可以看出所有的極線都匯聚以圖像外的一點,這個點就是極點。 爲了得到更好的結果,我們應該使用分辨率比較高的圖像和 non-planar點。
更多資源
1.一個重要的話題是相機的前進。然後，將在兩個位置的相同位置看到極點，並且從固定點出現極點。See this discussion.
2.基本矩陣估計對匹配質量、離羣值等敏感。當所有選擇的匹配都位於同一平面上時，情況會變得更糟. Checkthis discussion.
45.雙目立體視覺
雙目立體視覺（Binocular Stereo Vision）是機器視覺的一種重要形式，它是基於視差原理並利用成像設備從不同的位置獲取被測物體的兩幅圖像，通過計算圖像對應點間的位置偏差，來獲取物體三維幾何信息的方法。
雙目立體視覺理論建立在對人類視覺系統研究的基礎上，通過雙目立體圖象的處理，獲取場景的三維信息，其結果表現爲深度圖，再經過進一步處理就可得到三維空間中的景物，實現二維圖象到三維空間的重構。Marr-Poggio-Grimson最早提出並實現了一種基於人類視覺系統的計算視覺模型及算法。雙目立體視覺系統中，獲取深度信息的方法比其它方式（如由影到形方法）較爲直接，它是被動方式的，因而較主動方式（如程距法）適用面寬，這是它的突出特點。
雙目立體視覺融合兩隻眼睛獲得的圖像並觀察它們之間的差別，使我們可以獲得明顯的深度感，建立特徵間的對應關係，將同一空間物理點在不同圖像中的映像點對應起來，這個差別稱作視差(Disparity)圖像。
深度圖像也叫距離影像，是指將從圖像採集器到場景中各點的距離（深度）值作爲像素值的圖像。獲取方法有：激光雷達深度成像法、計算機立體視覺成像、座標測量機法、莫爾條紋法、結構光法。
點雲：當一束激光照射到物體表面時，所反射的激光會攜帶方位、距離等信息。若將激光束按照某種軌跡進行掃描，便會邊掃描邊記錄到反射的激光點信息，由於掃描極爲精細，則能夠得到大量的激光點，因而就可形成激光點雲。深度圖像經過座標轉換可以計算爲點雲數據；有規則及必要信息的點雲數據可以反算爲深度圖像。 兩者在一定條件下是可以相互轉化。深度圖不適合直觀的去察看，點雲的效果會更強，所以，一般我們都是將深度圖轉成點雲再察看。
雙目立體視覺的最終目標:三維重建。基於視覺的三維重建，指的是通過攝像機獲取場景物體的數據圖像，並對此圖像進行分析處理，再結合計算機視覺知識推導出現實環境中物體的三維信息。三維重建技術是計算機視覺、人工智能、虛擬現實等前沿領域的熱點和難點，也是人類在基礎研究和應用研究中面臨的重大挑戰之一。基於圖像的三維重建是圖像處理的一個重要研究分支，作爲當今熱門的虛擬現實和科學可視化的基礎，它被廣泛應用於檢測和觀察中。一個完整的三維重建系統通常可分爲圖像獲取、攝像機標定、特徵點提取、立體匹配、深度確定和後處理等6大部分。
三維重建視覺作爲計算機視覺中的一個重要分支，一直是計算機視覺研究的重點和熱點之一。它直接模擬了人類視覺處理景物的方式，可以在多種條件下靈活地測量景物的立體信息。對它的研究，無論是在視覺生理的角度還是在工程應用的角度都具有十分重要的意義。三維重建視覺技術在由物體的二維圖像獲得物體的深度信息上具有很大的優越性。
雙目視覺不僅僅用來測距，一般都把測距用於其它研究領域，如三維重建、圖像融合、增強現實、目標識別與跟蹤等等。
雙目測距的精度和基線長度（兩臺相機之間的距離）有關，兩臺相機布放的距離越遠，測距精度越高。但往往在實際應用中，相機的布放空間是有限的，最多也只有幾米或幾十米的基線長度，這就導致雙目測距在遠距離條件下的精度大打折扣。所以，雙目測距一般用於近距離的高精度測量，而遠距離測距一般用脈衝式的激光測距機。
圖像測量方法的優點是近距離精度高，但是圖像質量受外界光照等條件制約太大，且由於相機性能往往不夠穩定，加上算法相對複雜些，這些都會限制它的應用。總之，雙目測距雖然說是比較成熟的技術，但是其應用範圍有限。
雙目立體視覺深度相機簡化流程:
(1).首先需要對雙目相機進行標定，得到兩個相機的內外參數、單應矩陣。
(2).根據標定結果對原始圖像校正，校正後的兩張圖像位於同一平面且互相平行。
(3).對校正後的兩張圖像進行像素點匹配。
(4).根據匹配結果計算每個像素的深度，從而獲得深度圖。
45.1成像模型
(1).小孔成像模型
小孔成像模型也稱爲針孔模型，是計算機視覺中最理想的一種也是最簡單的一種成像模型，它近似爲線性結構。攝像機成像模型的一個主要作用便是將真實空間中的點與拍攝平面圖像上的點建立起聯繫。考慮到小孔成像模型的計算簡便、便於分析等衆多優點，其使用率比其它兩種模型正交投影模型和擬透視投影模型都要高。
在小孔成像模型中，主要由光心、光軸和成像平面幾個部分組成，且假設所有成像過程都滿足光的直線傳播條件。根據光的直線傳播理論，空間中的物點反射光在經過光心後，投影到平面爲一個倒立的像點。雖然作爲理想的成像模型小孔成像物理性質極佳，但在實際的攝像機光學系統中大多是由透鏡組成，在透鏡成像中需要滿足以下條件：
其中，f ’表示透鏡的焦距，u 表示物距，v 表示像距。在視覺應用的多數情況中都認爲透鏡攝像系統的成像模型與小孔成像模型擁有一致的成像關係，近似認爲 v≈f’。因此小孔成像模型一直以來都作爲相機成像的基本模型來使用。
由於小孔成像的像點倒立，不方便分析和計算，常將光心與視平面進行位置對調。其如圖所示：
(2).平行式與會聚式雙目立體視覺系統模型
相機的擺放位置不同，系統模型的空間幾何結構也不相同，相應的計算方式也會發生改變。依據相機的擺放方式，雙目立體視覺系統模型主要分爲兩種：平行放置方式和會聚放置放式。如圖所示:
其中，由於平行放置方式計算簡便，沒有垂直方向的視差，是最爲常用的模型。會聚方式兩臺相機的擺放位置要求不嚴格，相對比較隨意。按這種方式放置的相機靈活性較強，能依據現場環境、採集對象特點來進行調整，通過改變攝像機間的距離和角度來獲得最好的測量效果。這種方式的一大缺點是計算相對複雜，雖然靈活的調整能保證了兩個相機的共同視野足夠大，但同時也使得後續的處理變得更加麻煩，左右圖像的匹配難度增加了不少。 相機水平平行放置方式又稱爲標準放置方式，要求兩個相機的光軸平行，光心在同一水平線上。在雙目立體視覺技術中要求兩相機的內部參數是一樣的，兩個相機之間的距離稱爲基線，在水平平行放置模型中，理想情況是左相機沿基線距離移動後能與右相機完全重合，在有些場合將單相機安裝在導軌上來進行拍攝便是一種很好的選擇，這種情況下相機的內部參數一致度高，導軌也保證了水平方向的平行度，但這種方式需要移動相機比較不方便，而且無法實現實時測量。雙相機平行放置模型最爲簡單實用，大大減少了計算量也便於分析，尤其在相機的標定校正上可以非常方便。當然它也存在一些缺點，比如當目標和相機較近時比較容易出現盲區，增大視角的話又可能帶來一些畸變，但隨着相機制造水平的提高這種相機模型依然是最佳的選擇，本文采用相機平行放置方式進行分析計算和實驗。
(3).理想雙目相機成像模型
首先我們從理想的情況開始分析:假設左右兩個相機位於同一平面（光軸平行，並且攝像機的水平掃描線位於同一平面時），且相機參數（如焦距f）一致。那麼深度值的推導原理和公式如下。公式只涉及到初中學的三角形相似知識，不難看懂。
根據上述推導，空間點P離相機的距離（深度）z=f*b/d，可以發現如果要計算深度z，必須要知道：
1、相機焦距f，左右相機基線b。這些參數可以通過先驗信息或者相機標定得到。
2、視差d。需要知道左相機的每個像素點(xl, yl)和右相機中對應點(xr, yr)的對應關係。這是雙目視覺的核心問題。
45.2圖像中的四大座標系
(1).世界座標系就是物體在真實世界中的座標，比如黑白棋盤格的世界座標系原點定在第一個棋盤格的頂點，Xw，Yw，Zw互相垂直，Zw方向就是垂直於棋盤格面板的方向。可見世界座標系是隨着物體的大小和位置變化的，單位是長度單位。只要棋盤格的大小決定了，無論板子怎麼動，棋盤格角點座標一般就不再變動(因爲是相對於世界座標系原點的位置不變)，且認爲是Zw=0。
(2).相機座標系以光心爲相機座標系的原點，以平行於圖像的x和y方向爲Xc軸和Yc軸，Zc軸和光軸平行，Xc，Yc，Zc互相垂直，單位是長度單位。
(3).圖像物理座標系以主光軸和圖像平面交點爲座標原點，x和y方向如圖所示，單位是長度單位。
(4).圖像像素座標系以圖像的頂點爲座標原點，u和v方向平行於x和y方向，單位是以像素計。
座標系之間的轉換:
45.3極線約束
先思考一個問題：用兩個相機在不同的位置拍攝同一物體，如果兩張照片中的景物有重疊的部分，我們有理由相信，這兩張照片之間存在一定的對應關係，本節的任務就是如何描述它們之間的對應關係，描述工具是對極幾何 ，它是研究立體視覺的重要數學方法。
要尋找兩幅圖像之間的對應關係，最直接的方法就是逐點匹配，如果加以一定的約束條件:極線約束(epipolar constraint)，搜索的範圍可以大大減小。極線約束對於求解圖像對中像素點的對應關係非常重要。那什麼是極線呢？如下圖所示:
C1，C2是兩個相機，P是空間中的一個點，P和兩個相機中心點C1、C2形成了三維空間中的一個平面PC1C2，稱爲極平面(Epipolar plane)。極平面和兩幅圖像相交於兩條直線，這兩條直線稱爲極線(Epipolar line)。P在相機C1中的成像點是P1，在相機C2中的成像點是P2，但是P的位置事先是未知的。
我們的目標是：對於左圖的P1點，尋找它在右圖中的對應點P2，這樣就能確定P點的空間位置，也就是我們想要的空間物體和相機的距離（深度）。
所謂極線約束（Epipolar Constraint）就是指當同一個空間點在兩幅圖像上分別成像時，已知左圖投影點P1，那麼對應右圖投影點P2一定在相對於P1的極線上，這樣可以極大的縮小匹配範圍。
根據極線約束的定義，我們可以在下圖中直觀的看到P2一定在對極線上，所以我們只需要沿着極線搜索一定可以找到和P1的對應點P2。
上述過程考慮的情況（兩相機共面且光軸平行，參數相同）非常理想，相機C1，C2如果不是在同一直線上怎麼辦？事實上，這種情況非常常見，因爲有些場景下兩個相機需要獨立固定，很難保證光心C1，C2完全水平，即使是固定在同一個基板上也會因爲裝配的原因導致光心不完全水平。如下圖所示。我們看到兩個相機的極線不僅不平行，還不共面，之前的理想模型那一套推導結果用不了了，這可咋辦呢？圖像矯正（Image Rectification）技術就出現了。
45.4圖像矯正(立體校正)
圖像矯正是通過分別對兩張圖片用單應（homography）矩陣變換（可以通過標定獲得）得到的，目的就是把兩個不同方向的圖像平面（下圖中灰色平面）重新投影到同一個平面且光軸互相平行（下圖中黃色平面），這樣就可以用前面理想情況下的模型了，兩個相機的極線也變成水平的了。
經過圖像矯正後，左圖中的像素點只需要沿着水平極線方向搜索對應點就可以了。
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T[, R1[, R2[, P1[, P2[, Q[, flags[, alpha[, newImageSize]]]]]]]])
R1, R2, P1, P2, Q = cv2.fisheye.stereoRectify(K1, D1, K2, D2, imageSize, R, T,flags[, R1[, R2[, P1[, P2[, Q[, newImageSize[, balance[,fov_scale ]]]]]]]])
上述函數計算每個相機的旋轉矩陣，使兩個相機的圖像平面(實際上)是同一個平面。因此，這使得所有的極外線都是平行的，從而簡化了稠密的立體對應問題。該函數以stereoCalibrate計算的矩陣爲輸入。作爲輸出，它提供了兩個旋轉矩陣和兩個新座標中的投影矩陣。該函數區分了以下兩種情況:
Horizontal stereo:第一和第二相機的視圖相對移動，主要沿着x軸(可能有小的垂直移動)。在校正後的圖像中，左右攝像頭對應的極線是水平的，y座標相同。P1和P2看起來像:
其中T_x是相機和cx_1=cx_2之間的水平移動，如果設置calib_zero_視差。
Vertical stereo:第一和第二攝像機視圖主要在垂直方向上相對移動(可能也在水平方向上移動了一點)。經過校正的圖像中的極線是垂直的，並且具有相同的x座標。P1和P2看起來像:
其中T_y是相機和cy_1=cy_2之間的垂直位移，如果設置calib_zero_視差。
如您所見，P1和P2的前三列將有效地成爲新的“矯正”相機矩陣。然後可以將這些矩陣與R1和R2一起傳遞給initunt整流映射，以初始化每個相機的整流映射。
請看下面來自stereo_calib.cpp樣本的截圖。一些紅色的水平線通過相應的圖像區域。這意味着圖像得到了很好的矯正，這是大多數立體匹配算法所依賴的。綠色的矩形是roi1和roi2。你可以看到它們的內部都是有效的像素。
45.5立體匹配
立體匹配是根據對所選特徵的計算，建立特徵間的對應關係，將同一個空間點在不同圖像中的映像點對應起來，並由此得到相應的視差圖像，立體匹配是雙目視覺中最重要也是最困難的問題。當空間三維場景被投影爲二維圖像時，同一景物在不同視點下的圖像會有很大不同，而且場景中的諸多因素，如光照條件、景物幾何形狀和物理特性、噪聲干擾和畸變以及攝像機特性等，都被綜合成單一的圖像灰度值。
因此，要準確的對包含了如此之多不利因素的圖像進行無歧義匹配十分困難。 立體匹配的方法主要分爲兩大類，即灰度相關和特徵匹配。灰度相關直接用象素灰度進行匹配，該方法優點是匹配結果不受特徵檢測精度和密度的影響，可以得到很高的定位精度和密集的視差表面;缺點是依賴於圖像灰度統計特性，對景物表面結構以及光照反射較爲敏感，因此在空間景物表面缺乏足夠紋理細節、成像失真較大（如基線長度過大）的場合存在一定困難。
立體匹配是一種從平面圖像中恢復深度信息的技術。由於雙目立體匹配系統通過模擬人眼視覺感知原理，僅需要兩臺數字攝像機安裝在同一水平線上，經過立體矯正就可以投入使用。具有實現簡單、成本低廉並且可以在非接觸條件下測量距離等優點。近年來，隨着社會的科技進步，立體匹配技術的發展日新月異，隨着匹配算法精度與速度的提高，其應用場景進一步擴大。在此背景下，研究立體匹配的意義非凡。
立體匹配作爲三維重建、立體導航、非接觸測距等技術的關鍵步驟，通過匹配兩幅或者多幅圖像來獲取深度信息。並且廣泛應用於工業生產自動化、流水線控制、無人駕駛汽車（測距，導航）、安防監控、遙感圖像分析、機器人智能控制等方面。在機器人制導系統中可以用於導航判斷、目標拾取；在工業自動化控制系統中可用於零部件安裝、質量檢測，環境檢測；在安防監控系統中可用於人流檢測，危害報警。雖然立體匹配應用廣泛，但是還有很多尚未解決的難題，因此該技術成爲了近年來計算機視覺領域廣泛關注的難點和熱點。
在上一節中我們學習了對極約束的基本概念和相關術語。如果同一場景有兩幅圖像的話我們在直覺上就可以獲得圖像的深度信息。如下圖所示:
上圖包含相似三角形。寫出它們的等價方程將得到以下結果:
x 和 x' 分別是圖像中的點到 3D 空間中的點和到攝像機中心的距離。B 是這兩個攝像機之間的距離,f 是攝像機的焦距。上邊的等式告訴我們點的深度與x 和 x' 的差成反比。所以根據這個等式我們就可以得到圖像中所有點的深度圖， 這樣就可以找到兩幅圖像中的匹配點了。前面我們已經知道了對極約束可以使這個操作更快更準，一旦找到了匹配，就可以計算出 disparity 了。
下面的代碼顯示了構建深度圖的簡單過程。
import numpy as np
import cv2
imgL = cv2.imread('tsukuba_l.png',0)
imgR = cv2.imread('tsukuba_r.png',0)
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=15)
disparity = stereo.compute(imgL,imgR)
disp = cv2.normalize(src=disparity, dst=disparity, beta=0, alpha=255,
norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
cv2.imshow('disp',disp)
下圖左側爲原始圖像,右側爲深度圖像。如圖所示,結果中有很大的噪音。 通過調整 numDisparities 和 blockSize 的值,我們會得到更好的結果。
OpenCV中提供了三種立體匹配算法的實現:BM、SGBM、GC。
BM(Block Matching,塊匹配): 速度最快,但效果最差;
SGBM(Semi-Global Block Matching,半全局塊匹配): 匹配效果好,速度比BM稍慢;
GC(Graph Cuts,圖割): 匹配效果最佳,但速度最慢,不適合實時業務,只能在opencv2.x中找到實現。
注意: GC算法只能在C語言模式下運行，並且不能對視差圖進行預先的邊界延拓，左右視圖和左右視差矩陣的大小必須一致。
SGBM算法詳解
SGBM的思路：通過選取每個像素點的disparity，組成一個disparity map，設置一個和disparity map相關的全局能量函數，使這個能量函數最小化，以達到求解每個像素最優disparity的目的。能量函數形式如下：
D指disparity map。E(D)是該disparity map對應的能量函數。
p, q代表圖像中的某個像素。
Np 指像素p的相鄰像素點（一般認爲8連通）。
C(p, Dp)指當前像素點disparity爲Dp時，該像素點的cost。
P1 是一個懲罰係數，它適用於像素p相鄰像素中dsparity值與p的dsparity值相差1的那些像素。
P2 是一個懲罰係數，它適用於像素p相鄰像素中dsparity值與p的dsparity值相差大於1的那些像素。
I[.]函數返回1如果函數中的參數爲真，否則返回0。
利用上述函數在一個二維圖像中尋找最優解是一個NP-complete問題，耗時過於巨大，因此該問題被近似分解爲多個一維問題，即線性問題。而且每個一維問題都可以用動態規劃來解決。因爲1個像素有8個相鄰像素，因此一般分解爲8個一維問題。考慮從左到右這一方向，如下圖所示：
則每個像素的disparity只和其左邊的像素相關，有如下公式：
r指某個指向當前像素p的方向，在此可以理解爲像素p左邊的相鄰像素。
Lr(p, d) 表示沿着當前方向（即從左向右），當目前像素p的disparity取值爲d時，其最小cost值。
這個最小值是從4種可能的候選值中選取的最小值：
1.前一個像素（左相鄰像素）disparity取值爲d時，其最小的cost值。
2.前一個像素（左相鄰像素）disparity取值爲d-1時，其最小的cost值+懲罰係數P1。
3.前一個像素（左相鄰像素）disparity取值爲d+1時，其最小的cost值+懲罰係數P1。
4.前一個像素（左相鄰像素）disparity取值爲其它時，其最小的cost值+懲罰係數P2。
另外，當前像素p的cost值還需要減去前一個像素取不同disparity值時最小的cost。這是因爲Lr(p, d)是會隨着當前像素的右移不停增長的，爲了防止數值溢出，所以要讓它維持在一個較小的數值。
C(p, d)的計算很簡單，由如下兩個公式計算：
即，當前像素p和移動d之後的像素q之間，經過半個像素插值後，尋找兩個像素點灰度或者RGB差值的最小值，作爲C(p, d)的值。具體來說：設像素p的灰度/RGB值爲I(p)，先從I(p)、(I(p)+I(p-1))/2、(I(p)+I(p+1))/2三個值中選擇出和I(q)差值最小的,即d(p,p-d)。然後再從I(q)、(I(q)+I(q-1))/2、(I(q)+I(q+1))/2三個值中選擇出和I(p)差值最小的,即d(p-d,p)。最後從兩個值中選取最小值，就是C(p, d)。
上面是從一個方向（從左至右）計算出的像素在取值爲某一disparity值時的最小cost值。但是一個像素有8個鄰域，所以一共要從8個方向計算（左右，右左，上下，下上，左上右下，右下左上，右上左下，左下右上）這個cost值。然後把八個方向上的cost值累加，選取累加cost值最小的disparity值作爲該像素的最終disparity值。對於每個像素進行該操作後，就形成了整個圖像的disparity map。公式表達如下：
SGBM算法遍歷每個像素，針對每個像素的操作和disparity的範圍有關，故時間複雜度爲：O（w·h·n）。可以使用積分圖（Integral Image）或方框濾波（Box Filtering）的方法使時間複雜度下降到O（w*h）。
代碼實現:
num = 5
blockSize = 13
stereo_sgbm = cv2.StereoSGBM_create(
minDisparity=0, # 最小視差值(int類型)，通常情況下爲0。此參數決定左圖中的像素點在右圖匹配搜索的起點。最小視差值越小，視差圖右側的黑色區域越大
numDisparities=16 * num, # 視差搜索範圍，其值必須爲16的整數倍且大於0。視差窗口越大，視差圖左側的黑色區域越大
blockSize=blockSize, # 匹配塊大小（SADWindowSize(SAD代價計算的窗口大小)）,大於1的奇數。默認爲5,一般在3~11之間
P1=8 * 3 * blockSize * blockSize, # P1是相鄰像素點視差增/減 1 時的懲罰係數；需要指出，在動態規劃時，P1和P2都是常數。一般：P1=8*通道數*blockSize*blockSize，P2=4*P1
P2=32 * 3 * blockSize * blockSize, # P2是相鄰像素點視差變化值大於1時的懲罰係數。P2必須大於P1。p2值越大，差異越平滑
disp12MaxDiff=20, # 左右視差圖的最大容許差異（左右一致性檢測,超過將被清零），默認爲-1，即不執行左右視差檢查。
preFilterCap=15, # 圖像預處理參數，水平sobel預處理後，映射濾波器大小。默認爲15
uniquenessRatio=0, # 視差唯一性檢測百分比，視差窗口範圍內最低代價是次低代價的(1+uniquenessRatio/100)倍時，最低代價對應的視差值纔是該像素點的視差，否則該像素點的視差爲 0，通常爲5~15.
speckleWindowSize=100, # 視差連通區域像素點個數的大小。若大於，視差值認爲有效，否則認爲當前視差值是噪點。將其設置爲0可禁用斑點過濾。否則，將其設置在50-200的範圍內。
speckleRange=2, # 視差變化閾值，每個連接組件內的最大視差變化。如果你做斑點過濾，將參數設置爲正值，它將被隱式乘以16.通常，1或2就足夠好了
mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY # 模式,取值0,1,2,3。默認被設置爲false。
)
# stereo_sgbm_right_matcher = cv2.ximgproc.createRightMatcher(stereo_sgbm)
# 計算視差: 根據SGBM方法生成差異圖
disparity_left = stereo_sgbm.compute(gray_L, gray_R)
SGBM的參數說明:
立體匹配算法的步驟
總體來講包含以下6個步驟：
1.預處理( GaussBlur , SobelX, ...etc)
2.代價計算 ( AD, SAD, SSD, BT, NCC, Census, ...etc)
3.代價聚合 ( Boxfilter, CBCA, WMF, MST, ...etc)
4.代價優化 ( BP, GC, HBP, CSBP, doubleBP, ...)
5.視差計算 ( WTA)
6.後處理 ( MedianFilter, WeightMedianFilter, LR-check, ...etc)
一般情況下，組合12356稱爲局部立體匹配算法，12456稱爲全局立體匹配算法，區別在於是否構建全局能量優化函數。
1）匹配代價計算（Cost Computation）
計算匹配代價即計算參考圖像上的每個像素點IR(P)，以所有視差可能性去匹配目標圖像上對應點IT(pd)的代價值，因此計算得到的代價值可以存儲在一個h*w*d(MAX)的三維數組中，通常稱這個三維數組爲視差空間圖（Disparity Space Image，DSI）。匹配代價是立體匹配的基礎，設計抗噪聲干擾、對光照變化不敏感的匹配代價能提高立體匹配的精度。因此，匹配代價的設計在全局算法和局部算法中都是研究的重點。
2）代價聚合（Cost Aggregation）
通常全局算法不需要代價聚合，而局部算法需要通過求和、求均值或其它方法對一個支持窗口內的匹配代價進行聚合而得到參考圖像上一點p在視差d處的累積代價CA(p,d)，這一過程稱爲代價聚合。通過匹配代價聚合，可以降低異常點的影響，提高信噪比（SNR,Signal Noise Ratio）進而提高匹配精度。代價聚合策略通常是局部匹配算法的核心，策略的好壞直接關係到最終視差圖（Disparity maps）的質量。
3）視差計算（Disparity Computation）
局部立體匹配算法的思想，在支持窗口內聚合完匹配代價後，獲取視差的過程就比較簡單，只需在一定範圍內選取匹配代價聚合最優的點（SAD和SSD取最小值，NCC取最大值）作爲對應匹配點。通常採用‘勝者爲王’策略（WTA，Winner Take All），即在視差搜索範圍內選擇累積代價最優的點作爲對應匹配點，與之對應的視差即爲所求的視差。
NCC (Normalized Cross correlation，歸一化互相關):
NCC(u,v) = [(wl - w)/(|wl - w|)]*[(wr - w)/(|wr - w|)]
SSD (Sum of Squared Defferences，差值平方和):
SSD(u,v) = Sum{[Left(u,v) - Right(u,v)] * [Left(u,v) - Right(u,v)]}
SAD (Sum of Absolute Defferences，絕對差值和) :
SAD(u,v) = Sum{|Left(u,v) - Right(u,v)|}
STAD (Sum of Truncated Absolute Differences，截斷絕對差值和):
SAD算法的基本流程：
1.構造一個小窗口，類似與卷積核。
2.用窗口覆蓋左邊的圖像，選擇出窗口覆蓋區域內的所有像素點。
3.同樣用窗口覆蓋右邊的圖像並選擇出覆蓋區域的像素點。
4.左邊覆蓋區域減去右邊覆蓋區域，並求出所有像素點差的絕對值的和。
5.移動右邊圖像的窗口，重複3，4的動作。（這裏有個搜索範圍，超過這個範圍跳出）
6.找到這個範圍內SAD值最小的窗口，即找到了左邊圖像的最佳匹配的像素塊。
4）後處理（Post Process）
一般的，分別以左右兩圖爲參考圖像，完成上述三個步驟後可以得到左右兩幅視差圖。但所得的視差圖還存在一些問題，如遮擋點視差不準確、噪聲點、誤匹配點等存在，因此還需要對視差圖進行優化，採用進一步執行後處理步驟對視差圖進行修正。常用的方法有插值（Interpolation）、亞像素增強（Subpixel Enhancement）、精細化（Refinement）、圖像濾波（Image Filtering）、圖像分割等操作。
立體匹配的後續處理：左右檢測+遮擋填充+中值濾波
立體匹配算法，尤其是針對CPU實時處理的高度優化算法，在具有挑戰性的序列上往往會出現相當多的錯誤。這些誤差通常集中在均勻的無紋理區域、半遮擋區域和近深度不連續區域。處理立體匹配錯誤的一種方法是使用各種技術來檢測潛在的不準確的視差值並使其失效，從而使視差圖成爲半稀疏的。在StereoBM和StereoSGBM算法中已經實現了一些這樣的技術。另一種方法是使用某種過濾程序將視差圖的邊緣與源圖像的邊緣對齊，並將視差值從高可信區域傳播到低可信區域(如半遮擋)。最近在邊緣感知過濾方面的進展使得在CPU實時處理的約束下能夠執行這種後過濾。
後處理(post-processing)措施中最常用的是左右一致性檢測(Left-Right Consistency (LRC) check)。左右檢測對實驗效果的提升是很顯著的，無論是視差圖的視覺效果還是數據精度。很多時候LRC都是論文的遮羞布，在論文主體部分優勢不明顯的情況下，通過LRC依然能得到過得去的結果，從而掩蓋了核心算法的孱弱。
左右一致性檢測的作用是實現遮擋檢測，得到左圖對應的遮擋圖像。具體做法：
根據左右兩幅輸入圖像，分別得到左右兩幅視差圖。對於左圖中的一個點p，求得的視差值是d1，那麼p在右圖裏的對應點應該是(p-d1)，(p-d1)的視差值記作d2。若|d1-d2|>threshold，p標記爲遮擋點(occluded point)。
遮擋（Occlusion）是隻出現在一幅圖像而在另一幅圖中看不到的那些點。在立體匹配算法中如果不針對遮擋區域做一些特殊處理是不可能通過單幅圖提供的有限信息得到遮擋點的正確視差。遮擋點通常是一塊連續的區域，記作occluded region/area。
如下依次是:左圖，左圖對應的二值遮擋圖，左視差圖，右視差圖。
得到二值遮擋圖之後是爲所有黑色的遮擋點賦予合理的視差值。對於左圖而言，遮擋點一般存在於背景區域和前景區域接觸的地方。遮擋的產生正是因爲前景比背景的偏移量更大，從而將背景遮蓋。具體賦值方法是：對於一個遮擋點p，分別水平往左和往右找到第一個非遮擋點，記作pl、pr。點p的視差值賦成pl與pr的視差值中較小的那一個。d(p)= min (d(pl),d(pr)) (被遮擋的像素具有背景的深度)。
這種簡單的遮擋填充方法在遮擋區域賦值方面效果顯著，但是對初始視差的合理性和精度依賴較高。而且會出現類似於動態規劃算法的水平條紋，所以其後常常跟着一個中值濾波（Median Filtering）步驟以消除條紋。
下圖是中值濾波的結果:
這樣，通過LRC check檢測出遮擋點，對其進行視差估計，再對整幅圖做中值濾波，得到的結果就好多了。[官網示例]
[代碼]
num = 5
blockSize = 13
stereo_sgbm = cv2.StereoSGBM_create(
minDisparity=0, numDisparities=16 * num, blockSize=blockSize,
P1=8 * 3 * blockSize * blockSize, P2=32 * 3 * blockSize * blockSize, disp12MaxDiff=20,
preFilterCap=15, uniquenessRatio=0, speckleWindowSize=100, speckleRange=2,
mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)
# 新版opencv中沒有ximgproc模塊包(pip install opencv-contrib-python==3.4.2.16)
stereo_sgbm_right_matcher = cv2.ximgproc.createRightMatcher(stereo_sgbm)
# DisparityWLSFilter:基於加權最小二乘濾波的視差圖濾波(以快速全局平滑的形式，比傳統的加權最小二乘濾波實現快得多)，並可選地使用基於左右一致性的置信度來細化半遮擋和均勻區域的結果
wls_filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left=stereo_sgbm)
# Lambda是一個定義過濾期間正則化數量的參數。較大的值會迫使經過過濾的視差圖邊緣與源圖像邊緣粘連得更緊。典型值爲8000。
wls_filter.setLambda(8000)
# SigmaColor定義了濾波過程對源圖像邊緣的敏感度。較大的值會通過低對比度邊緣導致視差泄漏。較小的值會使過濾器對源圖像中的噪聲和紋理過於敏感。典型的值範圍從0.8到2.0。
wls_filter.setSigmaColor(1.2)
# 將圖片置爲灰度圖
gray_L = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
gray_R = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY)
# 計算視差: 根據SGBM方法生成差異圖
disparity_left = stereo_sgbm.compute(gray_L, gray_R)
disparity_right = stereo_sgbm_right_matcher.compute(gray_R,gray_L)
disparity_left = np.int16(disparity_left)
disparity_right = np.int16(disparity_right)
# 視差圖後處理
disparity_filtered = wls_filter.filter(disparity_left, gray_L, None, disparity_right)
# 將圖片擴展至3d空間中，其z方向的值則爲當前的距離;
# 通過reprojectImageTo3D這個函數將視差矩陣轉換成實際的物理座標矩陣,獲取世界座標
threeD = cv2.reprojectImageTo3D(disparity_filtered.astype(np.float32) / 16.0, Q)*2.8346
# threeD = cv2.reprojectImageTo3D(disparity_left.astype(np.float32) / 16.0, Q)*2.8346
# 獲取深度圖
disp_filtered = cv2.normalize(src=disparity_filtered, dst=disparity_filtered, alpha=255,
beta=0, norm_type=cv2.NORM_MINMAX)
disp_filtered = np.uint8(disp_filtered)
cv2.imshow("depth", disp_filtered)
立體匹配綜合論文集 : http://download.csdn.net/detail/wangyaninglm/9591251
基於圖像分割的立體匹配論文合集 : http://download.csdn.net/detail/wangyaninglm/9591253
並行立體匹配論文合集 : http://download.csdn.net/detail/wangyaninglm/9591255
基於置信傳播的立體匹配論文合集 : http://download.csdn.net/detail/wangyaninglm/9591256
基於稠密匹配的論文合集： http://download.csdn.net/detail/wangyaninglm/9591259
使用applyColorMap（僞彩色函數）給深度圖上色
applyColorMap(src, colormap, dst=None):在給定的圖像上(可以是灰度圖像,單通道或3通道圖像)應用GNU Octave/MATLAB的等效顏色映射，產生僞彩色圖像。
OpenCV中定義了20種colormap（色度圖），經常用僞彩色將高度、壓力、密度、溫度、溼度等圖像更好地顯示。參數colormap取值如下所示:
# disp_color=cv2.applyColorMap(cv2.convertScaleAbs(disp_filtered,alpha=1),cv2.COLORMAP_JET)
disp_color=cv2.applyColorMap(disp_filtered,cv2.COLORMAP_JET)
cv2.imshow("depth", disp_color)
效果如下(左:上色前; 右:上色後):
45.6雙目測距
距離公式: Z = f * T / D
f爲左攝像頭在x方向的焦距,單位是像素; T(雙目標定的平移向量Tx的絕對值)爲左右相機基線長度; D爲同一個點在左右兩圖像中的視差。D=xl-xr,單位也是像素。
(代碼部分不做分享,有問題儘管留言)!!!
因篇幅過長,剩餘部分參見:OpenCV-Python (官方)中文教程(部分四)
