第七節、雙目視覺之空間坐標計算

本文轉載自查看原文 2018-07-26 22:47 14553 OpenCV

在上一節我們已經介紹了如何對相機進行標定。然后獲取相機的內部參數，外部參數。

內參包括焦距、主點、傾斜系數、畸變系數：

$$M=\begin{bmatrix} f_x & γ & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{bmatrix}$$

其中$\gamma$為坐標軸傾斜參數，理想情況下為0。

外參包括旋轉矩陣$R$、平移向量$T$，它們共同描述了如何把點從世界坐標系轉換到攝像機坐標系，旋轉矩陣描述了世界坐標系的坐標軸相對於攝像機坐標軸的方向，平移向量描述了在攝像機坐標系下空間原點的位置。

人類可以看到3維立體的世界，是因為人的兩只眼睛，從不同的方向看世界，兩只眼睛中的圖像的視差，讓我們可以看到三維立體的世界。類似的，要想讓計算機“看到”三維世界，就需要使用至少兩個攝像頭構成雙目立體視覺系統。

這里我們想要通過獲取三維空間物體的坐標有兩種方法，下面會介紹。

一、自己動手實現(只進行圖片矯正、不進行立體校正)

1、標定雙目后，首先要根據其畸變系數來矯正原圖，這里用來去除圖片的畸變：

        cv::Mat mapx = cv::Mat(imageSize, CV_32FC1);
        cv::Mat mapy = cv::Mat(imageSize, CV_32FC1);
        cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
        std::cout << "顯示矯正圖像" << endl;
        for (int i = 0; i != imageCount; i++)
        {
            std::cout << "Frame #" << i + 1 << "..." << endl;
            //計算圖片畸變矯正的映射矩陣mapx、mapy(不進行立體校正，立體校正需要利用雙攝)
            initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
            //讀取一張圖片
            Mat imageSource = imread(filenames[i]);
            Mat newimage = imageSource.clone();
            //另一種不需要轉換矩陣的方式
            //undistort(imageSource,newimage,cameraMatrix,distCoeffs);
            //進行校正
            remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
            imshow("原始圖像", imageSource);
            imshow("矯正后圖像", newimage);
            waitKey();
        }

2、坐標計算

上一節我們介紹了從世界坐標系到像素坐標系的轉換：

其中${\begin{bmatrix} u & v & 1 \end{bmatrix}}^T$為矯正后的圖像中的點在像素坐標系中的坐標，${\begin{bmatrix} x_w & y_w & z_w & 1 \end{bmatrix}}^T$為點在世界坐標系中的坐標。

現在我們來研究一下從像素坐標系到世界坐標系的轉換：

光軸會聚模型(兩個攝像機的像平面不是平行的)：

對於左右相機分別有：

將上面兩式整理可以得到：

采用最小二乘法求解$X$,$Y$,$Z$，在opencv中可以用solve(A,B,XYZ,DECOMP_SVD)求解：

代碼如下(來源於網上)：

//opencv2.4.9 vs2012
#include <opencv2\opencv.hpp>
#include <fstream>
#include <iostream>
 
using namespace std;
using namespace cv;
 
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);
 
//圖片對數量
int PicNum = 14;
 
//左相機內參數矩陣
float leftIntrinsic[3][3] = {4037.82450,             0,        947.65449,
                                      0,    3969.79038,        455.48718,
                                      0,             0,                1};
//左相機畸變系數
float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};
//左相機旋轉矩陣
float leftRotation[3][3] = {0.912333,        -0.211508,         0.350590, 
                            0.023249,        -0.828105,        -0.560091, 
                            0.408789,         0.519140,        -0.750590};
//左相機平移向量
float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};
 
//右相機內參數矩陣
float rightIntrinsic[3][3] = {3765.83307,             0,        339.31958,
                                        0,    3808.08469,        660.05543,
                                        0,             0,                1};
//右相機畸變系數
float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};
//右相機旋轉矩陣
float rightRotation[3][3] = {-0.134947,         0.989568,        -0.050442, 
                              0.752355,         0.069205,        -0.655113, 
                             -0.644788,        -0.126356,        -0.753845};
