第七节、双目视觉之空间坐标计算

在上一节我们已经介绍了如何对相机进行标定。然后获取相机的内部参数，外部参数。

内参包括焦距、主点、倾斜系数、畸变系数：

$$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、坐标计算

上一节我们介绍了从世界坐标系到像素坐标系的转换：

$$z_c\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\begin{bmatrix} f_x & \gamma & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} r_{00} & r_{01} & r_{02} & T_X \\ r_{10} & r_{11} & r_{12} & T_Y \\ r_{20} & r_{21} & r_{22} & T_Z \\ 0& 0 & 0 & 1\end{bmatrix}\begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix}$$

$$=\begin{bmatrix} f_x & \gamma & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} R_{3\times{3}} & T_{3 \times {1}} \\ 0 & 1\end{bmatrix}\begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix}$$

其中${\begin{bmatrix} u & v & 1 \end{bmatrix}}^T$为矫正后的图像中的点在像素坐标系中的坐标，${\begin{bmatrix} x_w & y_w & z_w & 1 \end{bmatrix}}^T$为点在世界坐标系中的坐标。

现在我们来研究一下从像素坐标系到世界坐标系的转换：

光轴会聚模型(两个摄像机的像平面不是平行的)：

对于左右相机分别有：

$$z_{c1}\begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}=\begin{bmatrix} p_{00}^1 & p_{01}^1 & p_{02}^1 & p_{03}^1 \\ p_{10}^1 & p_{11}^1 & p_{12}^1 & p_{13}^1 \\ p_{20}^1 & p_{21}^1 & p_{22}^1 & p_{23}^1 \end{bmatrix}\begin{bmatrix} x_w \\ y_w \\ z_w \\ 1\end{bmatrix}$$

$$z_{c2}\begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix}=\begin{bmatrix} p_{00}^2 & p_{01}^2 & p_{02}^2 & p_{03}^2 \\ p_{10}^2 & p_{11}^2 & p_{12}^2 & p_{13}^2 \\ p_{20}^2 & p_{21}^2 & p_{22}^2 & p_{23}^2 \end{bmatrix}\begin{bmatrix} x_w & y_w & z_w & 1\end{bmatrix}$$

将上面两式整理可以得到：

$$(u_1p_{20}^1-p_{00}^1)x_w+(u_1p_{21}^1-p_{01}^1)y_w+(u_1p_{22}^1-p_{02}^1)z_w=p_{03}^1-u_1p_{23}^1$$

$$(v_1p_{20}^1-p_{10}^1)x_w+(v_1p_{21}^1-p_{11}^1)y_w+(v_1p_{22}^1-p_{12}^1)z_w=p_{13}^1-v_1p_{23}^1$$

$$(u_2p_{20}^2-p_{00}^2)x_w+(u_2p_{21}^2-p_{01}^2)y_w+(u_2p_{22}^2-p_{02}^2)z_w=p_{03}^2-u_2p_{23}^2$$

$$(v_2p_{20}^2-p_{10}^2)x_w+(v_2p_{21}^2-p_{11}^2)y_w+(v_2p_{22}^2-p_{12}^2)z_w=p_{13}^2-v_2p_{23}^2$$

采用最小二乘法求解$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$的所有点所组成的平面（即极面）可以用下式表示：

$$(P_{cl}-T)^T\cdot(P_{cl}\times{T})=0$$

向量的叉积又可表示为矩阵与向量的乘积：

$$P_{cl}\times{T}=SP_{cl}$$

注意：上面的运算符号$\times$表示向量之间的叉乘运算,运算符号.表示两个向量的点乘。

其中$S$：

$$s=\begin{bmatrix}0 & -T_z & T_y \\ T_z & 0 & -T_x \\ -T_y & T_x & 0 \end{bmatrix}$$

综合上式可得：

$$P_{cr}^TRSP_{cl}=0$$

乘积$RS$即为本征矩阵$E$，令$E=RS$，得到：

$$P_{cr}^TEP_{cl}=0$$

描述了同一物理点在左右摄像机图像平面上投影在相机坐标系下的关系。

（2）Fundamental Matrix

由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像机坐标系和像素坐标系联系起来。我们定义基础矩阵F，描述同一物理点在左右摄像机像素坐标系下的关系，单位为pix。

像素坐标系与摄相机坐标系的关系：

$$P_{pix}=MP_c$$

$$P_{cr}^TEP_{cl}=0$$

假设左右摄像机坐标系下点$P_{cl}$、$P_{cr}$在像素坐标系中对应的坐标分别为$ P_{pixl}$、$P_{pixr}$，我们可以得到：

$$P_{pixr}^T(M_r^{-1})^TEM_l^{-1}P_{pixl}=0$$

由此可将基础矩阵$F$定义为：

