基於雙目攝像頭的障礙物檢測

前言：關於雙目攝像頭的障礙物檢測以及基於OpenCV的障礙物檢測在CSDN以及部落格園上都有幾篇相關的文章。然而，相當一部分的關於障礙物檢測的文章多偏向於理論，而有實踐的文章卻少之又少。在這裡，我將按照我從網上學習到的例子進行整合並加入了我自己的理解。希望能為大家在障礙物檢測方面起到一定的參考作用。

特別鳴謝：
亦軒Dhc的部落格
琪其齊奇旗棋的CSDN
_寒潭雁影的CSDN

下面開始進入正題啦！

前期準備：

• 這裡我使用的是一個雙目攝像頭進行的障礙物檢測。該雙目攝像頭如圖所示：
  

前期知識點：

• 雙目攝像頭的標定
• 使用OpenCV獲取圖片
• 雙目校正
• 立體匹配

雙目攝像頭的標定的前期知識點：

在這裡，我使用的標定方法是張正友標定法。

• 首先你需要明確，對於一個攝像頭來說，分為內參和外參
• • 內參分別有五個：
• • 攝像頭拍攝到的物體和實際物體在x,y軸上的對映關係（兩個引數）。
• • 攝像頭中心和影象中心的偏移關係（兩個引數）。
• • 攝像頭和鏡頭安裝非完全垂直，存在一個角度的偏差。（一個引數）。
• • 外參有六個：

• • 分別是x，y，z方向上的平移和旋轉。

    只要有了上面的兩種引數，我們基本上就知道了攝像頭拍攝到的影象和現實事物的對應關係了。然而， 攝像頭拍攝影象與現實事物中還是會有“畸變

    ”上的誤差。這是由於鏡頭質量等原因導致的2D點的偏移。

因此，我們使用張正友標定法對攝像頭進行內外引數以及畸變引數的標定。

張正友標定法

前言：張正友標定又稱“張氏標定”。是張正友教授在1998年提出的單平面棋盤格的攝像機標定法。

• 前期準備：
• • 標定板

• • 需要自定準備一個標定板，這標定板的長相大致如下：

• • 實景拍攝的影象如下：

• 這種標定板有兩種方式可以得到：
• • 第一種：直接從opencv官網上下載：
    官網標定板下載

• • 第二種：使用Python+OpenCV生成棋盤格圖片：

    import cv2 
    import numpy as np
    
    width = 350
    height = 500
    length = 50
    
    image = np.zeros((width,height),dtype = np.uint8)
    print(image.shape[0],image.shape[1])
    
    for j in range(height):
       for i in range(width):
           if((int)(i/length) + (int)(j/length))%2:
               image[i,j] = 255;
    cv2.imwrite("pic/chess.jpg",image)
    cv2.imshow("chess",image)
    cv2.waitKey(0)

    所生成的圖片和在OpenCV官網下載得到的圖片是一樣的規格的。

OpenCV下的張正友標定法

• 角點提取

使用的函式1： bool findChessboardCorners(InputArray image,Size atternSIze,OutputArray corners,int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE);

作用：用於提取標定板的內角點，也就是提取示例圖中中每四個黑白格中間的那些角點。

• 引數解析：
• • image：拍攝到的棋盤影象;
• • patternSize：每個棋盤圖上的內角點數。（如果是上圖的話，內角點數Size(9,6),即：每行9個角點，每列9個角點）;
• • corners：用於儲存檢測到的內角點的影象座標位置。（一般用Point2f的向量來表示）;
• • flags：用於定義棋盤圖上的內角點查詢的不同處理方式。
• • 返回值型別為bool，用以返回是否從圖中找到角點。

使用函式2： bool find4QuadCornerSubpix(InputArray img,InputOutputArray corners,Size region_size);

作用：用於在初步提取的角點資訊上進一步提取亞畫素資訊，降低相機標定偏差，該方法專門用來獲取棋盤圖上內角點的精確位置。（有時候也會使用cornerSubPix函式）

• 引數解析：
• • img：輸入的Mat矩陣，最好是8位灰度影象;
• • corners：初始的角點座標向量，同時作為亞畫素座標位置的輸出，所以需要的是浮點型資料，一般用Pointf2f/Point2d的向量來表示。即輸入上面findChessboardCorners函式的第三個引數;
• • region_size：角點搜尋視窗的尺寸;
• 在一般情況下，其實我們用得較多的是cornerSubPix，但是我們這裡用的是棋盤格，而
  find4QuadCornerSubpix是專門用來獲取棋盤圖上內角點的精確位置的。

使用函式3： drawChessboardCorners( InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound);

作用：在棋盤上繪製找到的內角點。