//右相機平移向量
float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};
 
 
int main()
{
    //已知空間坐標求像素坐標
    Point3f point(700,220,530);
    cout<<"左相機中坐標："<<endl;
    cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;
    cout<<"右相機中坐標："<<endl;
    cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;
 
    //已知左右相機像素坐標求空間坐標
    Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);
    Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);
    Point3f worldPoint;
    worldPoint = uv2xyz(l,r);
    cout<<"空間坐標為:"<<endl<<uv2xyz(l,r)<<endl;
 
    system("pause");
 
    return 0;
}
 
 
//************************************
// Description: 根據左右相機中像素坐標求解空間坐標
// Method:    uv2xyz
// FullName:  uv2xyz
// Access:    public 
// Parameter: Point2f uvLeft
// Parameter: Point2f uvRight
// Returns:   cv::Point3f
// Author:    小白
// Date:      2017/01/10
// History:
//************************************
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)
{
    //     [u1]      [xw]                      [u2]      [xw]
    //zc1 *|v1| = Pl*[yw]                  zc2*|v2| = P2*[yw]
    //     [ 1]      [zw]                      [ 1]      [zw]
    //               [1 ]                                [1 ]
    Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
    Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
    Mat mLeftRT = Mat(3,4,CV_32F);//左相機RT矩陣
    hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
    Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
    Mat mLeftP = mLeftIntrinsic * mLeftRT;
    //cout<<"左相機P矩陣 = "<<endl<<mLeftP<<endl;
 
    Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
    Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
    Mat mRightRT = Mat(3,4,CV_32F);//右相機RT矩陣
    hconcat(mRightRotation,mRightTranslation,mRightRT);
    Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
    Mat mRightP = mRightIntrinsic * mRightRT;
    //cout<<"右相機P矩陣 = "<<endl<<mRightP<<endl;
 
    //最小二乘法A矩陣
    Mat A = Mat(4,3,CV_32F);
    A.at<float>(0,0) = uvLeft.x * mLeftP.at<float>(2,0) - mLeftP.at<float>(0,0);
    A.at<float>(0,1) = uvLeft.x * mLeftP.at<float>(2,1) - mLeftP.at<float>(0,1);
    A.at<float>(0,2) = uvLeft.x * mLeftP.at<float>(2,2) - mLeftP.at<float>(0,2);
 
    A.at<float>(1,0) = uvLeft.y * mLeftP.at<float>(2,0) - mLeftP.at<float>(1,0);
    A.at<float>(1,1) = uvLeft.y * mLeftP.at<float>(2,1) - mLeftP.at<float>(1,1);
    A.at<float>(1,2) = uvLeft.y * mLeftP.at<float>(2,2) - mLeftP.at<float>(1,2);
 
    A.at<float>(2,0) = uvRight.x * mRightP.at<float>(2,0) - mRightP.at<float>(0,0);
    A.at<float>(2,1) = uvRight.x * mRightP.at<float>(2,1) - mRightP.at<float>(0,1);
    A.at<float>(2,2) = uvRight.x * mRightP.at<float>(2,2) - mRightP.at<float>(0,2);
 
    A.at<float>(3,0) = uvRight.y * mRightP.at<float>(2,0) - mRightP.at<float>(1,0);
    A.at<float>(3,1) = uvRight.y * mRightP.at<float>(2,1) - mRightP.at<float>(1,1);
    A.at<float>(3,2) = uvRight.y * mRightP.at<float>(2,2) - mRightP.at<float>(1,2);
 
    //最小二乘法B矩陣
    Mat B = Mat(4,1,CV_32F);
    B.at<float>(0,0) = mLeftP.at<float>(0,3) - uvLeft.x * mLeftP.at<float>(2,3);
    B.at<float>(1,0) = mLeftP.at<float>(1,3) - uvLeft.y * mLeftP.at<float>(2,3);
    B.at<float>(2,0) = mRightP.at<float>(0,3) - uvRight.x * mRightP.at<float>(2,3);
    B.at<float>(3,0) = mRightP.at<float>(1,3) - uvRight.y * mRightP.at<float>(2,3);
 
    Mat XYZ = Mat(3,1,CV_32F);
    //采用SVD最小二乘法求解XYZ
    solve(A,B,XYZ,DECOMP_SVD);
 
    //cout<<"空間坐標為 = "<<endl<<XYZ<<endl;
 
    //世界坐標系中坐標
    Point3f world;
    world.x = XYZ.at<float>(0,0);
    world.y = XYZ.at<float>(1,0);
    world.z = XYZ.at<float>(2,0);
 
    return world;
}
 