$$F=(M_r^{-1})^TEM_l^{-1}$$

最终得到同一物理点在左右摄像机像素坐标系下的关系：

$$P_{pixr}^TFP_{pixl}=0$$

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$分别为左右相机的投影矩阵，其作用是将世界坐标系的点转换到像素坐标系下(左相机光心为世界坐标系原点):

$$Z_c\begin{bmatrix} u \\ v \\1 \end{bmatrix}=P\begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix} $$

$Q$矩阵为重投影矩阵，即矩阵$Q$可以把像素坐标系下的点转换到世界坐标系下:
$$\begin{bmatrix} X_w \\ Y_w \\ Z_w \\ W \end{bmatrix}=Q\begin{bmatrix} u \\ v \\ d \\ 1 \end{bmatrix} $$

其中$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，则有比例关系：

$$\frac{P_LP_R}{Z-f}=\frac{O_LO_R}{Z}$$

$$\frac{b-[(X_L-\frac{L}{2})+(\frac{L}{2}-X_R)]}{Z-f}=\frac{b}{z}$$

$$\frac{b+X_R-X_L}{Z_w-f}=\frac{b}{Z_w}$$

由于上面的$X_R$,$X_L$的单位均是物理单位，而我们想使用像素单位去表示，我们在上一节中介绍从图像坐标系转到像素坐标系时说到$dx$,$dy$,分别表示每个像素在横轴$x$和纵轴$y$上的物理尺寸，则上式变为：

$$\frac{b+(u_R-u_L)dx}{Z_w-f}=\frac{b}{Z_w}$$

$$Z_w=\frac{bf}{(u_L-u_R)dx}=\frac{bf_x}{u_L-u_R}$$

$u_R$,$u_L$分别表示$P_L$、$P_R$距离各自像平面左边缘的像素距离。$f_x$是我们通过相机标定的内参值。

我们定义视差$d=u_L-u_R$($d$和$f_x$的单位都是像素，两个正好消去，$Z_w$的单位和$b$一样，都是物理单位)，所以有：

$$Z_w=\frac{bf_x}{d}$$

从上式我们知道距离摄像机越远的物体，视差值越小，越近的物体，视差值越大，这也符合我们人眼的规律。

现在我们已经求出来$Z_w$的值，再来看看$X_w$,$Y_w$如何求解：

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

$$\frac{(u-u_0)dx}{f}=\frac{u-u_0}{f_x}=\frac{X_w}{Z_w}$$

$$\frac{(v-v_0)dy}{f}=\frac{v-v_0}{f_y}=\frac{Y_w}{Z_w}$$

当$f_x=f_y$时，我们得到$X_w$,$Y_w$的坐标为：

$$X_w=\frac{u-u_0}{f_x}Z_w=\frac{u-u_0}{f_x}\frac{bf_x}{d}=\frac{b(u-u_0)}{d}$$

$$Y_w=\frac{v-v_0}{f_y}Z_w=\frac{v-v_0}{f_y}\frac{bf_x}{d}=\frac{b(v-v_0)}{d}$$

其中$u$,$v$表示像素坐标系下的点的坐标，$u_0$、$v_0$为左相机图像平面的原点在像素坐标系下的坐标值。

我们$X_w$,$Y_w$,$Z_w$坐标联立在一块得到：

$$\begin{bmatrix} X \\ Y \\ Z \\ W \end{bmatrix}=\begin{bmatrix} 1 & 0 & 0 & -u_0 \\ 0 & 1 & 0 & -v_0 \\ 0 & 0 & 0 & f_x \\ 0 & 0 & \frac{1}{b} & 0 \end{bmatrix}\begin{bmatrix} u \\ v \\ d \\ 1 \end{bmatrix}$$

空间中某点的三维坐标就是$X_w=X/W$, $Y_w=Y/W,$ $Z_w=Z/W$。

但是OpenCV中转换公式如下：

$$\begin{bmatrix} X \\ Y \\ Z \\ W \end{bmatrix}=\begin{bmatrix} 1 & 0 & 0 & -u_0 \\ 0 & 1 & 0 & -v_0 \\ 0 & 0 & 0 & f_x \\ 0 & 0 & \frac{-1}{T_x} & \frac{u_0 - u_0}{T_x} \end{bmatrix}\begin{bmatrix} u \\ v \\ d \\ 1 \end{bmatrix}$$

$$Q=\begin{bmatrix} 1 & 0 & 0 & -u_0 \\ 0 & 1 & 0 & -v_0 \\ 0 & 0 & 0 & f_x \\ 0 & 0 & \frac{-1}{T_x} & \frac{u_0 - u_0}{T_x} \end{bmatrix}$$

这里$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算法去匹配，虽然可以看到了视差图，但是轮廓都不对，很无奈：