• 引數解析：
• • image：8位灰度或者彩色影象。
• • patternSize：每張標定棋盤上內角點的行列數,即findChessboardCorners的第二個引數;
• • corners：角點座標向量，可用find4QuadCornerSubpix函式的第二個引數輸出做輸入;
• • patternWasFound：標誌位，用來指示定義的棋盤內角點是否被完整的探測到，true表示被完整的探測到，函式會用直線依次連線所有的內角點，作為一個整體，false表示有未被探測到的內角點，這時候函式會以（紅色）圓圈標記處檢測到的內角點;

• • 總查詢角點的實力程式碼大致如下：

Mat imageInput = imread("chess.jpg");
Size board_size = Size(9, 6);//標定板上每行、列的角點數
vector<Point2f> image_points_buf;//快取每幅影象上檢測到的角點
/*提取角點*/
if (!findChessboardCorners(imageInput, board_size, image_points_buf))
{
    cout << "can not find chessboard corners!\n"; //找不到角點  
    return;
}
else
{
    Mat view_gray;
    cvtColor(imageInput, view_gray, CV_RGB2GRAY);
    /*亞畫素精確化*/
    find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5)); //對粗提取的角點進行精確化  
    drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用於在圖片中標記角點  
    imshow("Camera Calibration", view_gray);//顯示圖片  
    waitKey(0);     
}

• 相機標定

  利用上面獲取到的影象角點（理論上需要三張影象，即三組資料，事實上以10~20張為宜，因為這樣誤差會比較小），便可以用calibrateCamera函式做攝像頭標定，計算出攝像頭的內參、外參和畸變引數了。

使用函式1： double calibrateCamera(InputArrays objectPoints,InputAttaysOfArrays imagePoints,Size imageSize,CV_OUT InputOutputArray cameraMatrix,CV_OUT InputOutputArray distCoeffs,OutputArrayOfArrays rvecs,OutputArrayOfArrays tvecs,int flags=0,TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,30,DBL_EPSILON));

• 引數解析：
• • objectPoints：為世界座標系中的三維點。在使用時，應該輸入一個三維座標點的向量集合。一般我們假定標定板放在z=0的平面上，然後依據棋盤上單個黑白方塊的大小（也可以直接都取10，如果不需要很準確的對映到現實事物的話）可以計算出每個內角點的世界座標。
• • imagePoints：為每一個內角點對應的影象座標點。也即是上面求得的各張影象的角點集合。
• • imageSize：為影象的畫素尺寸大小，在計算相機的內參和畸變矩陣的時候需要用到的該引數。
• • cameraMatrix：為相機的內參矩陣。輸入一個Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0))。
• • distCoeffs：為畸變矩陣。輸入一個Mat distCoffs=Mat(1,5,CV_32FC1,Scalar::all(0));即可。
• • rvecs：旋轉向量。應該輸入一個Mat型別的vector，即vector
• • tvecs：位移向量。和rvecs一樣，應該為vector
• • flags：標定時所採用的演算法。flags有如下幾個引數（直接不寫則依據下面引數描述中沒設引數的情況進行）：
• • • CV_CALIB_USE_INTRINSIC_GUESS：使用該引數時，在cameraMatrix矩陣中應該有fx,fy,u0,v0的估計值。否則的話，將初始化(u0,v0）影象的中心點，使用最小二乘估算出fx，fy。
• • • CV_CALIB_FIX_PRINCIPAL_POINT：在進行優化時會固定光軸點。當CV_CALIB_USE_INTRINSIC_GUESS引數被設定，光軸點將保持在中心或者某個輸入的值。
• • • CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只將fy作為可變數，進行優化計算。當CV_CALIB_USE_INTRINSIC_GUESS沒有被設定，fx和fy將會被忽略。只有fx/fy的比值在計算中會被用到。
• • • CV_CALIB_ZERO_TANGENT_DIST：設定切向畸變引數（p1,p2）為零。
• • • CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：對應的徑向畸變在優化中保持不變。
• • • CV_CALIB_RATIONAL_MODEL：計算k4，k5，k6三個畸變引數。如果沒有設定，則只計算其它5個畸變引數。
• • criteria：最優迭代終止條件設定。

在使用該函式進行標定運算之前，需要對棋盤上每個角點的空間座標系位置座標進行初始化（就是對其進行賦值），算出相機內參矩陣、相機畸變、另外每張圖片會生成屬於自己的平移向量和旋轉向量。

具體實現程式碼大致如下：

Size image_size;//影象的尺寸
Size board_size = Size(9, 6);     //標定板上每行、列的角點數
vector<Point2f> image_points_buf;  //快取每幅影象上檢測到的角點
vector<vector<Point2f>> image_points_seq; //儲存檢測到的所有角點
/*提取角點*/
char filename[10];
for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
{
    sprintf_s(filename, "%d.jpg", image_num);
    Mat imageInput = imread(filename);
    if (!findChessboardCorners(imageInput, board_size, image_points_buf))
    {
        cout << "can not find chessboard corners!\n";//找不到角點  
        return;
    }
    else
    {
        Mat view_gray;
        cvtColor(imageInput, view_gray, CV_RGB2GRAY);
        /*亞畫素精確化*/
        find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5));//對粗提取的角點進行精確化  
        drawChessboardCorners(view_gray, board_size, image_points_buf, true);//用於在圖片中標記角點  
        image_points_seq.push_back(image_points_buf);//儲存亞畫素角點  
        imshow("Camera Calibration", view_gray);//顯示圖片  
        //waitKey(500);//停半秒
    }
    image_size.width = imageInput.cols;
    image_size.height = imageInput.rows;
    imageInput.release();
}
/*相機標定*/
vector<vector<Point3f>> object_points; //儲存標定板上角點的三維座標,為標定函式的第一個引數
Size square_size = Size(10, 10);//實際測量得到的標定板上每個棋盤格的大小，這裡其實沒測，就假定了一個值，感覺影響不是太大，後面再研究下
for (int t = 0; t<IMGCOUNT; t++)
{
    vector<Point3f> tempPointSet;
    for (int i = 0; i<board_size.height; i++)
    {
        for (int j = 0; j<board_size.width; j++)
        {
            Point3f realPoint;
            //假設標定板放在世界座標系中z=0的平面上
            realPoint.x = i*square_size.width;
            realPoint.y = j*square_size.height;
            realPoint.z = 0;
            tempPointSet.push_back(realPoint);
        }
    }
    object_points.push_back(tempPointSet);
}
//內外引數物件
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//攝像機內參數矩陣
vector<int> point_counts;// 每幅影象中角點的數量  
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));//攝像機的5個畸變係數：k1,k2,p1,p2,k3
vector<Mat> tvecsMat;//每幅影象的旋轉向量
vector<Mat> rvecsMat;//每幅影象的平移向量
calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);//攝像頭標定

