學習OpenCV雙目測距原理及常見問題解答
轉自博客：https://blog.csdn.net/angle_cal/article/details/50800775
一. 整體思路和問題轉化.
圖1. 雙攝像頭模型俯視圖
圖1解釋了雙攝像頭測距的原理，書中Z的公式如下： 
在OpenCV中，f的量綱是像素點，T的量綱由定標棋盤格的實際尺寸和用戶輸入值確定，一般總是設成毫米，當然為了精度提高也可以設置為0.1毫米量級，d=xl-xr的量綱也是像素點。因此分子分母約去，z的量綱與T相同 
圖2, 雙攝像頭模型立體視圖
圖2解釋了雙攝像頭獲取空間中某點三維坐標的原理。
注意,左鏡頭光心是世界坐標系的原點,該坐標系是左手屏幕坐標系,所以在該點上方,前方的物體,他的世界坐標中,Y是負數. 
可以看到，實際的坐標計算利用的都是相似三角形的原理，其表達式就如同Q矩陣所示。空間中某點的三維坐標就是(X/W, Y/W, Z/W)。所以,得到了Q矩陣,就是得到了一個點的世界坐標.
因此，為了精確地求得某個點在三維空間里的距離，我們需要獲得的參數有焦距f、視差d、攝像頭中心距Tx。如果還需要獲得X坐標和Y坐標的話，那么還需要額外知道左右像平面的坐標系與立體坐標系中原點的偏移cx和cy。
要測距,就又有這五個參數.
f, Tx, cx和cy可以通過立體標定獲得初始值，並通過立體校准優化，使得兩個攝像頭在數學上完全平行放置，並且左右攝像頭的cx, cy和f相同(也就是實現下圖中左右視圖完全平行對准的理想形式)。 
立體匹配所做的工作，就是在之前的基礎上，求取最后一個變量：視差d(這個d一般需要達到亞像素精度)。從而最終完成求一個點三維坐標所需要的准備工作。
總結:使用OpenCV進行的立體視覺測距,其實思路是這樣的:根據攝像頭的成像原理(表現為小孔成像等效模型)和具體設備的成像特性(表現為光心距離,兩個焦距,兩個鏡頭安裝的相對位置),設法建立像和真實世界的映射關系(表現為上述五個參數),並以攝像機系統為參考系,為真實世界建立絕對坐標系(該坐標系的具體形式請看下文),通過上述映射關系,該方法能夠把像點所在的世界坐標位置求解出來,實現距離測量.
在清楚了上述原理之后，我們也就知道了，所有的這幾步：標定、校准和匹配，都是圍繞着如何更精確地獲得f, d, Tx, cx和cy而設計的。
二. OpenCV2.4.11的實現.
按照上述解釋,步驟是這樣的:
1.准備工作:依次完成:標定->校准->匹配這三個操作,獲得所需的參數:
T_x,f,d, C_x, C_y.其中,標定和校准的目的是求解Q矩陣,匹配的目的是求解視差值d. 
2. 通過某種手段,獲得待測點在”像世界”中的坐標(x.y),構造向量: 
3. 通過Q•W運算,得到世界坐標向量,進而計算距離.
4. 具體實現步驟:
(1)標定和矯正
函數 findChessboardCorners()以標定盤圖片和標定盤參數為輸入,得到立體標定的輸入參數;
函數:stereoCalibrate()輸出變量:
R – Output rotation matrix (旋轉)
T – Output translation vector(平移)
E – Output essential matrix.(本征)
F – Output fundamental matrix.(基礎)
函數:stereoRectify()的輸入變量已經從上面得到,他的輸出變量如下:
R1 – Output 3x3 rectification transform (rotation matrix) for the first camera.
R2 – Output 3x3 rectification transform (rotation matrix) for the second camera.
P1 – Output 3x4 projection matrix in the new (rectified) coordinate systems for the first camera.
P2 – Output 3x4 projection matrix in the new (rectified) coordinate systems for the second camera.
Q – Output disparity-to-depth mapping matrix
(2)匹配
OpenCV2.4.11中提供了若干種匹配方法,這里采用的是BM,SGBM,VAR算法,這三個算法在使用的時候都需要構造相應的對象,對其中的輸入屬性進行設置,利用其提供的”()”操作符的重載執行相關的算法.他們都要求輸入左右視圖,輸出視差圖,具體情況如下:
SGBM: left – Left 8-bit single-channel or 3-channel image.
right – Right image of the same size and the same type as the left one.
disp – Output disparity map. It is a 16-bit signed single-channel image of the same size as the input image. It contains disparity values scaled by 16. So, to get the floating-point disparity map, you need to divide each disp element by 16.
BM: left – Left 8-bit single-channel image.
right – Right image of the same size and the same type as the left one.
disparity – Output disparity map. It has the same size as the input images. When disptype==CV_16S, the map is a 16-bit signed single-channel image, containing disparity values scaled by 16. To get the true disparity values from such fixed-point representation, you will need to divide each disp element by 16. If disptype==CV_32F, the disparity map will already contain the real disparity values on output.
disptype – Type of the output disparity map, CV_16S (default) or CV_32F.
Var: left – Left 8-bit single-channel or 3-channel image.
right – Right image of the same size and the same type as the left one.
disp – Output disparity map. It is a 8-bit signed single-channel image of the same size as the input image.
輸出統一以8位視差圖計算,不是8位轉換成8位.全部采用CV_16S制式.
(3)取得待測點的圖像空間坐標值
這一步很簡單,只需要設法獲取圖片上的某個點的坐標就可以,此坐標只是相對於圖片本身而言,所以可以使用例如鼠標點擊或輸入坐標的方法
(4)已經得到了全部的數據,利用Q矩陣進行計算就可以了。
三.代碼中的關鍵函數:
cvInitUndistortRectifyMap()函數是開源視覺庫OpenCV的庫函數。該函數是以C語言為基礎編寫的，而initUndistortRectifyMap()以C++語言為基礎編寫的。
OpenCV三種立體匹配求視差圖算法總結:
首先我們看一下BM算法：
Ptr<StereoBM> bm = StereoBM::create(16,9);//局部的BM;
// bm算法
bm->setROI1(roi1);//左右視圖的有效像素區域 在有效視圖之外的視差值被消零
bm->setROI2(roi2);
bm->setPreFilterType(CV_STEREO_BM_XSOBEL);
bm->setPreFilterSize(9);//濾波器尺寸 [5,255]奇數
bm->setPreFilterCap(31);//預處理濾波器的截斷值 [1-31]
bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 15);//sad窗口大小
bm->setMinDisparity(0);//最小視差值，代表了匹配搜索從哪里開始
bm->setNumDisparities(numberOfDisparities);//表示最大搜索視差數
bm->setTextureThreshold(10);//低紋理區域的判斷閾值 x方向導數絕對值之和小於閾值
bm->setUniquenessRatio(15);//視差唯一性百分比 匹配功能函數
bm->setSpeckleWindowSize(100);//檢查視差連通域 變化度的窗口大小
bm->setSpeckleRange(32);//視差變化閾值 當窗口內視差變化大於閾值時，該窗口內的視差清零
bm->setDisp12MaxDiff(-1);// 1
該方法速度最快，一副320*240的灰度圖匹配時間為31ms
第二種方法是SGBM方法這是OpenCV的一種新算法：
Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);//全局的SGBM;
// sgbm算法
sgbm->setPreFilterCap(63);//預處理濾波器的截斷值 [1-63]
int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
sgbm->setBlockSize(sgbmWinSize);
int cn = img0.channels();
sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize);// 控制視差變化平滑性的參數。P1、P2的值越大，視差越平滑。
//P1是相鄰像素點視差增/減 1 時的懲罰系數；P2是相鄰像素點視差變化值大於1時的懲罰系數。P2必須大於P1
sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);//最小視差值，代表了匹配搜索從哪里開始
sgbm->setNumDisparities(numberOfDisparities);//表示最大搜索視差數
sgbm->setUniquenessRatio(10);//表示匹配功能函數
sgbm->setSpeckleWindowSize(100);//檢查視差連通域 變化度的窗口大小
sgbm->setSpeckleRange(32);//視差變化閾值 當窗口內視差變化大於閾值時，該窗口內的視差清零
sgbm->setDisp12MaxDiff(-1);// 1
//左視圖差（直接計算）和右視圖差（cvValidateDisparity計算得出）之間的最大允許差異
if(alg==STEREO_HH) sgbm->setMode(StereoSGBM::MODE_HH);
else if(alg==STEREO_SGBM) sgbm->setMode(StereoSGBM::MODE_SGBM);
else if(alg==STEREO_3WAY) sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);
各參數設置如BM方法，速度比較快，320*240的灰度圖匹配時間為78ms，
第三種為GC方法：
該方法速度超慢，但效果超好。
四.FAQ:
Q1：標定時棋盤格的大小如何設定，對最后結果有沒有影響？
A：當然有。在標定時，需要指定一個棋盤方格的長度，這個長度(一般以毫米為單位，如果需要更精確可以設為0.1毫米量級)與實際長度相同，標定得出的結果才能用於實際距離測量。一般如果尺寸設定准確的話，通過立體標定得出的Translation的向量的第一個分量Tx的絕對值就是左右攝像頭的中心距。一般可以用這個來驗證立體標定的准確度。比如我設定的棋盤格大小為270 (27mm)，最終得出的Tx大小就是602.8 (60.28mm)，相當精確。
Q2：通過立體標定得出的Tx符號為什么是負的？
A：這個其實我也不是很清楚。個人的解釋是，立體標定得出的T向量指向是從右攝像頭指向左攝像頭(也就是Tx為負)，而在OpenCV坐標系中，坐標的原點是在左攝像頭的。因此，用作校准的時候，要把這個向量的三個分量符號都要換一下，最后求出的距離才會是正的。
但是這里還有一個問題，就是Learning OpenCV中Q的表達式，第四行第三列元素是-1/Tx，而在具體實踐中，求出來的實際值是1/Tx。這里我和maxwellsdemon討論下來的結果是，估計書上Q表達式里的這個負號就是為了抵消T向量的反方向所設的，但在實際寫OpenCV代碼的過程中，那位朋友卻沒有把這個負號加進去。(一家之言，求更詳細的解釋)
Q3：cvFindStereoCorrespondenceBM的視差輸出結果單位是？
A：在OpenCV中，BM函數得出的結果是以16位符號數的形式的存儲的，出於精度需要，所有的視差在輸出時都擴大了16倍(2^4)。其具體代碼表示如下：
dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);
可以看到，原始視差在左移8位(256)並且加上一個修正值之后又右移了4位，最終的結果就是左移4位
因此，在實際求距離時，cvReprojectTo3D出來的X/W,Y/W,Z/W都要乘以16 (也就是W除以16)，才能得到正確的三維坐標信息
Q4：利用雙攝像頭進行測距的時候世界坐標的原點究竟在哪里？
A：世界坐標系的原點是左攝像頭凸透鏡的光心。
說起這個，就不得不提到針孔模型。如圖3所示，針孔模型是凸透鏡成像的一種簡化模型。當物距足夠遠時(遠大於兩倍焦距)，凸透鏡成像可以看作是在焦距處的小孔成像。
在實際計算過程中，為了計算方便，我們將像平面翻轉平移到針孔前，從而得到一種數學上更為簡單的等價形式（方便相似三角形的計算），如下圖所示。
因此，對應圖2就可以知道，世界坐標系原點就是左攝像頭針孔模型的針孔，也就是左攝像頭凸透鏡的光心
Q5：f和d的單位是像素，那這個像素到底表示什么，它與毫米之間又是怎樣換算的？
A：這個問題也與針孔模型相關。在針孔模型中，光線穿過針孔（也就是凸透鏡中心）在焦距處上成像，因此，圖3的像平面就是攝像頭的CCD傳感器的表面。每個CCD傳感器都有一定的尺寸，也有一定的分辨率，這個就確定了毫米與像素點之間的轉換關系。舉個例子，CCD的尺寸是8mm X 6mm，分辨率是640X480，那么毫米與像素點之間的轉換關系就是80pixel/mm。
在實際運用中，我們在數學上將這個像平面等效到小孔前（圖4），這樣就相當於將在透鏡中心點之前假設了一塊虛擬的CCD傳感器。
Q6：為什么cvStereoRectify求出的Q矩陣cx, cy, f都與原來的不同？
A：這個在前文有提到過。在實際測量中，由於攝像頭擺放的關系，左右攝像頭的f, cx, cy都是不相同的。而為了使左右視圖達到完全平行對准的理想形式從而達到數學上運算的方便，立體 校准所做的工作事實上就是在左右像重合區域最大的情況下，讓兩個攝像頭光軸的前向平行，並且讓左右攝像頭的f, cx, cy相同。因此，Q矩陣中的值與兩個instrinsic矩陣的值不一樣就可以理解了。
相關網址：
opencv雙目測距實現：https://cloud.tencent.com/developer/article/1082064
initUndistortRectifyMap：http://www.voidcn.com/article/p-qyftgzxl-brg.html
真實場景的雙目立體匹配（Stereo Matching）獲取深度圖詳解——最終得到深度圖像並且填充：https://www.cnblogs.com/riddick/p/8486223.html
OpenCV畸變校正原理以及損失有效像素原理分析：https://www.cnblogs.com/riddick/p/6711263.html
github雙目相關理論與實踐：https://github.com/melodybinbin/MVision/tree/master/stereo