前些日子做了一個關(guān)于雙目立體視覺的入門作業(yè)，現(xiàn)在在這里總結(jié)一下學(xué)到的一些知識（寫的可能會有很多欠缺的地方，還望海涵?。?
本篇博客不涉及雙目標(biāo)定的知識，關(guān)于雙目標(biāo)定網(wǎng)上資料很多，大家可以自行查找學(xué)習(xí)。
先說一下本博客的雙目立體視覺的實(shí)現(xiàn)基礎(chǔ)，已知以下信息：
（1）雙目采集圖像分辨率為1920X1024；

（2）雙目相機(jī)相對于虛擬焦平面的外參及各自內(nèi)參如下：

 左視相機(jī)：

 內(nèi)參：

 像元大小 = 5.86微米；
 焦距     = [ 4334.09568   4334.09568 ]（像素）;
 主點(diǎn)坐標(biāo) = [ 959.50000   511.50000 ]  （像素）;

 相對于虛擬焦平面的外參：

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

 右視相機(jī)：

 內(nèi)參：

 像元大小 = 5.86微米；
 焦距     = [ 4489.55770   4507.13412 ]（像素）;
 主點(diǎn)坐標(biāo) = [ 801.86552   530.72579 ]  （像素）;

 相對于虛擬焦平面的外參：

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

注：不考慮畸變校正；

通過以上信息，首先確定解決問題的技術(shù)思路如下圖：