• 影象矯正：

  現在已經通過標定，然後得到攝像頭的各個引數，後面就可以用這些得到的引數來做攝像頭的矯正了。

使用函式： void undistort(InputArray src, OutputArray dst,InputArray cameraMatrix,InputArray distCoeffs,InputArray newCameraMatrix=noArray());

• 引數解析：
• • src：輸入引數，代表畸變的原始影象;
• • dst：矯正後的輸出影象，跟輸入影象具有相同的型別和大小;
• • cameraMatrix：之前求得的相機內參矩陣;
• • distCoeffs：之前求得的相機畸變矩陣;
• • newCameraMatrix：預設跟cameraMatrix保持一致;

具體使用程式碼如下：

/*用標定的結果矯正影象*/
for (size_t image_num = 1; image_num <= IMGCOUNT; image_num++)
{
    sprintf_s(filename, "%d.jpg", image_num);
    Mat imageSource = imread(filename);
    Mat newimage = imageSource.clone();
    undistort(imageSource, newimage, cameraMatrix, distCoeffs);
    imshow("source", imageSource);//顯示圖片 
    imshow("drc", newimage);//顯示圖片  
    waitKey(500);//停半秒
    imageSource.release();
    newimage.release();
}

matlab下的張正友標定法

前期準備：工具箱下載

安裝：

• 將下載的工具箱檔案toolbox_calib.zip解壓縮，將目錄toolbox_calib拷貝到Matlab的目錄下，也可以放在其他目錄。
• 執行Matlab並新增資料夾TOOLBOX_calib的位置到matlab路徑path中。
• 具體操作為：File->SetPath->Add Folder To Path，然後找到剛剛存放的資料夾
  TOOLBOX_calib，save一下就OK了。
• 採集影象：採集的影象統一命名後，拷貝到toolbox_calib目錄中。命名規則為基本名和編號，基本名在前，後面直接跟著數字編號。編號最多為3位十進位制數字。

單目標定

• 準備工作

將雙目攝像機拍攝的左右影象的資料夾作為matlab的當前資料夾：我的影象名稱類似L1，L2……，R1，R2……（注：影象的命名格式：字母+數字，即字母在前，數字在後。）

• matlab中命令視窗輸入calib_gui，回車後彈出如下視窗：

• 選擇第一個選項，彈出下面的主視窗：

• 備註：

• • “Image names”鍵：指定影象的基本名(Basename)和影象格式，並將相應的影象讀入記憶體。
• • “Read names”鍵：將指定基本名和格式的影象讀入記憶體。
• • “Extract grid corners”鍵：提取網格角點。
• • “Calibration”鍵：內參數標定。
• • “Show Extrinsic”鍵：以圖形方式顯示攝像機與標定靶標之間的關係。
• • “Project on images”鍵：按照攝像機的內參數以及攝像機的外引數(即靶標座標系相對於攝像機座標系的變換關係)，根據網格點的笛卡爾空間座標，將網格角點反投影到影象空間。
• • “Analyse error”鍵：影象空間的誤差分析
• • “Recomp. corners”鍵：重新提取網格角點。
• • “Add/Suppress images”鍵：增加/刪除影象。
• • “Save”鍵：儲存標定結果。將內參數標定結果以及攝像機與靶標之間的外引數儲存為m檔案Calib_results.m，存放於toolbox_calib目錄中。
• • “Load”鍵：讀入標定結果。從存放於toolbox_calib目錄中的標定結果檔案Calib_results.mat讀入。
• • “Exit”鍵：退出標定。
• • “Comp. Extrinsic”鍵：計算外引數。
• • “Undistort image”鍵：生成消除畸變後的影象並儲存。
• • “Export calib data”鍵：輸出標定資料。分別以靶標座標系中的平面座標和影象中的影象座標，將每一幅靶標影象的角點儲存為兩個tex檔案。
• • “Show calib results”鍵：顯示標定結果。

