1. 程式人生 > >VS2017+OpenCV3.3基於SGBM演算法的雙目立體視覺、雙目測距(雙目校正和立體匹配)

VS2017+OpenCV3.3基於SGBM演算法的雙目立體視覺、雙目測距(雙目校正和立體匹配)

前些日子做了一個關於雙目立體視覺的入門作業,現在在這裡總結一下學到的一些知識(寫的可能會有很多欠缺的地方,還望海涵!)
本篇部落格不涉及雙目標定的知識,關於雙目標定網上資料很多,大家可以自行查詢學習。
先說一下本部落格的雙目立體視覺的實現基礎,已知以下資訊:
(1)雙目採集影象解析度為1920X1024;

(2)雙目相機相對於虛擬焦平面的外參及各自內參如下:

 左視相機:

 內參:

 像元大小 = 5.86微米;
 焦距     = [ 4334.09568   4334.09568 ](畫素);
 主點座標 = [ 959.50000   511.50000 ]  (畫素);

 相對於虛擬焦平面的外參:

 旋轉向量 = [ 0.04345   -0.05236  -0.01810 ];
 平移向量 = [ -518.97666   01.20629  9.14632 ](毫米)
 右視相機:

 內參:

 像元大小 = 5.86微米;
 焦距     = [ 4489.55770   4507.13412 ](畫素);
 主點座標 = [ 801.86552   530.72579 ]  (畫素);

 相對於虛擬焦平面的外參:

 旋轉向量 = [ 0.04345   -0.05236  -0.01810 ];
 平移向量 = [ 518.97666   -01.20629  -9.14632 ](毫米)

注:不考慮畸變校正;

通過以上資訊,首先確定解決問題的技術思路如下圖:
這裡寫圖片描述
由上圖可以看到在已知內外引數的前提下,只需要讀取圖片直接進行校正,接下來介紹關於雙目校正的知識。
雙目校正


要計算目標點在左右兩個檢視上形成的視差,首先要把該點在左右檢視上兩個對應的像點匹配起來。校正的目的是對左右兩幅影象平面重投影,使得它們精確落在同一個平面上,而且影象的行完全地對準到前向平行的結構上。
題目已經給出了旋轉矩陣R和平移向量 T,立體校正Bouguet 演算法能簡單地使左右影象中的每幅重投影次數最小且重投影畸變最大,所以使立體匹配更加準確和快速,並使左右影象的觀測面積最大。
通過投影矩陣P把三維點變換成可以在平面上顯示的二維點
[■(x@y@w)]=P[■(X@Y@■(Z@1))]   (1) (1)
投影平面上的點座標為(x /w,y /w) 。同理,二維點也可通過重投影矩陣Q 重投影為三維點
(2)(3)
其中(c_x,c_y )為主點在左影象上的座標,f 為焦距,T_x
為雙目間距,c_x^'為主點在右影象的x座標。根據式(2)得到三維座標為: (X /W,Y/W,Z /W) 。在 OpenCV中可通過stereoRectify() 函式完成以上校正功能,該函式輸入引數為攝像機矩陣,畸變向量,左右旋轉矩陣R 和平移向量T。輸出引數有式 (1)中投影矩陣P,分別為P_leftP_right,以及重投影矩陣Q。可呼叫函式InitUndistortRectifyMap( ) 生成影象校正所需的對映矩陣。最後經過remap()函式,使左右相機的影象共面並且行對準。效果如下圖:
這裡寫圖片描述

立體匹配與生成深度資訊
立體匹配完成匹配左右攝像機檢視的相同特徵,並得到視差圖,視差值是匹配時相同特徵點在x座標軸上的差值x_l-x_r。得到視差圖後可通過三角相似的原理得到目標物體的距離。
2.2.1立體成像原理
假設攝像機沒有畸變,左右攝像機的成像平面已經嚴格對準,左右主點已經校準,主光線也是平行的。理想立體攝像機模型如下圖:
這裡寫圖片描述
設兩個攝像機分別移動到世界座標系的原點,可分別得到各自獨立的W點相對像平面點的X和Y座標式:
這裡寫圖片描述
由式(6) 易知視差d和距離Z成反比,當視差很小時,視差的變化對距離Z的影響較大; 當視差較大時,視差的變化對距離Z的影響較小,因此,測距系統僅當距離較近時精度較高。