由上圖可以看到在已知內(nèi)外參數(shù)的前提下，只需要讀取圖片直接進(jìn)行校正，接下來介紹關(guān)于雙目校正的知識。
雙目校正
要計算目標(biāo)點(diǎn)在左右兩個視圖上形成的視差，首先要把該點(diǎn)在左右視圖上兩個對應(yīng)的像點(diǎn)匹配起來。校正的目的是對左右兩幅圖像平面重投影，使得它們精確落在同一個平面上，而且圖像的行完全地對準(zhǔn)到前向平行的結(jié)構(gòu)上。
題目已經(jīng)給出了旋轉(zhuǎn)矩陣Ｒ和平移向量 T，立體校正Bouguet 算法能簡單地使左右圖像中的每幅重投影次數(shù)最小且重投影畸變最大，所以使立體匹配更加準(zhǔn)確和快速，并使左右圖像的觀測面積最大。
通過投影矩陣P把三維點(diǎn)變換成可以在平面上顯示的二維點(diǎn)
（1）
投影平面上的點(diǎn)坐標(biāo)為(x /w，y /w) 。同理，二維點(diǎn)也可通過重投影矩陣Q 重投影為三維點(diǎn)
（2）（3）
其中為主點(diǎn)在左圖像上的坐標(biāo)，f 為焦距，為雙目間距，為主點(diǎn)在右圖像的x坐標(biāo)。根據(jù)式(2)得到三維坐標(biāo)為: (X /W，Y/W，Z /W) 。在 OpenCV中可通過stereoRectify() 函數(shù)完成以上校正功能，該函數(shù)輸入?yún)?shù)為攝像機(jī)矩陣，畸變向量，左右旋轉(zhuǎn)矩陣Ｒ 和平移向量T。輸出參數(shù)有式 (1)中投影矩陣P，分別為和 ，以及重投影矩陣Q?？烧{(diào)用函數(shù)InitUndistortRectifyMap( ) 生成圖像校正所需的映射矩陣。最后經(jīng)過remap（）函數(shù)，使左右相機(jī)的圖像共面并且行對準(zhǔn)。效果如下圖：

立體匹配與生成深度信息
立體匹配完成匹配左右攝像機(jī)視圖的相同特征，并得到視差圖，視差值是匹配時相同特征點(diǎn)在x坐標(biāo)軸上的差值。得到視差圖后可通過三角相似的原理得到目標(biāo)物體的距離。
2.2.1立體成像原理
假設(shè)攝像機(jī)沒有畸變，左右攝像機(jī)的成像平面已經(jīng)嚴(yán)格對準(zhǔn)，左右主點(diǎn)已經(jīng)校準(zhǔn)，主光線也是平行的。理想立體攝像機(jī)模型如下圖:

設(shè)兩個攝像機(jī)分別移動到世界坐標(biāo)系的原點(diǎn)，可分別得到各自獨(dú)立的W點(diǎn)相對像平面點(diǎn)的X和Y坐標(biāo)式：

由式(6) 易知視差d和距離Z成反比，當(dāng)視差很小時，視差的變化對距離Z的影響較大; 當(dāng)視差較大時，視差的變化對距離Z的影響較小，因此，測距系統(tǒng)僅當(dāng)距離較近時精度較高。

立體匹配算法介紹
SGBM算法介紹
在OpenCV中使用函數(shù)StereoSGBM ( ) 實(shí)現(xiàn)了SGBM算法。SGBM 算法核心步驟為：選取匹配基元；構(gòu)建基于多個方向的掃描線的代價能量和函數(shù)；求取能量代價和函數(shù)的最優(yōu)解。OpenCV中SGMB算法的實(shí)現(xiàn)主要分為以下四個步驟：
①預(yù)處理
SGBM采用水平Sobel算子，把圖像做處理，然后用一個函數(shù)將經(jīng)過水平Sobel算子處理后的圖像上每個像素點(diǎn)（P表示其像素值）映射成一個新的圖像，表示新圖像上的像素值。映射函數(shù)如下：

preFilterCap為一個常數(shù)參數(shù)，openCv默認(rèn)取15。預(yù)處理實(shí)際上是得到圖像的梯度信息。經(jīng)預(yù)處理的圖像保存起來，將會用于計算代價。
②代價計算
代價有兩部分組成：經(jīng)過預(yù)處理得到的圖像的梯度信息經(jīng)過基于采樣的方法得到的梯度代價；原圖像經(jīng)過基于采樣的方法得到的SAD代價。
③動態(tài)規(guī)劃
用一維約束近似二維約束。在P的周圍，以 45°為間隔設(shè)置了8個路徑。通過8個路徑計算最小代價路徑，以此來近似二維約束匹配計算.
其中動態(tài)規(guī)劃很重要兩個參數(shù)P1，P2是這樣設(shè)定的：
P1 =8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
cn是圖像的通道數(shù), SADWindowSize是SAD窗口大小，數(shù)值為奇數(shù)。可以看出，當(dāng)圖像通道和SAD窗口確定下來，SGBM的規(guī)劃參數(shù)P1和P2是常數(shù)。
④后處理
openCvSGBM的后處理包含以下幾個步驟：
Step1：唯一性檢測：視差窗口范圍內(nèi)最低代價是次低代價的(1 + uniquenessRatio/100)倍時，最低代價對應(yīng)的視差值才是該像素點(diǎn)的視差，否則該像素點(diǎn)的視差為0。其中uniquenessRatio是一個常數(shù)參數(shù)。
Step2：亞像素插值
Step3：左右一致性檢測：誤差閾值disp12MaxDiff默認(rèn)為1，可以自己設(shè)置。

獲得深度信息
經(jīng)過sgbm->compute(rectifyImageL, rectifyImageR, disp)獲得視差映射后，利用式(2)，式(3) ，通過簡單的矩陣相乘就可提取深度信息。三維坐標(biāo)就是( X /W，Y/W，Z /W) 。OpenCV中使用reprojectImageTo3D( )函數(shù)實(shí)現(xiàn)該功能，該函數(shù)輸入上面得到的視差數(shù)據(jù)，輸出所需的三維點(diǎn)陣，然后提取深度信息。

SGBM參數(shù)設(shè)置：（在下面的程序中已經(jīng)標(biāo)明）
MinDisparity設(shè)置為0，因?yàn)閮蓚€攝像頭是前向平行放置，相同的物體在左圖中一定比在右圖中偏右。如果為了追求更大的雙目重合區(qū)域而將兩個攝像頭向內(nèi)偏轉(zhuǎn)的話，這個參數(shù)是需要考慮的。
UniquenessRatio主要可以防止誤匹配，此參數(shù)對于最后的匹配結(jié)果是有很大的影響。立體匹配中，寧愿區(qū)域無法匹配，也不要誤匹配。如果有誤匹配的話，碰到障礙檢測這種應(yīng)用，就會很麻煩。該參數(shù)不能為負(fù)值，一般5-15左右的值比較合適，int型。
BlockSize：SAD窗口大小，容許范圍是[5,255]，一般應(yīng)該在 5x5..21x21 之間，參數(shù)必須為奇數(shù)值, int型。
NumDisparities：視差窗口，即最大視差值與最小視差值之差,窗口大小必須是 16的整數(shù)倍，int型。
在SGBM算法的參數(shù)中，對視差生成效果影響較大的主要參數(shù)是BlockSize、NumDisparities和UniquenessRatio三個，一般只需對這三個參數(shù)進(jìn)行調(diào)整，其余參數(shù)按默認(rèn)設(shè)置即可。
具體實(shí)現(xiàn)代碼如下：（SGBM算法匹配效果較好，但是時間較長，程序運(yùn)行時請耐心等待?。?/p>