• 選擇第一個按鈕，在命令視窗中會出現當前資料夾中的所有資訊：

並提示你輸入“Basename camera calibration images (without number norsuffix):”，對於我的影象名稱做左圖的標定時輸入：L。影象格式的選擇 j。回車後，顯示load的所有影象。

共讀入20幅影象

• 回到主視窗，選擇第三個選項,命令視窗有如下提示：選擇預設，敲擊回車即可:

• 備註：
• • a:“wintx ([] = 5) =”和“winty ([] = 5) =”輸入行中輸入角點提取區域的視窗半寬m和半高n。m和n為正整數，單位為畫素，預設值為5個畫素。選定m和n後，命令視窗顯示角點提取區域的視窗尺寸(2n+1)x(2m+1)。例如，選擇預設時角點提取區域的視窗尺寸為11x11畫素。
• • b:”Do you want to use the automatic square counting mechanism (0=[]=default) or do you always want to enter the number of squares manually (1,other)? “時，選擇預設值0表示自動計算棋盤格靶標選定區域內的方格行數和列數，選擇值1表示人工計算並輸入棋盤格靶標選定區域內的方格行數和列數。
• 回車敲完後，顯示第一幅棋盤格，進行角點的提取工作：用滑鼠單擊棋盤格外圍4個角點，點選的第一個點是原點O，順序（逆序）點選其他點，過程如圖：

• 注意：

• • 1)、 這裡有的要求標定內角點，如圖所示。查閱多方資料並未見明確要求，但通過比較，我個人認為是因標定板而異。
• • 
• • 2)、所形成的四邊形的邊應與棋盤格靶標的網格線基本平行。否則，影響角點提取精度，甚至導致角點提取錯誤。

• 回到命令視窗，輸入棋盤資訊：

“Number of squares along the X direction ([]=10) =”輸入X方向方格數目：7

“Number of squares along the Y direction ([]=10) =”輸入X方向方格數目：9

“Size dX of each square along the X direction ([]=100mm) =”輸入X方向方格長度（mm）：29；我的棋盤格X、Y方向長度相同均是29mm

回車後，顯示角點提取結果：（我的棋盤是列印後貼在紙盒上的，棋盤並不是絕對的平面，所以有些角點的位置與真實位置有些出入）

• 備註：

在Matlab命令窗口出現“Need of an initial guess for distortion? ([]=no, other=yes) ”時，如果選擇no則不輸入畸變初始值，如果選擇yes則輸入畸變初始值。輸入的畸變初始值，將同時賦值給需要估計的5個畸變係數，即徑向畸變係數kc(1)、kc(2)、kc(5)和切向畸變係數kc(3)、kc(4)。如果不估計6階徑向畸變係數kc(5)，則kc(5)被賦值為0

• 依次迴圈標定後面的影象（eg.我的是20幅）

注意：當第一幅影象的資訊填好後，第二幅會以第一幅的資訊為預設值，只需回車即可。但是，要注意XY兩個方向只與你點選的第一個角點有關，舉例說明：

兩圖棋盤方向相同（橫向），但是，我點選的第一個角點不同，原點O不同，XY的方向也不同。所以，為了不用每次都重新填寫棋盤資訊，第一個角點選擇同一個位置的點（如果所拍攝的棋盤方向不同<橫向、縱向都有>，則第一個點是相對於棋盤位置相同的點，說多了又是淚）

• 角點提取完成以後進行標定處理，資料夾中出現.mat檔案。（此處最好將其更名為 calib_data_left.mat,以免後面對右影象進行標定時將此結果覆蓋。）

迴歸主視窗，選擇第四個選項：

• 顯示攝像機與標定板間的關係：

主視窗點選，即可在新的圖形視窗顯示攝像機與標定靶標之間的關係:

• 誤差分析：

點選，即可在新的圖形視窗顯示出標定使用的所有角點反投影到影象空間的影象座標誤差，如圖所示：

在圖所示的圖形視窗，利用滑鼠移動十字標尺可以選擇角點，即可在命令視窗顯示出該角點的資訊，包括該角點所屬影象、索引號、以方格為單位的座標、影象座標、反投影后的影象座標誤差、角點提取區域的視窗半寬m和半高n。