立體匹配演算法介紹
SGBM演算法介紹
在OpenCV中使用函式StereoSGBM ( ) 實現了SGBM演算法。SGBM 演算法核心步驟為:選取匹配基元;構建基於多個方向的掃描線的代價能量和函式;求取能量代價和函式的最優解。OpenCV中SGMB演算法的實現主要分為以下四個步驟:
①預處理
SGBM採用水平Sobel運算元,把影象做處理,然後用一個函式將經過水平Sobel運算元處理後的影象上每個畫素點(P表示其畫素值)對映成一個新的影象,P_NEW表示新影象上的畫素值。對映函式如下:
這裡寫圖片描述
preFilterCap為一個常數引數,openCv預設取15。預處理實際上是得到影象的梯度資訊。經預處理的影象儲存起來,將會用於計算代價。
②代價計算
代價有兩部分組成:經過預處理得到的影象的梯度資訊經過基於取樣的方法得到的梯度代價;原影象經過基於取樣的方法得到的SAD代價。
③動態規劃
用一維約束近似二維約束。在P的周圍,以 45°為間隔設定了8個路徑。通過8個路徑計算最小代價路徑L_r (p,d),以此來近似二維約束匹配計算.
其中動態規劃很重要兩個引數P1,P2是這樣設定的:
P1 =8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
cn是影象的通道數, SADWindowSize是SAD視窗大小,數值為奇數。可以看出,當影象通道和SAD視窗確定下來,SGBM的規劃引數P1和P2是常數。
④後處理
openCvSGBM的後處理包含以下幾個步驟:
Step1:唯一性檢測:視差視窗範圍內最低代價是次低代價的(1 + uniquenessRatio/100)倍時,最低代價對應的視差值才是該畫素點的視差,否則該畫素點的視差為0。其中uniquenessRatio是一個常數引數。
Step2:亞畫素插值
Step3:左右一致性檢測:誤差閾值disp12MaxDiff預設為1,可以自己設定。

獲得深度資訊
經過sgbm->compute(rectifyImageL, rectifyImageR, disp)獲得視差對映後,利用式(2),式(3) ,通過簡單的矩陣相乘就可提取深度資訊。三維座標就是( X /W,Y/W,Z /W) 。OpenCV中使用reprojectImageTo3D( )函式實現該功能,該函式輸入上面得到的視差資料,輸出所需的三維點陣,然後提取深度資訊。

SGBM引數設定:(在下面的程式中已經標明)
MinDisparity設定為0,因為兩個攝像頭是前向平行放置,相同的物體在左圖中一定比在右圖中偏右。如果為了追求更大的雙目重合區域而將兩個攝像頭向內偏轉的話,這個引數是需要考慮的。
UniquenessRatio主要可以防止誤匹配,此引數對於最後的匹配結果是有很大的影響。立體匹配中,寧願區域無法匹配,也不要誤匹配。如果有誤匹配的話,碰到障礙檢測這種應用,就會很麻煩。該引數不能為負值,一般5-15左右的值比較合適,int型。
BlockSize:SAD視窗大小,容許範圍是[5,255],一般應該在 5x5..21x21 之間,引數必須為奇數值, int型。
NumDisparities:視差視窗,即最大視差值與最小視差值之差,視窗大小必須是 16的整數倍,int型。
在SGBM演算法的引數中,對視差生成效果影響較大的主要引數是BlockSize、NumDisparities和UniquenessRatio三個,一般只需對這三個引數進行調整,其餘引數按預設設定即可。
具體實現程式碼如下:(SGBM演算法匹配效果較好,但是時間較長,程式執行時請耐心等待!)

#include <opencv2/opencv.hpp>  
#include <iostream>  
#include <stdio.h>

using namespace std;
using namespace cv;

const int imageWidth = 1920;                             //攝像頭的解析度  
const int imageHeight = 1024;
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;    //是否選擇物件


Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);

