本文采用MATLAB标定工具箱和OpenCV3.10來實作雙目測距，裝置為兩個CMOS工業相機和相應的雙目雲台。
首先感謝CSDN上兩位大神前輩鄒宇華和scyscyao，雖然是六年前的部落格，OpenCV也從1.0的版本更新到了3.1版本，但部落格對機器視覺初學者來說仍然提供了巨大的幫助。本文主要參考了 OpenCV學習筆記（17）雙目測距與三維重建的OpenCV實作問題集錦（二）雙目定标與雙目校正、OpenCV學習筆記（18）雙目測距與三維重建的OpenCV實作問題集錦（三）立體比對與視差計算、分享一些OpenCV實作立體視覺的經驗、雙攝像頭測距的OpenCV實作、學習筆記：使用opencv做雙目測距（相機标定+立體比對+測距）.、OPENCV3.0 雙目立體标定等文章，并摘錄了部分内容。
由于初學機器視覺，有些問題尚不是很清楚，文中不免存在錯誤，還請觀者指正。
一、雙目标定
雙目攝像頭标定不僅要得出每個攝像頭的内部參數，還需要通過标定來測量兩個攝像頭之間的相對位置（即右攝像頭相對于左攝像頭的平移向量 t和旋轉矩陣R）。
由于OpenCV中StereoCalibrate标定的結果極其不穩定，甚至會得到很誇張的結果，是以最後還是決定Matlab标定工具箱立體标定，再将标定的結果讀入OpenCV，來進行後續圖像校準和比對。
首先對左右攝像頭分别進行标定，得到兩個攝像頭各自的内參矩陣和畸變參數向量。下圖是單目标定的結果示意圖和反投影誤差分析圖。
左右攝像頭都标定完成之後，就可以利用單目标定得到的兩個攝像頭的參數進行立體标定。下圖是雙目标定的結果示意圖。
注意事項：
1. 采集棋盤圖的時候要注意，盡量讓棋盤占據盡可能多的畫面，這樣可以得到更多有關攝像頭畸變方面的資訊。
2. 在标定工具箱中，可以通過reproject on images，得到根據目前标定結果得到的反投影誤差，從點雲的聚集情況和分散的最大範圍可以看出反投影誤差的大小。Recomp. corners選項，主要完成根據反向投影得到的角點坐标重作為對角點的估計，重新計算角點的功能。針對第一次标定結果誤差太大的情況，可以通過此方法重新計算焦點。計算完成後，點選Calibration根據新的焦點進行标定。此時，得到的标定資訊，比第一次得到的反投影誤差分布更集中，直徑也小。（該步驟在标定過程中需謹慎使用，因為往往首次得到的三維坐标精确度并不高，如果參考誤差較大的話，有可能使結果與正解偏差更大。）
3. 兩個攝像頭的焦距應該保持一緻，因為在後續的視差圖轉換為三維圖時的Q矩陣隻有一個f值。是以必須要求至少焦距相近。而且立體成像的三角測量（Learning OpenCV書中提到）的前提假設就是fl=fr。（調整兩個攝像頭的焦距相同的方法：離兩個相機相同遠處放置标定闆，分别調節兩個相機的焦距，使得兩個畫面的清晰度相似。）
二、雙目校正
要計算目标點在左右兩個視圖上形成的視差，首先要把該點在左右視圖上兩個對應的像點比對起來。然而，在二維空間上比對對應點是非常耗時的，為了減少比對搜尋範圍，我們可以利用極線限制使得對應點的比對由二維搜尋降為一維搜尋。而雙目校正的作用就是要把消除畸變後的兩幅圖像嚴格地行對應，使得兩幅圖像的對極線恰好在同一水準線上，這樣一幅圖像上任意一點與其在另一幅圖像上的對應點就必然具有相同的行号，隻需在該行進行一維搜尋即可比對到對應點。
經過雙目标定得到攝像頭的各項參數後，采用OpenCV中的stereoRectify得到校正旋轉矩陣R、投影矩陣P、重投影矩陣Q，再采用initUndistortRectifyMap函數得出校準映射參數，然後用remap來校準輸入的左右圖像。其中remap的圖像剪裁系數alpha，取值範圍是-1、0~1。當取值為 0 時，OpenCV會對校正後的圖像進行縮放和平移，使得remap圖像隻顯示有效像素（即去除不規則的邊角區域），适用于機器人避障導航等應用；當alpha取值為1時，remap圖像将顯示所有原圖像中包含的像素，該取值适用于畸變系數極少的高端攝像頭；alpha取值在0-1之間時，OpenCV按對應比例保留原圖像的邊角區域像素。Alpha取值為-1時，OpenCV自動進行縮放和平移。
二、立體比對
采用Block Matching算法進行立體比對，Block Matching用的是SAD方法，速度比較快，但效果一般。
參數設定：
MinDisparity設定為0，因為兩個攝像頭是前向平行放置，相同的物體在左圖中一定比在右圖中偏右。如果為了追求更大的雙目重合區域而将兩個攝像頭向内偏轉的話，這個參數是需要考慮的。
UniquenessRatio主要可以防止誤比對，此參數對于最後的比對結果是有很大的影響。立體比對中，甯願區域無法比對，也不要誤比對。如果有誤比對的話，碰到障礙檢測這種應用，就會很麻煩。該參數不能為負值，一般5-15左右的值比較合适，int型。
BlockSize：SAD視窗大小，容許範圍是[5,255]，一般應該在 5x5..21x21 之間，參數必須為奇數值, int型。
NumDisparities：視差視窗，即最大視內插補點與最小視內插補點之差,視窗大小必須是 16的整數倍，int型。
在BM算法的參數中，對視差生成效果影響較大的主要參數是BlockSize、NumDisparities和UniquenessRatio三個，一般隻需對這三個參數進行調整，其餘參數按預設設定即可。
雙目測距的原理如下圖所示。
BM算法計算出的視差disp是CV_16S格式，通過disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.))變換才能得到真實的視內插補點。
然後通過reprojectImageTo3D這個函數将視差矩陣轉換成實際的實體坐标矩陣。在實際求距離時，reprojectImageTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三維坐标資訊。
三個物體分别放置在距離攝像頭40cm，50cm和70cm處，通過雙目測距測出來的距離基本能接受。
鑒于最近咨詢的朋友較多，我也是OpenCV新手，很多問題也還沒太弄明白，在此将我的代碼（不包括相機标定部分，我用MATLAB标定的）粘在這裡，希望能對大家有所幫助。我的程式設計能力較弱，請輕拍。
/******************************/
/* 立體比對和測距 */
/******************************/
#include <opencv2/opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;
const int imageWidth = ; //攝像頭的分辨率
const int imageHeight = ;
Size imageSize = Size(imageWidth, imageHeight);
Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;
Rect validROIL;//圖像校正之後，會對圖像進行裁剪，這裡的validROI就是指裁剪之後的區域
Rect validROIR;
Mat mapLx, mapLy, mapRx, mapRy; //映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋轉矩陣R，投影矩陣P 重投影矩陣Q
Mat xyz; //三維坐标
Point origin; //滑鼠按下的起始點
Rect selection; //定義矩形選框
bool selectObject = false; //是否選擇對象
int blockSize = , uniquenessRatio =, numDisparities=;
Ptr<StereoBM> bm = StereoBM::create(, );
/*
事先标定好的相機的參數
fx 0 cx
0 fy cy
0 0 1
*/
Mat cameraMatrixL = (Mat_<double>(, ) << , , ,
, , ,
, , );
Mat distCoeffL = (Mat_<double>(, ) << -, , , -, );
Mat cameraMatrixR = (Mat_<double>(, ) << , , ,
, , ,
, , );
Mat distCoeffR = (Mat_<double>(, ) << -, , , -, );
Mat T = (Mat_<double>(, ) << -, , -);//T平移向量
Mat rec = (Mat_<double>(, ) << -, -, );//rec旋轉向量
Mat R;//R 旋轉矩陣
/*****立體比對*****/
void stereo_match(int,void*)
{
bm->setBlockSize(*blockSize+); //SAD視窗大小，5~21之間為宜
bm->setROI1(validROIL);
bm->setROI2(validROIR);
bm->setPreFilterCap();
bm->setMinDisparity(); //最小視差，預設值為0, 可以是負值，int型
bm->setNumDisparities(numDisparities*+);//視差視窗，即最大視內插補點與最小視內插補點之差,視窗大小必須是16的整數倍，int型
bm->setTextureThreshold();
bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止誤比對
bm->setSpeckleWindowSize();
bm->setSpeckleRange();
bm->setDisp12MaxDiff(-);
Mat disp, disp8;
bm->compute(rectifyImageL, rectifyImageR, disp);//輸入圖像必須為灰階圖
disp.convertTo(disp8, CV_8U, / ((numDisparities * + )*));//計算出的視差是CV_16S格式
reprojectImageTo3D(disp, xyz, Q, true); //在實際求距離時，ReprojectTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三維坐标資訊。
xyz = xyz * ;
imshow("disparity", disp8);
}
/*****描述：滑鼠操作回調*****/
static void onMouse(int event, int x, int y, int, void*)
{
if (selectObject)
{
selection.x = MIN(x, origin.x);
selection.y = MIN(y, origin.y);
selection.width = std::abs(x - origin.x);
selection.height = std::abs(y - origin.y);
}
switch (event)
{
case EVENT_LBUTTONDOWN: //滑鼠左按鈕按下的事件
origin = Point(x, y);
selection = Rect(x, y, , );
selectObject = true;
cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
break;
case EVENT_LBUTTONUP: //滑鼠左按鈕釋放的事件
selectObject = false;
if (selection.width > && selection.height > )
break;
}
}
/*****主函數*****/
int main()
{
/*
立體校正
*/
Rodrigues(rec, R); //Rodrigues變換
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
, imageSize, &validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
/*
讀取圖檔
*/
rgbImageL = imread("left1.jpg", CV_LOAD_IMAGE_COLOR);
cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
rgbImageR = imread("right1.jpg", CV_LOAD_IMAGE_COLOR);
cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);
imshow("ImageL Before Rectify", grayImageL);
imshow("ImageR Before Rectify", grayImageR);
/*
經過remap之後，左右相機的圖像已經共面并且行對準了
*/
remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
/*
把校正結果顯示出來
*/
Mat rgbRectifyImageL, rgbRectifyImageR;
cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR); //僞彩色圖
cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);
//單獨顯示
//rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
//rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
imshow("ImageL After Rectify", rgbRectifyImageL);
imshow("ImageR After Rectify", rgbRectifyImageR);
//顯示在同一張圖上
Mat canvas;
double sf;
int w, h;
sf = / MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width * sf);
h = cvRound(imageSize.height * sf);
canvas.create(h, w * , CV_8UC3); //注意通道
//左圖像畫到畫布上
Mat canvasPart = canvas(Rect(w * , , w, h)); //得到畫布的一部分
resize(rgbRectifyImageL, canvasPart, canvasPart.size(), , , INTER_AREA); //把圖像縮放到跟canvasPart一樣大小
Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf), //獲得被截取的區域
cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
//rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8); //畫上一個矩形
cout << "Painted ImageL" << endl;
//右圖像畫到畫布上
canvasPart = canvas(Rect(w, , w, h)); //獲得畫布的另一部分
resize(rgbRectifyImageR, canvasPart, canvasPart.size(), , , INTER_LINEAR);
Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
//rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
cout << "Painted ImageR" << endl;
//畫上對應的線條
for (int i = ; i < canvas.rows; i += )
line(canvas, Point(, i), Point(canvas.cols, i), Scalar(, , ), , );
imshow("rectified", canvas);
/*
立體比對
*/
namedWindow("disparity", CV_WINDOW_AUTOSIZE);
// 建立SAD視窗 Trackbar
createTrackbar("BlockSize:\n", "disparity",&blockSize, , stereo_match);
// 建立視差唯一性百分比視窗 Trackbar
createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, , stereo_match);
// 建立視差視窗 Trackbar
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, , stereo_match);
//滑鼠響應函數setMouseCallback(視窗名稱, 滑鼠回調函數, 傳給回調函數的參數，一般取0)
setMouseCallback("disparity", onMouse, );
stereo_match(,);
waitKey();
return ;
}