• 點選，資料夾中出現如下檔案：

• 注： Result.mat件在雙目標定中能夠用到。將"Calib_Results.mat"改成"Calib_Results_left.mat "

• 點選“Undistort image”對影象進行去畸變處理，選擇對某一張影象還是所有影象進行處理，預設是all。隨後，儲存所有畸變處理後的影象。

雙目測定

• 用同樣的辦法處理右相機拍攝的影象。

標定結果：

• 單獨得到攝像頭標定完成以後就可以進行立體標定了。在matlab命令視窗中輸入stereo_gui，彈出如下視窗：

• 點選，命令列視窗提示.mat檔案的名稱，預設的檔名(Calib_Result_left.mat和Calib_Result_right.mat)，直接回車即可，或者輸入自己的檔名稱。

• load檔案後，命令視窗顯示左右攝像機的引數資訊，

• 備註：

• fc_left是左攝像機的放大係數，即焦距歸一化成像平面上的成像點座標到影象座標的放大係數。cc_left為左攝像機的主點座標，單位為畫素。alpha_c_left是對應於左攝像機的實際y軸與理想y軸之間的夾角，單位為弧度，預設值為0弧度。kc_left為左攝像機的畸變係數。fc_right是右攝像機的放大係數，即焦距歸一化成像平面上的成像點座標到影象座標的放大係數。cc_right為右攝像機的主點座標，單位為畫素。alpha_c_right是對應於右攝像機的實際y軸與理想y軸之間的夾角，單位為弧度，預設為0弧度。kc_right為右攝像機的畸變係數。om為左攝像機相對於右攝像機的姿態矩陣的rodrigues旋轉向量，利用函式rodrigues可以轉換為姿態矩陣。T為左攝像機相對於右攝像機的位移向量，即左攝像機座標系原點在右攝像機座標系中的位移向量，單位mm。

• 點選，計算優化後的外引數。命令視窗輸出左、右攝像機的內參數和優化後的外引數。輸出結果如下所示：

• 點選，顯示標定靶面相對於雙目攝像機的位置：

• 點選，將標定結果儲存為檔案

• 備註：

• 雙目標定各個按鈕功能：

• • “Load left and right calibration files”鍵：讀入左、右攝像機的標定結果，並對左攝像機相對於右攝像機的位姿進行初步標定。
• • “Run stereo calibration”鍵：計算優化後的外引數。
• • “Show Extrinsics of stereo rig”鍵：顯示靶標相對於攝像機的位姿。
• • “Show Intrinsic parameters”鍵：在Matlab的命令視窗顯示左、右攝像機的內參數和優化後的外引數。
• • “Save stereo calib results”鍵：將標定結果儲存為檔案Calib_Results_stereo.mat，存放於toolbox_calib目錄中。
• • “Load stereo calib results”鍵：讀入標定結果。從存放於toolbox_calib目錄中的標定結果檔案Calib_Results_stereo.mat讀入。
• • “Rectify the calibration images”鍵：按照畸變係數對左、右攝像機採集的所有靶標影象進行處理，生成消除畸變後的影象並儲存在toolbox_calib目錄中。生成的消除畸變後的影象，以原影象的檔名在基本名和編號之間插入_rectified作為其檔名。
• • “Exit”鍵：退出立體視覺標定。

總結：

Matlab工具箱標定，注意問題：