/*
事先標定好的相機的引數
fx 0 cx
0 fy cy
0 0  1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 4334.09568, 0, 959.50000,
    0, 4334.09568, 511.50000,
    0, 0, 1.0);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.0, 0.0,0.0, 0.0, 0.0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 4489.55770, 0, 801.86552,
    0, 4507.13412, 530.72579,
    0, 0, 1.0);
Mat distCoeffR = (Mat_<double>(5, 1) << 0.0, 0.0, 0.0, 0.0, 0.0);

Mat T = (Mat_<double>(3, 1) << -518.97666, 01.20629,9.14632);//T平移向量
Mat rec = (Mat_<double>(3, 1) <<0.04345, -0.05236, -0.01810);//rec旋轉向量
Mat R;//R 旋轉矩陣


static void saveXYZ(const char* filename, const Mat& mat)
{
    const double max_z = 16.0e4;
    FILE* fp = fopen(filename, "wt");
    printf("%d %d \n", mat.rows, mat.cols);
    for (int y = 0; y < mat.rows; y++)
    {
        for (int x = 0; x < mat.cols; x++)
        {
            Vec3f point = mat.at<Vec3f>(y, x);
            if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);

        }
    }
    fclose(fp);
}

/*給深度圖上色*/
void GenerateFalseMap(cv::Mat &src, cv::Mat &disp)
{
    // color map  
    float max_val = 255.0f;
    float map[8][4] = { { 0,0,0,114 },{ 0,0,1,185 },{ 1,0,0,114 },{ 1,0,1,174 },
    { 0,1,0,114 },{ 0,1,1,185 },{ 1,1,0,114 },{ 1,1,1,0 } };
    float sum = 0;
    for (int i = 0; i<8; i++)
        sum += map[i][3];

    float weights[8]; // relative   weights  
    float cumsum[8];  // cumulative weights  
    cumsum[0] = 0;
    for (int i = 0; i<7; i++) {
        weights[i] = sum / map[i][3];
        cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
    }

    int height_ = src.rows;
    int width_ = src.cols;
    // for all pixels do  
    for (int v = 0; v<height_; v++) {
        for (int u = 0; u<width_; u++) {

            // get normalized value  
            float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

            // find bin  
            int i;
            for (i = 0; i<7; i++)
                if (val<cumsum[i + 1])
                    break;

            // compute red/green/blue values  
            float   w = 1.0 - (val - cumsum[i])*weights[i];
            uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
            uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
            uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);
            //rgb記憶體連續存放  
            disp.data[v*width_ * 3 + 3 * u + 0] = b;
            disp.data[v*width_ * 3 + 3 * u + 1] = g;
            disp.data[v*width_ * 3 + 3 * u + 2] = r;
        }
    }
}

      /*****立體匹配*****/
void stereo_match(int, void*)
{
    sgbm->setPreFilterCap(63);
    int sgbmWinSize =  5;//根據實際情況自己設定
    int NumDisparities = 416;//根據實際情況自己設定
    int UniquenessRatio = 6;//根據實際情況自己設定
    sgbm->setBlockSize(sgbmWinSize);
    int cn = rectifyImageL.channels();

    sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(NumDisparities);
    sgbm->setUniquenessRatio(UniquenessRatio);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(10);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(StereoSGBM::MODE_SGBM);
    Mat disp, dispf, disp8;
    sgbm->compute(rectifyImageL, rectifyImageR, disp);
    //去黑邊
    Mat img1p, img2p;
    copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
    copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
    dispf = disp.colRange(NumDisparities, img2p.cols- NumDisparities);

    dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
    reprojectImageTo3D(dispf, xyz, Q, true); //在實際求距離時,ReprojectTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16),才能得到正確的三維座標資訊。
    xyz = xyz * 16;
    imshow("disparity", disp8);
    Mat color(dispf.size(), CV_8UC3);
    GenerateFalseMap(disp8, color);//轉成彩圖
    imshow("disparity", color);
    saveXYZ("xyz.xls", xyz);
}



/*****描述:滑鼠操作回撥*****/
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;
        break;
    case EVENT_LBUTTONUP:    //滑鼠左按鈕釋放的事件
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
            break;
    }
}


/*****主函式*****/
int main()
{
    /*  立體校正    */
    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, Pl, imageSize, CV_16SC2, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

    /*  讀取圖片    */
    rgbImageL = imread("left_cor.bmp", CV_LOAD_IMAGE_COLOR);//CV_LOAD_IMAGE_COLOR
    rgbImageR = imread("right_cor.bmp", -1);


    /*  經過remap之後,左右相機的影象已經共面並且行對準了 */
    remap(rgbImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//INTER_LINEAR
    remap(rgbImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

    /*  把校正結果顯示出來*/

    //顯示在同一張圖上
    Mat canvas;
    double sf;
    int w, h;
    sf = 700. / 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(rectifyImageL, 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(rectifyImageR, 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);

    /*  立體匹配    */
    namedWindow("disparity", CV_WINDOW_NORMAL);
    //滑鼠響應函式setMouseCallback(視窗名稱, 滑鼠回撥函式, 傳給回撥函式的引數,一般取0)
    setMouseCallback("disparity", onMouse, 0);//disparity
    stereo_match(0, 0);

    waitKey(0);
    return 0;
}

效果圖如下:
這裡寫圖片描述
這裡寫圖片描述
可以看到廣告牌距離大概為10米,車身距離大概為7米多,符合實際。