#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;//圖像校正之后，會對圖像進(jìn)行裁剪，這里的validROI就是指裁剪之后的區(qū)域  
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表  
Mat Rl, Rr, Pl, Pr, Q;              //校正旋轉(zhuǎn)矩陣R，投影矩陣P 重投影矩陣Q
Mat xyz;              //三維坐標(biāo)

Point origin;         //鼠標(biāo)按下的起始點(diǎn)
Rect selection;      //定義矩形選框
bool selectObject = false;    //是否選擇對象


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

/*
事先標(biāo)定好的相機(jī)的參數(shù)
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旋轉(zhuǎn)向量
Mat R;//R 旋轉(zhuǎn)矩陣


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內(nèi)存連續(xù)存放  
            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;//根據(jù)實(shí)際情況自己設(shè)定
    int NumDisparities = 416;//根據(jù)實(shí)際情況自己設(shè)定
    int UniquenessRatio = 6;//根據(jù)實(shí)際情況自己設(shè)定
    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); //在實(shí)際求距離時，ReprojectTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正確的三維坐標(biāo)信息。
    xyz = xyz * 16;
    imshow("disparity", disp8);
    Mat color(dispf.size(), CV_8UC3);
    GenerateFalseMap(disp8, color);//轉(zhuǎn)成彩圖
    imshow("disparity", color);
    saveXYZ("xyz.xls", xyz);
}



/*****描述：鼠標(biāo)操作回調(diào)*****/
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:   //鼠標(biāo)左按鈕按下的事件
        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:    //鼠標(biāo)左按鈕釋放的事件
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
            break;
    }
}


/*****主函數(shù)*****/
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);


    /*  經(jīng)過remap之后，左右相機(jī)的圖像已經(jīng)共面并且行對準(zhǔn)了 */
    remap(rgbImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//INTER_LINEAR
    remap(rgbImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

    /*  把校正結(jié)果顯示出來*/

    //顯示在同一張圖上
    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),                //獲得被截取的區(qū)域    
        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;

    //畫上對應(yīng)的線條
    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);
    //鼠標(biāo)響應(yīng)函數(shù)setMouseCallback(窗口名稱, 鼠標(biāo)回調(diào)函數(shù), 傳給回調(diào)函數(shù)的參數(shù)，一般取0)
    setMouseCallback("disparity", onMouse, 0);//disparity
    stereo_match(0, 0);

    waitKey(0);
    return 0;
}

效果圖如下：


可以看到廣告牌距離大概為10米，車身距離大概為7米多，符合實(shí)際。

源程序下載地址 （包含測試圖片）