• 內參數標定需要注意的問題
• • 製作棋盤格靶標時應特別注意，黑色方格與白色方格尺寸需要相同，而且所有方格的尺寸必須嚴格一致。靶標的方格數量不宜太小，行數和列數以大於10為宜。方格的尺寸不宜太大或太小，採集的整幅靶標影象中方格的邊長尺寸不小於20畫素。
• • 採集靶標影象時應特別注意，需要在不同的角度不同的位置採集靶標的多幅影象。採集到的影象必須清晰，靶標影象尺寸以佔整幅影象尺寸的1/3～3/4為宜。靶標影象最好在整幅影象的不同位置都有分佈，不宜過於集中於同一區域。靶標放置位置與攝像機之間的距離最好為視覺系統的主要工作距離。靶標相對於攝像機的角度應有較大範圍的變化，應包含繞三個軸較大角度的旋轉，最好不小於30度。採集的靶標影象數量不應太少，建議以10～20幅靶標影象為宜。 
• • 採集影象過程中，攝像機的焦距不能調整。因為焦距屬於攝像機的內參數，不同焦距下采集的影象隱含了不同的內參數，這些影象放在一起進行標定不能得到正確的結果。
• • 採集的靶標影象統一命名，由基本名和編號構成，如Image1~Image15。靶標影象的資料格式必須相同。
• • 提取角點時，在圖形視窗利用滑鼠點選設定棋盤格靶標的選定區域。點選的第一個角點作為靶標座標系的原點，順序點選4個角點形成四邊形。相鄰兩次點選的角點應在同一條網格線上，使得所形成的四邊形的邊應與棋盤格靶標的網格線基本平行。為提高點選的角點的精度，建議將顯示靶標影象的影象視窗放大到最大，利用滑鼠的十字標線儘可能準確的點選4個角點。
• • 攝像機的實際y軸與理想y軸之間的夾角ac是否標定，由est_alpha標誌位設定。est_alpha=1時對alpha_c進行標定，est_alpha=0時不對alpha_c進行標定。
• • 陣列est_dist(1:5)是畸變係數kc(1:5)是否標定的標誌，只對標誌取值為1的畸變係數標定，標誌取值為0的畸變係數不標定。預設值為est_dist(1:5)=[1 1 1 1 0]，即對畸變係數kc1～kc4進行標定，對kc5不進行標定，kc5=0。
• • 執行calib_gui指令後，Matlab處於busy狀態，Matlab命令視窗不再響應其它命令。只有在點選標定工具箱的“Exit”鍵退出標定後，Matlab命令窗口才能恢復響應其它命令。
• 外引數標定需要注意的問題
• • 方格尺寸必須輸入實際尺寸
• • 提取角點時，在圖形視窗利用滑鼠點選的第一個角點作為靶標座標系的原點，得到的外引數是靶標座標系在攝像機座標系中的位姿
• • rodrigues旋轉向量omc_ext與姿態矩陣Rc_ext可以利用rodrigues函式進行轉換。omc_ext=rodrigues(Rc_ext)，Rc_ext=rodrigues(omc_ext)
• 立體視覺標定需要注意的問題
• • 提取角點時，在圖形視窗利用滑鼠點選的第一個角點作為靶標座標系的原點，左右攝像機對應的靶標影象對需要選擇相同的第一個角點作為原點。其他的3個角點在左右攝像機的靶標影象中也應相同。
• • 左右攝像機採集的影象數量必須相同。相同的編號的左右攝像機採集的影象是靶標在同一位姿時左右攝像機採集的影象，構成一組立體視覺的靶標影象對。
• • 得到的外引數是左攝像機相對於右攝像機的位姿，即左攝像機座標系在右攝像機座標系中的位姿。
• • 執行stereo_gui指令後，Matlab命令視窗可以響應其它命令。

OpenCV獲取圖片

使用OpenCV獲取圖片有兩種程式碼，一種是使用Python，另外一種是使用C++，但是道理其實是一樣的。先讀取視訊，然後再取幀。

• Python程式碼：
• 注意：儲存路徑需要根據自己的電腦進行配置，我這裡只是使用我在我的電腦上使用的路徑進行的拍照、儲存功能。

import cv2
import time
from PIL import Image


def shot(pos, frame):
    global counter
    path = folder + pos + "_" + str(counter) + ".jpg"
    cv2.imwrite(path, frame)
    print("snapshot saved into: " + path)

if __name__ == '__main__':

    AUTO = True  # 自動拍照，或手動按s鍵拍照
    INTERVAL = 1  # 自動拍照間隔

    cv2.namedWindow("middle")
    cv2.moveWindow("middle", 400, 0)
    middle_camera = cv2.VideoCapture(1)

    counter = 0
    utc = time.time()
    pattern = (12, 8)  # 棋盤格尺寸
    folder = "F:/PyCharm_code/OpenCVDemo/snapshot/"  # 拍照檔案目錄(請根據自己的路徑進行更改)

    while True:
        # ret, left_frame = left_camera.read()
        ret, middle_frame = middle_camera.read()

        # cv2.imshow("left", left_frame)
        cv2.imshow("middle", middle_frame)

        now = time.time()
        if AUTO and now - utc >= INTERVAL:
            shot("middle", middle_frame)
            img = Image.open(folder + "middle" + "_" + str(counter) + ".jpg")
            width, height = img.size
            w = width * 0.5
            right_box = (0, 0, w, height)
            left_box = (w, 0, width, height)
            right_region = img.crop(right_box)
            left_region = img.crop(left_box)
            right_region.save(folder + "RightTest" + "/" + str(counter) + ".jpg")
            left_region.save(folder + "LeftTest" + "/" + str(counter) + ".jpg")

            counter += 1
            utc = now


        key = cv2.waitKey(1)
        if key == ord("q"):
            break
        elif key == ord("s"):
            shot("middle", middle_frame)
            counter += 1

    middle_camera.release()
    cv2.destroyWindow("middle")

• 備註：由於我使用的雙目攝像頭拍照的時候所呈現的是左、右攝像頭顯示在同一個螢幕上，所以我中間有一個分割影象的過程。分割影象程式碼如下所示：

            img = Image.open(folder + "middle" + "_" + str(counter) + ".jpg")
            width, height = img.size
            w = width * 0.5
            right_box = (0, 0, w, height)
            left_box = (w, 0, width, height)
            right_region = img.crop(right_box)
            left_region = img.crop(left_box)
            right_region.save(folder + "RightTest" + "/" + str(counter) + ".jpg")
            left_region.save(folder + "LeftTest" + "/" + str(counter) + ".jpg")