//************************************
// Description: 將世界坐標系中的點投影到左右相機像素坐標系中
// Method:    xyz2uv
// FullName:  xyz2uv
// Access:    public 
// Parameter: Point3f worldPoint
// Parameter: float intrinsic[3][3]
// Parameter: float translation[1][3]
// Parameter: float rotation[3][3]
// Returns:   cv::Point2f
// Author:    小白
// Date:      2017/01/10
// History:
//************************************
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])
{
    //    [fx γ u0]                            [xc]        [xw]        [u]       [xc]
    //M = |0 fy v0|       TEMP = [R T]         |yc| = TEMP*[yw]     zc*[v] =   M*[yc]
    //    [ 0 0 1 ]                            [zc]        [zw]        [1]       [zc]
    //                                                     [1 ]
    Point3f c;
    c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;
    c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;
    c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;
 
    Point2f uv;
    uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z;
    uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z;
 
    return uv;
}

上面的代碼看起來可以實現從像素坐標到三維空間坐標的轉換。但是實際上會存在一個問題，假設世界坐標系中的點在左像平面的投影坐標為p1，你如何找到該點在右像平面的投影坐標p2，雖然現在有很多圖片匹配算法比如SDA,BM,SGBM等，但是如果我們直接在右像平面的二維空間中逐個像素的取搜索匹配點，這個效率是非常低的。因此我們提出了利用極點約束，實現把二維空間匹配降維到一維匹配，這樣就加快了運算的速度，這也是我下面將要介紹到的內容立體校正。

二、利用OpenCV庫實現(進行圖片矯正、立體校正)

假設我們已經完成了對兩個攝像頭的單目標定，並且得到了左右攝像攝像機坐標系相對同一世界坐標系的旋轉矩陣和平移矩陣，然后我們就可以獲取兩個攝像頭之間的相對位置關系，這也就是我們經常所說的雙目標定（或者叫雙目立體標定）。

1、雙目立體標定

雙目攝像機需要標定的參數：攝像機內參數矩陣$M$，畸變系數矩陣，本征矩陣$E$，基礎矩陣$F$，旋轉矩陣$R$以及平移矩陣$T$（其中攝像機內參數矩陣和畸變系數矩陣可以通過單目標定的方法標定出來）。

雙目攝像機標定和單目攝像機標定最主要的區別就是雙目攝像機需要標定出左右攝像機坐標系之間的相對關系。

我們用旋轉矩陣$R$和平移矩陣$T$來描述左右兩個攝像機坐標系的相對關系，具體為：在左相機上建立世界坐標系。

假設空間中有一點$P$，其在世界坐標系下的坐標為$P_w$, $P_{W}$

$P_{W}$

綜合上式，可以推得：

$$R=R_rR_l^{-1}=R_rR_l^T$$

$$T=T_r-R_rR_l^{-1}T_l$$

其中$R_l$,$T_l$為左攝像頭經過單目標定得到的相對標定物的旋轉矩陣和平移向量，$R_r$,$T_r$為右攝像頭經過單目標定得到的相對標定物的旋轉矩陣和平移向量。

左右相機分別進行單目標定，就可以分別$R_l$,$T_l$，$R_r$,$T_r$。帶入上式就可以求出左右相機之間的旋轉矩陣$R$和平移$T$。

注意：因為旋轉矩陣$R$、$R_l$、$R_r$均是單位正交矩陣。正交矩陣的逆（inverse）等於正交矩陣的轉置（transpose）。

單目攝像機需要標定的參數，雙目都需要標定，雙目攝像機比單目攝像機多標定的參數：$R$和$T$，主要是描述兩個攝像機相對位置關系的參數，這些參數在立體校正和對極幾何中用處很大,那么得到了立體標定的結果，下一步我們就該進行立體校正。

2、立體校正

在介紹立體校正的具體方法之前，讓我們來看一下，為什么要進行立體校正？

雙目攝像機系統主要的任務就是測距，而視差求距離公式是在雙目系統處於理想情況下推導的，但是在現實的雙目立體視覺系統中，是不存在完全的共面行對准的兩個攝像機圖像平面的。所以我們要進行立體校正。立體校正的目的就是，把實際中消除畸變后的非共面行對准的兩幅圖像，校正成共面行對准。（共面行對准：兩攝像機圖像平面在同一平面上，且同一點投影到兩個攝像機圖像平面時，兩個像素坐標系的同一行，這樣圖片匹配時速度就會有很大的提升）,將實際的雙目系統校正為理想的雙目系統。

理想雙目系統：兩攝像機圖像平面平行，光軸和圖像平面垂直，極點處於無窮遠處，此時點$(x_0,y_0)$對應的級線就是$y=y_0$。

立體校正前：

立體校正后：

3、Bouguet校正原理

