二、搭建系統和雙目測量方案
1.系統搭建
如圖所示，機械部分做的比較簡易。主體支架是用CAD畫的圖，然後找淘寶亞克力加工店加工就可以了，之所以使用支架是為了準確建立舵機雲臺和攝像機座標系的空間關係。其他硬體型號主要是，
相機，普通的30萬畫素攝像頭，解析度640*480，60幀，配3。6mm，60°視角鏡頭（焦距比3。6mm更小的話畸變會更大，5mm或者6mm鏡頭也是可以的，但是立體視場比3。6mm的更遠離攝像頭）
舵機雲臺，淘寶搜雙軸雲臺即可，一般有配套的安裝說明，安裝稍微麻煩一點，需要削減材料，買的時候最好找一站式採購的淘寶店，把鐳射頭和一些固定螺絲、銅柱也買了。
微控制器控制部分，我用的某MINISTM32的開發板，沒有開發板的朋友隨便買個最小系統板就可以勝任demo的功能了，我用的晶片是STM32F103RCT6。
如果有朋友DIY我這套系統的話，我補充一下安裝的注意事項，亞克力板安裝的接插口需要纏少量膠帶，以保證安裝足夠牢固。亞克力底板上打了較多的孔主要是為了靈活的安裝硬體。攝像頭用φ2mm，長15mm的銅柱固定在相機安裝板上，需要購買零件詳細列表的朋友也可以私我。
2.雙目立體矯正和座標計算
雙目立體矯正網上的介紹已經很多了，可以參看以下一些部落格：
【OpenCV】雙目測距（雙目標定、雙目校正和立體匹配）
雙攝像頭測距的OpenCV實現
這兩個部落格已經把雙目測距原理說的比較清楚了，部落格一還貼出了程式碼。
demo使用的測距原理是，影象採集——>單目標定——>立體標定——>立體矯正——>在2幅矯正圖片上尋找目標中心——>計算目標在相機座標系下的三維座標。大家都知道雙目視覺的難點在於立體匹配，而本demo透過在兩幅立體矯正後的圖片上尋找同一物體的中心點直接實現單目標的立體匹配，從而避開整幅影象的立體匹配運算，降低計算難度，提高運算速度。
雙目的影象採集當然要同時顯示兩幅影象了，本demo採用Trackbar來觸發儲存圖片命令，一共採集了15對圖片（我採用的是官方提供的25mm，9*6標定板，也嘗試使用了20mm的標定板但立體標定總是失敗，不知道原因在哪），採集圖片的時候最好拖動滑塊，點選滑塊的話數值容易亂跳。另外需要注意的是不要把雙目立體視覺的左右攝像頭順序弄反了。
對於單目標定和立體標定，有人使用MATLAB完成這兩個功能，我覺得完全沒有必要，opencv本身就能夠很好的完成它們。demo中首先進行單目標定，然後將標定好的相機內參和畸變係數作為立體標定的輸入來實現立體標定，據說這樣可以得到更好的立體標定結果。當然，直接進行立體標定也是可以的，兩者的標定結果差距並不大。下面的部落格提高了參考程式碼OPENCV3。0 雙目立體標定
立體標定後儲存標定結果並矯正圖片對觀察立體的結果，為了方便後續立體矯正的影象顯示validROI區域，將validROIL 和validROIR也進行儲存。：
void outputCameraParam（void）
{
FileStorage fs（“intrinsics。yml”， FileStorage：：WRITE）；
if （fs。isOpened（））
{
fs << “cameraMatrixL” << cameraMatrixL << “cameraDistcoeffL” << distCoeffL
<< “cameraMatrixR” << cameraMatrixR << “cameraDistcoeffR” << distCoeffR；
fs。release（）；
}
else
cout << “Error： can not save the intrinsics！！！！！” << endl；
fs。open（“extrinsics。yml”， FileStorage：：WRITE）；
if （fs。isOpened（））
{
fs << “R” << R << “T” << T << “Rl” << Rl << “Rr” << Rr << “Pl” << Pl << “Pr”
<< Pr << “Q” << Q<< “validROIL” << validROIL << “validROIR” << validROIR；
fs。release（）；
}
else
cout << “Error： can not save the extrinsic parameters\n”；
}
立體矯正圖片對效果，使用標定結果對非標定集影象進行矯正，透過線條觀察標定結果。
下面將重點講解立體標定結果中的矩陣Q，使用矩陣Q中的元素和左右影象對中的物體座標就可以求得該物體在攝像機座標中的三維座標了。
待續：
本demo得到的Q矩陣
雙目立體標定的結果之一Q矩陣
從Q矩陣中我們可以得到以下資訊：
1。立體標定後的左相機主點座標（cx，cy）
2。相機焦距f
3。相機基線距離Tx
4。Q矩陣Q［3］［3］的分子兩項是相等的，所以Q［3］［3］=0
那麼，左相機座標系中點的X、Y、Z均可求得，程式碼如下：
//PL，PR分別為物體在左右影象中的匹配座標點
Vec3d
calXYZ
（
Point2d
PL
，
Point2d
PR
）
{
double
cx
=
-
Q
。
at
<
double
>
（
0
，
3
）；
double
cy
=
-
Q
。
at
<
double
>
（
1
，
3
）；
double
f
=
Q
。
at
<
double
>
（
2
，
3
）；
double
Tx
=
1
/
Q
。
at
<
double
>
（
3
，
2
）；
double
d
=
PL
。
x
-
PR
。
x
；
Vec3d
real
；
real
［
0
］
=
（
PL
。
x
-
cx
）
*
Tx
/
d
；
real
［
1
］
=
（
PL
。
y
-
cy
）
*
Tx
/
d
；
real
［
2
］
=
f
*
Tx
/
d
；
return
real
；
}
至此，雙目立體視覺求取物體實際座標的原理和程式碼講解完畢。