• 讀取圖片然後再進行分割，之後儲存。

• C++程式碼：

    VideoCapture capture(0);
    Mat frame;
    if (!capture.isOpened())
    {
        cout << "攝像頭開啟失敗" << endl;
        return -1;
    }
    char key;
    char filename[200] = "F:\VisualStudioProject\OpencvTest\srcPicture";//此路徑需要根據自己電腦進行設定
    int count = 0;
    namedWindow("【視訊】", 1);
    namedWindow("【圖片】", 1);
    while (1) {
        key = waitKey(30);
        capture >> frame;
        imshow("【視訊】", frame);
        /*imwrite(filename, frame);*/
        if (key == 27)
            break;//按ESC鍵退出程式  
        if (key == 32)//按空格鍵進行拍照  
        {
            sprintf_s(filename, "%d.jpg", ++count);
            cout << "執行完畢" << endl;
            imwrite(filename, frame);//圖片儲存到本工程目錄中  
            imshow("【圖片】", frame);
        }

    }

對圖片進行立體矯正

/*立體校正*/
Rodrigues(rec, R); //Rodrigues變換
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR,imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,0, imageSize, &validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL,Rl,Pr,imageSize,CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR,Rr,Pr,imageSize,CV_32FC1, mapRx, mapRy);

立體匹配

採用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)，才能得到正確的三維座標資訊。

• 立體匹配程式碼：

/*****立體匹配*****/
void stereo_match(int, void*)
{
    bm->setBlockSize(2 * blockSize + 5);     //SAD視窗大小，5~21之間為宜
    bm->setROI1(validROIL);
    bm->setROI2(validROIR);
    bm->setPreFilterCap(31);
    bm->setMinDisparity(0);  //最小視差，預設值為0, 可以是負值，int型
    bm->setNumDisparities(numDisparities * 16 + 16);//視差視窗，即最大視差值與最小視差值之差,視窗大小必須是16的整數倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止誤匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8,copyImage;
    bm->compute(rectifyImageL, rectifyImageR, disp);//輸入影象必須為灰度圖
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//計算出的視差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在實際求距離時，ReprojectTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正確的三維座標資訊。
    xyz = xyz * 16;
    imshow("disparity", disp8);
    copyImage = disp8.clone();
    imshow("contour", copyImage);
    //根據現有的視差圖進行凸包的繪製
    Mat threshold_output;
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    RNG rng(12345);
    threshold(copyImage, threshold_output, 20, 255, CV_THRESH_BINARY);//二值化
    findContours(threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));//尋找輪廓
    /// 對每個輪廓計算其凸包
    vector<vector<Point> >hull(contours.size());
    vector<vector<Point> > result;
    for (int i = 0; i < contours.size(); i++)
    {
        convexHull(Mat(contours[i]), hull[i], false);

    }

    /// 繪出輪廓及其凸包
    Mat drawing = Mat::zeros(threshold_output.size(), CV_8UC3);
    for (int i = 0; i< contours.size(); i++)
    {
        if (contourArea(contours[i]) < 500)//面積小於area的凸包，可忽略
            continue;
        result.push_back(hull[i]);
        Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
        drawContours(drawing, contours, i, color, 1, 8, vector<Vec4i>(), 0, Point());
        drawContours(drawing, hull, i, color, 1, 8, vector<Vec4i>(), 0, Point());
    }
    imshow("contours", drawing);//凸包大小
}

總結：

使用MatLab或者OpenCV進行內參以及外參的設定，然後將測出的資料填入到程式碼中，再使用立體矯正以及立體匹配，這樣就可以得到深度圖，使用深度圖再進行視差運算，得到視差圖。

• 完整程式碼如下：

#include <opencv2\opencv.hpp>
#include <string>
#include <iostream>
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

const int imageWidth = 320;                             //攝像頭的解析度  
const int imageHeight = 240;
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 = 7, uniquenessRatio = 20, numDisparities = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);