上面兩張圖中可以看到，立體校正前的左右相機的光心並不是平行的，兩個光心的連線就叫基線，像平面與基線的交點就是極點，像點與極點所在的直線就是極線，左右極線與基線構成的平面就是空間點對應的極平面。

校正后，極點在無窮遠處，兩個相機的光軸平行。像點在左右圖像上的高度一致。這也就是極線校正的目標。校正后做后續的立體匹配時，只需在同一行上搜索左右像平面的匹配點即可，能使效率大大提高。

Bouguet的方法，是將旋轉矩陣$R$和平移矩陣$T$分解成左右相機各旋轉一半的旋轉和平移矩陣$R_l,T_l,R_r,T_r$。分解的原則是使得，左右圖像重投影造成的畸變最小，左右視圖的共同面積最大。

1、將右圖像平面相對於左圖像平面的旋轉矩陣分解成兩個矩陣$R_l$和$R_r$，叫做左右相機的合成旋轉矩陣。

$$R_l=R^{\frac{1}{2}}$$

$$R_r=R^{-\frac{1}{2}}$$

其中$R^{\frac{1}{2}}R^{\frac{1}{2}}=R$,$R^{-\frac{1}{2}}$是$R^{\frac{1}{2}}$的逆矩陣。

2、將左右相機各旋轉一半，使得左右相機的光軸平行。此時左右相機的成像面達到平行，但是基線與成像平面不平行。

3、構造變換矩矩陣$R_{rect}$使得基線與成像平面平行。構造的方法是通過右相機相對於左相機的偏移矩陣$T$完成的。

構造$e_1$。變換矩陣將左視圖的極點變換到無窮遠處，則使極線達到水平，可見，左右相機的投影中心之間的平移向量就是左極點方向：

$$e_1=\frac{T}{\| T\|}，T={\begin{bmatrix} T_x & T_y & T_z\end{bmatrix}}^T$$

$e_2$方向與主光軸方向正交，沿圖像方向，與$e_1$垂直，則知$e_2$方向可通過$e_1$與主光軸方向的叉積並歸一化獲得：

$$e_2=\frac{\begin{bmatrix}-T_y & T_x & 0 \end{bmatrix}}{\sqrt{T_x^2+T_y^2}}$$

獲取了$e_1$與$e_2$后，$e_3$與$e_1$和$e_2$正交，$e_1$自然就是他們兩個的叉積：

$$e_3=e_1\times{e_2}$$

則可將左相機的極點轉換到無窮遠處的矩陣$R_{rect}$,如下：

$$R_{rect}=\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix}$$

通過合成旋轉矩陣與變換矩陣相乘獲得左右相機的整體旋轉矩陣。左右相機坐標系乘以各自的整體旋轉矩陣就可使得左右相機的主光軸平行，且像平面與基線平行。

$$R_l'=R_{rect}R_l，R_r'=R_{rect}R_r$$

通過上述的兩個整體旋轉矩陣，就能夠得到理想的平行配置的雙目立體系圖像。校正后根據需要對圖像進行裁剪，需重新選擇一個圖像中心，和圖像邊緣從而讓左、右疊加部分最大。

4、stereoCalibrate 函數

OpenCV提供了函數cvStereoCalibrate，用來進行雙目標定，直接通過stereoCalibrate來實現雙目定標，很容易產生比較大的圖像畸變，邊角處的變形較厲害。最好先通過calibrateCamera() 對每個攝像頭單獨進行定標，再利用stereoCalibrate進行雙目定標。這樣定標所得參數才比較准確，隨后的校正也不會有明顯的畸變。(注意：opencv提供的立體標定函數穩定性不太好，而且精度相對來言也不准，推薦使用matlab的立體標定)

/*
    進行立體雙目標定
    由於左右攝像機分別都經過了單目標定
    所以在此處選擇flag = CALIB_USE_INTRINSIC_GUESS
    */
    double rms = stereoCalibrate(objRealPoint,   //vector<vector<point3f>> 型的數據結構，存儲標定角點在世界坐標系中的位置
        imagePointL,                             //vector<vector<point2f>> 型的數據結構，存儲標定角點在第一個攝像機下的投影后的亞像素坐標
        imagePointR,                             //vector<vector<point2f>> 型的數據結構，存儲標定角點在第二個攝像機下的投影后的亞像素坐標
        cameraMatrixL,                           //輸入/輸出型的第一個攝像機的相機矩陣。如果CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO ,CV_CALIB_FIX_INTRINSIC , or CV_CALIB_FIX_FOCAL_LENGTH其中的一個或多個標志被設置，該攝像機矩陣的一些或全部參數需要被初始化
        distCoeffL,                              //第一個攝像機的輸入/輸出型畸變向量。根據矯正模型的不同，輸出向量長度由標志決定
        cameraMatrixR,                           //輸入/輸出型的第二個攝像機的相機矩陣。參數意義同第一個相機矩陣相似
        distCoeffR,                              //第一個攝像機的輸入/輸出型畸變向量。根據矯正模型的不同，輸出向量長度由標志決定
        Size(imageWidth, imageHeight),           //圖像的大小
        R,                                       //輸出型，第一和第二個攝像機之間的旋轉矩陣
        T,                                       //輸出型，第一和第二個攝像機之間的平移矩陣
        E,                                       //輸出本征矩陣
        F,                                       //輸出基礎矩陣
        CALIB_USE_INTRINSIC_GUESS,
        TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));

上面的cameraMatrixL($R$)，distCoeffL($R$) 分別是單目定標后獲得的左（右）攝像頭的內參矩陣(3*3）和畸變參數向量（1*5或者5*1）；$R$, $T$ 分別是雙目立體標定獲取的右攝像頭相對於左攝像頭的旋轉矩陣（3*3）和平移向量（3*1）, $E$是包含了兩個攝像頭相對位置關系的本征矩陣Essential Matrix（3*3），$F$ 則是既包含兩個攝像頭相對位置關系、也包含攝像頭各自內參信息的基礎矩陣（3*3）。

對極幾何在雙目問題中非常的重要，可以簡化立體匹配等問題，而要應用對極幾何去解決問題，比如求極線，需要知道本征矩陣或者基礎矩陣，因此雙目標定過程中也會把本征矩陣和基礎矩陣算出來。

stereoCalibrate 是怎樣計算 Essential Matrix 和 Fundamental Matrix 的？

（1）Essential Matrix

本征矩陣常用字母$E$來表示，其物理意義是左右圖像坐標系相互轉換的矩陣，描述的是同一空間點投影在左右攝像機圖像平面上對應點之間的關系，單位為mm。

給定一個目標點$P$，以左攝像頭光心$O_l$為世界坐標系原點，其在世界坐標系下的坐標為$P_w$，其在左右攝像機坐標系下的坐標分別為$P_{cl}$和$P_{cr}$，右攝像機坐標系原點在左攝像機坐標系的坐標為${\begin{bmatrix}T_x & T_y & T_z\end{bmatrix}}^T$,則有：

$$P_{cr}=R(P_{cl}-T)$$

變換得：

$$P_{cl}-T=R^{-1}P_{cr}$$

$$(P_{cl}-T)^T=(R^{-1}P_{cr})^T=(R^TP_{cr})^T$$

則通過點$T$的所有點所組成的平面（即極面）可以用下式表示：

向量的叉積又可表示為矩陣與向量的乘積：

注意：上面的運算符號$\times$表示向量之間的叉乘運算,運算符號.表示兩個向量的點乘。

其中$S$：

綜合上式可得：

乘積$RS$即為本征矩陣$E$，令$E=RS$，得到：

描述了同一物理點在左右攝像機圖像平面上投影在相機坐標系下的關系。

（2）Fundamental Matrix

由於矩陣E並不包含攝像頭內參信息，且E是面向攝像頭坐標系的。實際上我們更感興趣的是在像素坐標系上去研究一個像素點在另一視圖上的對極線，這就需要用到攝像機的內參信息將攝像機坐標系和像素坐標系聯系起來。我們定義基礎矩陣F，描述同一物理點在左右攝像機像素坐標系下的關系，單位為pix。

像素坐標系與攝相機坐標系的關系：

假設左右攝像機坐標系下點$P_{cl}$、$P_{cr}$在像素坐標系中對應的坐標分別為$ P_{pixl}$、$P_{pixr}$，我們可以得到：

由此可將基礎矩陣$F$定義為：

最終得到同一物理點在左右攝像機像素坐標系下的關系：

5、 stereoRectify函數

介紹完了stereoCalibrate 函數我們再來看看stereoRectify函數，這個函數是用來雙目校正的，主要包括畸變矯正、立體校正兩部分。

如上圖所示：雙目校正是根據攝像頭定標后獲得的單目內參數據（焦距、成像原點、畸變系數）和雙目相對位置關系（旋轉矩陣和平移向量），分別對左右視圖進行消除畸變和行對准，使得左右視圖的成像原點坐標一致（CV_CALIB_ZERO_DISPARITY 標志位設置時發生作用）、兩攝像頭光軸平行、左右成像平面共面、對極線行對齊。在OpenCV2.1版之前，cvStereoRectify 的主要工作就是完成上述操作，校正后的顯示效果如上圖(c) 所示。可以看到校正后左右視圖的邊角區域是不規則的，而且對后續的雙目匹配求取視差會產生影響，因為這些邊角區域也參與到匹配操作中，其對應的視差值是無用的、而且一般數值比較大，在三維重建和機器人避障導航等應用中會產生不利影響。

因此，OpenCV2.1 版之后stereoRectify新增了4個參數用於調整雙目校正后圖像的顯示效果，分別是 double alpha, CvSize newImgSize, CvRect* roi1, CvRect* roi2。

CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1,
                                 InputArray cameraMatrix2, InputArray distCoeffs2,
                                 Size imageSize, InputArray R, InputArray T,
                                 OutputArray R1, OutputArray R2,
                                 OutputArray P1, OutputArray P2,
                                 OutputArray Q, int flags = CALIB_ZERO_DISPARITY,
                                 double alpha = -1, Size newImageSize = Size(),
                                 CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 );