/*
事先標定好的相機的引數
fx 0 cx
0 fy cy
0 0  1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 248.32797, 0, 248.24842,
    0, 150.87402, 114.30813,
    0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.04477, -0.10081, 0.01026, 0.00132, 0.00000);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 248.74867, 0, 248.84978,
    0, 152.62972, 98.07575,
    0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << -0.04158, 0.08338, -0.00584, 0.00611, 0.00000);

Mat T = (Mat_<double>(3, 1) << 182.44004, 0.20804, 0.41865);//T平移向量
Mat rec = (Mat_<double>(3, 1) << -0.05347, -0.00229, -0.00203);//rec旋轉向量
Mat R;//R 旋轉矩陣


/*****立體匹配*****/
void stereo_match(int, void*)
{
    bm->setBlockSize(2 * blockSize + 5);     //SAD視窗大小，5~21之間為宜
    bm->setROI1(validROIL);
    bm->setROI2(validROIR);
    bm->setPreFilterCap(31);
    bm->setMinDisparity(0);  //最小視差，預設值為0, 可以是負值，int型
    bm->setNumDisparities(numDisparities * 16 + 16);//視差視窗，即最大視差值與最小視差值之差,視窗大小必須是16的整數倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止誤匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8,copyImage;
    bm->compute(rectifyImageL, rectifyImageR, disp);//輸入影象必須為灰度圖
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//計算出的視差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在實際求距離時，ReprojectTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正確的三維座標資訊。
    xyz = xyz * 16;
    imshow("disparity", disp8);
    copyImage = disp8.clone();
    imshow("contour", copyImage);
    //根據現有的視差圖進行凸包的繪製
    Mat threshold_output;
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    RNG rng(12345);
    threshold(copyImage, threshold_output, 20, 255, CV_THRESH_BINARY);//二值化
    findContours(threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));//尋找輪廓
    /// 對每個輪廓計算其凸包
    vector<vector<Point> >hull(contours.size());
    vector<vector<Point> > result;
    for (int i = 0; i < contours.size(); i++)
    {
        convexHull(Mat(contours[i]), hull[i], false);

    }

    /// 繪出輪廓及其凸包
    Mat drawing = Mat::zeros(threshold_output.size(), CV_8UC3);
    for (int i = 0; i< contours.size(); i++)
    {
        if (contourArea(contours[i]) < 500)//面積小於area的凸包，可忽略
            continue;
        result.push_back(hull[i]);
        Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
        drawContours(drawing, contours, i, color, 1, 8, vector<Vec4i>(), 0, Point());
        drawContours(drawing, hull, i, color, 1, 8, vector<Vec4i>(), 0, Point());
    }
    imshow("contours", drawing);//凸包大小
}



/*****描述：滑鼠操作回撥*****/
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, 0, 0);
        selectObject = true;
        cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
        /*cout << origin << "深度資訊為：" << xyz.at<Vec3f>(origin) << endl;*/
        break;
    case EVENT_LBUTTONUP:    //滑鼠左按鈕釋放的事件
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
            break;
    }
}




int main()
{
    VideoCapture capture(0);
    Mat frame;
    if (!capture.isOpened())
    {
        cout << "攝像頭開啟失敗" << endl;
        return -1;
    }
    char key;
    char filename[200] = "F:\VisualStudioProject\OpencvTest\srcPicture";
    int count = 0;
    namedWindow("【視訊】", 1);
    namedWindow("【圖片】", 1);
    while (1) {
        key = waitKey(30);
        capture >> frame;
        imshow("【視訊】", frame);
        /*imwrite(filename, frame);*/
        if (key == 27)
            break;//按ESC鍵退出程式  
        if (key == 32)//按空格鍵進行拍照  
        {
            sprintf_s(filename, "%d.jpg", ++count);
            cout << "執行完畢" << endl;
            imwrite(filename, frame);//圖片儲存到本工程目錄中  
            imshow("【圖片】", frame);
        }

    }
    
    /*
    立體校正
    */
    Rodrigues(rec, R); //Rodrigues變換
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
        0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

    /*
    讀取圖片
    */
    string left_test = "F:\\VisualStudioProject\\OpencvTest\\LeftPicture\\LDemo.jpg";
    string right_test = "F:\\VisualStudioProject\\OpencvTest\\RightPicture\\RDemo.jpg";
    rgbImageL = imread(left_test, CV_LOAD_IMAGE_COLOR);
    cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
    rgbImageR = imread(right_test, 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 = 600. / MAX(imageSize.width, imageSize.height);
    w = cvRound(imageSize.width * sf);
    h = cvRound(imageSize.height * sf);
    canvas.create(h, w * 2, CV_8UC3);   //注意通道

                                        //左影象畫到畫布上
    Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到畫布的一部分  
    resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, 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, 0, w, h));                                      //獲得畫布的另一部分  
    resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, 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 = 0; i < canvas.rows; i += 16)
        line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
    imshow("rectified", canvas);
    //find_obstacle(grayImageL, 20, 255, 500);

    /*
    立體匹配
    */
    namedWindow("disparity", CV_WINDOW_AUTOSIZE);
    // 建立SAD視窗 Trackbar
    createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match);
    // 建立視差唯一性百分比視窗 Trackbar
    createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
    // 建立視差視窗 Trackbar
    createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
    //滑鼠響應函式setMouseCallback(視窗名稱, 滑鼠回撥函式, 傳給回撥函式的引數，一般取0)
    setMouseCallback("disparity", onMouse, 0);
    stereo_match(0, 0);

    waitKey(0);
    return 0;
}

備註：

• 上面的程式碼中，需要自己填入的資料有：
• • 測定的攝像頭的內參以及外參
• • 儲存以及讀取圖片的路徑