下面簡要介紹這4個參數的作用：

（1）newImgSize：校正后remap圖像的分辨率。如果輸入為(0,0)，則是與原圖像大小一致。對於圖像畸變系數比較大的，可以把newImgSize 設得大一些，以保留圖像細節(你可以把畸變矯正的過程想象成和圖片旋轉類似，在圖片旋轉中可能會出現邊角超出當前圖像，因此為了保留所有的圖片信息，我們可以利用公式計算出旋轉之后的面積，然后再進行旋轉變換，這里畸變矯正也可能存在同樣的問題，但是這個大小並不好計算，因此我們可以人為的設置大一些)。

（2）alpha：圖像剪裁系數，取值范圍是-1、0~1。當取值為 0 時，OpenCV會對校正后的圖像進行縮放和平移，使得remap圖像只顯示有效像素（即去除不規則的邊角區域），如下圖，適用於機器人避障導航等應用；

當alpha取值為1時，remap圖像將顯示所有原圖像中包含的像素，該取值適用於畸變系數極少的高端攝像頭；alpha取值在0-1之間時，OpenCV按對應比例保留原圖像的邊角區域像素。Alpha取值為-1時，OpenCV自動進行縮放和平移，其顯示效果如圖所示。

（3）roi1, roi2：用於標記remap圖像中包含有效像素的矩形區域。

上面還有幾個參數需要介紹一下，$R1$和$R2$分別為左右相機消除畸變后的像平面投影到公共像平面的旋轉矩陣,也就是我們之前介紹的Bouguet校正原理中求解得到的$R_l'$和$R_r'$矩陣。左相機經過$R1$旋轉，右相機經過$R2$旋轉之后，兩幅圖像就已經共面並且行對准了。

$P1$,$P2$分別為左右相機的投影矩陣，其作用是將世界坐標系的點轉換到像素坐標系下(左相機光心為世界坐標系原點):

$Q$矩陣為重投影矩陣，即矩陣$Q$可以把像素坐標系下的點轉換到世界坐標系下:

其中$d$為左右兩幅圖像的視差,三維坐標為$(X_w/W,Y_w/W,Z_w/W)$。下面我們來研究一下如何求解$Q$矩陣：

經過雙目相機標定和校准后，雙目相機的主光軸到達平行，如圖所示是雙目相機模型，世界坐標系中的任意一點都滿足，該點與它在左右相機的成像點在同一個極平面上。$OL$和$OR$是左右相機的的光心，長為$L$的兩條線段（端點為藍色星星的線段）表示的是左右相機的像面。則光心到像面的最短距離就是焦距長度f(物理單位),兩個相機的焦距f要求設置為一樣的。若$P(X_w,Y_w,Z_w)$是世界坐標系中的一點，它在左右像面上的成像點是$P_L$和$P_R$。$P_L$和$P_R$距各自像面的左邊緣的物理距離是$X_L$和$X_R$。視差就是$X_R-X_L$或者是$X_L-X_R$。標定和匹配后$f$，$b$，$X_R$，$X_L$都能夠得到，那么物體的景深$Z$是多少呢？它和物體的視差有什么關系呢？

三角行PL-PR-P相似於三角形OL-OR-P，則有比例關系：

由於上面的$X_R$,$X_L$的單位均是物理單位，而我們想使用像素單位去表示，我們在上一節中介紹從圖像坐標系轉到像素坐標系時說到$dx$,$dy$,分別表示每個像素在橫軸$x$和縱軸$y$上的物理尺寸，則上式變為：

$u_R$,$u_L$分別表示$P_L$、$P_R$距離各自像平面左邊緣的像素距離。$f_x$是我們通過相機標定的內參值。

我們定義視差$d=u_L-u_R$($d$和$f_x$的單位都是像素，兩個正好消去，$Z_w$的單位和$b$一樣，都是物理單位)，所以有：

從上式我們知道距離攝像機越遠的物體，視差值越小，越近的物體，視差值越大，這也符合我們人眼的規律。

現在我們已經求出來$Z_w$的值，再來看看$X_w$,$Y_w$如何求解：

同樣利用三角形相似定理，我們可以得到：

當$f_x=f_y$時，我們得到$X_w$,$Y_w$的坐標為：

其中$u$,$v$表示像素坐標系下的點的坐標，$u_0$、$v_0$為左相機圖像平面的原點在像素坐標系下的坐標值。

我們$X_w$,$Y_w$,$Z_w$坐標聯立在一塊得到：

空間中某點的三維坐標就是$X_w=X/W$, $Y_w=Y/W,$ $Z_w=Z/W$。

但是OpenCV中轉換公式如下：

這里$T_x$就是我們上面的$b$，即兩個相機光心的中心距(通過立體標定得出的$T$的向量的第一個分量$T_x$的絕對值就是左右攝像頭的中心距，你回到上面推導一翻，你會發現$T_x$一般為負數，因此$b=-T_x$)，$u_0$、$u_0'$分別為左右相機圖像平面的原點水平分量在像素坐標系下的坐標值，實際上兩個數值的差值很小，近似為0。如果我們忽略Q的最后一項，我們可以看到這個和我們自己求得近似一樣。

6、initUndistortRectifyMap函數

根據相機單目標定得到的內參以及stereoRectify 計算出來的$R$ 和 $P$ 來計算畸變矯正和立體校正的映射變換矩陣 mapx,mapy。

    /*    
    根據stereoRectify 計算出來的R 和 P 來計算畸變矯正和立體校正的映射變換矩陣 mapx,mapy
    mapx,mapy這兩個映射表接下來可以給remap()函數調用，來校正圖像，使得兩幅圖像共面並且行對准
    */
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

三立體匹配算法

上面我們說到把像素坐標系下的一點轉換到世界坐標系需要知道該點的視差值，但是如何求視差，一直是雙目視覺的研究熱點。雙目相機拍攝同一場景的左、右兩幅視點圖像，運用立體匹配匹配算法獲取視差圖，進而獲取深度圖。而深度圖的應用范圍非常廣泛，由於其能夠記錄場景中物體距離攝像機的距離，可以用以測量、三維重建、以及虛擬視點的合成等。在這里我不去敘述立體匹配算法，有興趣的童鞋可以去看一看SAD匹配算法、BM算法、SGBM算法、GC算法等。

以SGBM算法為例，SGBM的基本步驟涉及：預處理、代價計算、動態規划以及后處理：

四程序講解

講完了原理，我們怎么能少了代碼呢。opencv提供了案例程序和數據，該數據可以在opencv的安裝路徑\opencv\sources\samples\cpp下找到：

calibration.cpp：相機單目標定；
stereo_calib.cpp：相機雙目標定；
stereo_match.cpp：相機立體匹配；

有興趣的話可以去閱讀下源代碼，這里我介紹一個可視化雙目測距的程序：https://github.com/AngelaViVi/Evision，這里包含了MFC程序和QT版本的程序。

1、標准數據集測試

該程序需要先加載標定圖片，然后進行單目標定，雙目標定；標定完之后，我們加載左右相機拍攝到的圖片，進行測距。下面我們以程序鏈接中自帶測試圖片為例：

先利用MFC版本程序進行相機標定，然后立體匹配，效果如下圖：

上面兩幅圖為左右相機原始圖片經過畸變校正、雙目校正得到的圖像，下面一副圖片是通過對上面兩幅利用BM匹配算法得到的左視差圖。

在圖像匹配時，其中有三個參數比較重要，分別是uniquenessRatio，numberOfDisparities，windowSize。

1、我們嘗試修改uniquenessRatio參數分別為5，15，25，結果如下：

uniquenessRatio主要可以防止誤匹配，其主要作用從上面三幅圖的視差效果比對就可以看出。在立體匹配中，我們寧願區域無法匹配，也不要誤匹配。如果有誤匹配的話，碰到障礙檢測這種應用，就會很麻煩。

2、我們嘗試修改windowSize參數分別為5,25,51,結果如下：

windowSize表示SAD窗口大小，也就是匹配代價計算的窗口越小，容許范圍是[5,255]，一般應該在 5x5 至 21x21 之間，參數必須是奇數。windowSize越大，視差圖越平滑；太大的size容易導致過平滑，並且誤匹配增多，體現在視差圖中空洞增多。

3.我們嘗試修改numberOfDisparities參數為16、32、64、128、256，結構如下：

numberOfDisparities表示視差窗口，即最大視差值與最小視差值之差, 窗口大小必須是 16 的整數倍，我們上面設置的默認最小視差為0，那么最大視差就為0+numberOfDisparities。從上面可以看到該值的影響很大。

下面我們看一下，測距效果，我們點擊深度重建，這里我們利用二值化把前景和背景分離，然后利用 findContours()函數查找視差圖的輪廓，並用最小矩形圈出來，效果如下：

我們可以看到當前兩點的距離為125mm，大概是12.5cm，書本距離我們0.76m。這個距離還是比較准確地。

2、basler相機數據集

我利用實驗室的兩個balser相機拍攝了幾十張用來標定的圖片如下：

然后利用程序進行單目標定，雙目標定，把相關參數保存在calib_paras.xml文件中。

下面我們拍攝了不少用於匹配的圖片，然后我們挑選兩幅圖片利用BM算法進行匹配：

從上面兩幅圖像我們可以看到極線已經校正成功了，在同一條線上，我們可以在右圖中找到與左圖相對應得匹配點。但是我們觀察視差圖會發現，圖像全是黑色，這主要是因為匹配的問題(在剛開始，你可能以為是相機標定有問題，但是你可以通過測試圖片去校驗，相機標定大概也就是那幾個函數，只要你流程對了，一般標定結果差別不是太大，而且標定結果一般只對我們的測量精度有影響，但是如果匹配算法有問題，或者選擇的匹配參數有問題，你就會出現類似上圖的結果)。

針對這種情況我們應該怎么做，我們首先要考慮是不是我們拍攝的圖片有問題，因為有些圖片在進行匹配時，生產的視差圖會有缺陷，具體可以參考：【關於立體視覺的一切】立體匹配成像算法BM，SGBM，GC，SAD一覽。

（1）光學失真和噪聲（亮度、色調、飽和度等失衡）

（2）平滑表面的鏡面反射

高光處無細節，無特征點。

（3）投影縮減（Foreshortening）

攝影測量學中的一個概念，指物體近大遠小。由於相對左右照相機距離的不同，看到的同一個物體在左右視圖中的投影尺寸也會不同，造成匹配障礙。

（4）透視失真（Perspective distortions）

由於鏡頭畸變造成的被攝物體失真。譬如畫面中的鼻子被拉長。

（5）低紋理（Low texture）

無細節。主動紋理光可以解決這一問題。(可以加入光源，投影一個紋理進去)

（6）重復紋理（Repetitive/ambiguous patterns）

高度相似的特征點描述向量接近，行掃描時難以判斷哪一個是對應的特征點。

（7）透明物體

同低紋理。

（8）重疊和非連續

紋理中斷，不利於行查找。

我們再來觀察一下我們的圖片，很顯然我們的圖片是有一些缺陷的，比如平滑表面的鏡面反射，重復紋理，但是也不至於視差圖什么都看不到。

因此我嘗試去調節uniquenessRatio，numberOfDisparities，windowSize這三個參數，調了快一個禮拜，不過可惜，並沒有什么顯著效果。

當uniquenessRatio調到很小時，可以看到很多誤匹配點。然后我用SURF算法進行斑點匹配：

我發現基本上所有點都匹配錯了，尷尬，這主要是因為重復紋理的問題，不同的特征點提取到的特征描述符可能是近似相等的，因此會造成很多誤匹配。但是BM算法是行匹配，難道也會出現那么多匹配么？由於沒有去看opencv BM匹配算法的實現，也很難推測到問題出現在哪里，后面我還會繼續嘗試。有興趣的朋友也可以問我索要圖片集，自己嘗試一下，歡迎大家一起來討論。

后面我嘗試了使用SGBM算法去匹配，雖然可以看到了視差圖，但是輪廓都不對，很無奈：