立体校正

我们知道cvStereoRectify()函数只是计算了校正需要的参数，并没有完成图像上实际的校正。

下面介绍两个函数：

cvInitUndistortRectifyMap()函数来计算左右视图的校正查找映射表mapx,mapy。

cvRemap()函数利用查找映射表和插值算法实现将输入图像的重投影，从而得到校正后图像。

为了理解下面的话，我们将书上图附上：

对左右摄像机而言，(a)原始图像(b)非畸变化(c)校正(d)最后裁切成两幅图像间的重叠区域。校正实际上是右(c)到(a)的反过程。

对任何图像到图像的映射函数，由于目标位置是浮点型的缘故，正向映射（即根据原始图像上的点计算其到目标图像上的点），不会命中目标图像对应的像素位置。

为了理解上面的话，我们引入OpenCV中cvRemap函数中的解释：

例如，我们可以说像素的位于（20，17）位置，当这些整数位置映射到新的图像中，可能会有一些差异——源图像中像素中的像素位置是整形的而目标图像是浮点型的，就必须将其四舍五入到最相近的整型，因为可能映射后的位置完全就是没有像素的（联想一下通过拉伸图像扩大两倍，其他的每个目标像素将是空白）。这个问题一般被称为正向投影问题。为了解决这些四舍五入和目标差异的问题，实际上我们可以反过来解决：通过目标图像中的每一个像素去问：“哪个像素需要来填充这个目标像素？“这些源像素的位置几乎都是小数（非整数），所以必须对这些像素进行插值以得到目标位置的正确值。

因此我们采用逆向映射：对目标图像上的每一个整形的像素位置，首先查找出其对应源图像上的浮点位置，然后利用周围源像素的每个整形值插值出新的值来。

计算这样映射关系的实现函数cvInitUndistortRectifyMap()被调用两次，即分别求解左右原始图像到校正图像之间的映射查找表，声明如下：

void cvInitUndistortRectifyMap(const CvMat* M,const CvMat* distCoeffs,const CvMat* Rrect,const CvMat* Mrect,CvArr* mapx,CvArr* mapy);

1、M,是3×3的摄像机内参数矩阵，校正前的。

2、disCoeffs,是5×1的摄像机畸变系数。

3、Rrect，是cvStereoRecify()函数输出的Rl或者Rr

4、Mrect是3×3的校正后的摄像机内参数矩阵，该矩阵是cvStereoRectify()计算得到投影矩阵P的前三列。

5、mapx,mapy,是函数返回的输出查找映射表。

（个人对查找表的理解：我们知道查找表的大小和目标图像一样，那么我们对于目标图像的每一个位置如(12,8)，那么mapx和mapy存储着位于该处（即在mapx和mapy矩阵中也是(12,8)位置处）的源图像对应的x和y坐标位置，此时的在源图像中的坐标为浮点型，我们利用插值算法求解出该位置的像素值，然后将该像素值给目标图像的(12,8)位置的像素，要像正确理解，还需要深入的了解算法的操作流程，这里理解不免有误）。

利用计算得到查找映射表重投影图像的cvRemap()函数声明如下：

void cvRemap(const CvArr* src,CvArr* dst,const CvArr* mapx,const CvArr* mapy,int flags = CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS,CvScalar fillval = cvScalarAll(0));

cvRemap函数将一幅图像中一个位置的像素平滑的重映射到另一个位置。

1、src源图像

2、dst目标图像，源图像和目标图像必须大小和通道一致，数据类型可以任意。

3、mapx,mapy，为单通道，通常为IPL_DEPTH_32F浮点型数据，和源图像、目标图像有同样的大小，指明具体像素重新分别的位置。

4、flags，插值方法，默认采用双线性插值。

插值方法：

CV_INTER_NN 最邻近插值

CV_INTER_LINEAR 双线性插值（默认）

CV_INTER_AREA  像素区域重新采样

CV_INTER_CUBIC 双三次插值

CV_WARP_FILL_OUTLIERS,其效果是可以用最后一个变量fillval设置的值填充目标图像的像素，而这些像素在原始输入图像中没有任意像素与之对应。

如果使用cvStereoRectifyUncalibrated()函数，即不使用摄像机内参数来校正视觉时，关于讲解请看Learning OpenCV中文版 475页倒数第二段以及阅读OpenCV自带的stereo_calib.cpp程序来详细了解具体的操作。

双目定标和双目校正原理如下;

双目摄像头定标不仅要得出每个摄像头的内部参数，还需要通过标定来测量两个摄像头之间的相对位置（即右摄像头相对于左摄像头的三维平移 t 和旋转 R 参数）。

图6

要计算目标点在左右两个视图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起来。然而，在二维空间上匹配对应点是非常耗时的，为了减少匹配搜索范围，我们可以利用极线约束使得对应点的匹配由二维搜索降为一维搜索。

        

图7

而双目校正的作用就是要把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

图8

 

1． 关于cvStereoCalibrate的使用

如果按照 Learning OpenCV 的例程，直接通过cvStereoCalibrate来实现双目定标，很容易产生比较大的图像畸变，边角处的变形较厉害。最好先通过cvCalibrateCamera2() 对每个摄像头单独进行定标，再利用cvStereoCalibrate进行双目定标。这样定标所得参数才比较准确，随后的校正也不会有明显的畸变。我使用的程序主要基于Learning OpenCV 的例程ch12_ex12_3.cpp，其中主要部分如下：

    //////////////////////////////////////////////////////////////////////////    // 是否首先进行单目定标？    cvCalibrateCamera2(&_objectPoints, &_imagePoints1, &_npoints, imageSize,            &t_M1, &t_D1, NULL, NULL, CV_CALIB_FIX_K3);    cvCalibrateCamera2(&_objectPoints, &_imagePoints2, &_npoints, imageSize,            &t_M2, &t_D2, NULL, NULL, CV_CALIB_FIX_K3);    //////////////////////////////////////////////////////////////////////////    // 进行双目定标    cvStereoCalibrate( &_objectPoints, &_imagePoints1,        &_imagePoints2, &_npoints,        &t_M1, &t_D1, &t_M2, &t_D2,        imageSize, &t_R, &t_T, &t_E, &t_F,        cvTermCriteria(CV_TERMCRIT_ITER+        CV_TERMCRIT_EPS, 100, 1e-5));  // flags为默认的CV_CALIB_FIX_INTRINSIC

上面的t_M1(2), t_D1(2) 分别是单目定标后获得的左（右）摄像头的内参矩阵（3*3）和畸变参数向量（1*5）；t_R, t_T 分别是右摄像头相对于左摄像头的旋转矩阵（3*3）和平移向量（3*1）, t_E是包含了两个摄像头相对位置关系的Essential Matrix（3*3），t_F 则是既包含两个摄像头相对位置关系、也包含摄像头各自内参信息的Fundamental Matrix（3*3）。

图9

 

2． cvStereoCalibrate 是怎样计算 Essential Matrix 和 Fundamental Matrix 的？

首先我们以Learning OpenCV第422页为基础，讨论下 Essential Matrix 和 Fundamental Matrix 的构造过程，再看看 OpenCV 是如何进行计算的。

图10

 注：原文中对p_l、p_r 和q_l、q_r 物理意义和计算公式的表述有误，已修正。（2011-04-12）

（1）Essential Matrix

如上图所示，给定一个目标点P，以左摄像头光心O_l为原点。点P相对于光心O_l的观察位置为P_l，相对于光心O_r的观察位置为P_r。点P在左摄像头成像平面上的位置为p_l，在右摄像头成像平面上的位置为p_r。注意P_l、P_r、p_l、p_r 都处于摄像机坐标系，其量纲是与平移向量T相同的（p_l、p_r 在图像坐标系中对应的像素坐标为 q_l、q_r ）。

假设右摄像头相对于左摄像头的相对位置关系由旋转矩阵R和平移向量T表示，则可得：P_r = R(P_l-T)。

现在我们要寻找由点P、O_l和O_r确定的对极平面的表达式。注意到平面上任意一点x与点a的连线垂直于平面法向量n，即向量 (x-a) 与向量 n 的点积为0：(x-a)·n = 0。在Ol坐标系中，光心Or的位置为T，则P、O_l和O_r确定的对极平面可由下式表示：(P_l-T)^T·(P_l×T) = 0。

由P_r = R(P_l-T) 和 R^T=R^-1 可得：(R^TP_r)^T·(P_l×T) = 0。

另一方面，向量的叉积又可表示为矩阵与向量的乘积，记向量T的矩阵表示为S，得：P_l×T = SP_l。

图11

那么就得到：(P_r)^TRSP_l = 0。这样，我们就得到Essential Matrix：E = RS。

通过矩阵E我们知道P_l和P_r的关系满足：(P_r)^TEP_l = 0。进一步地，由 p_l = f_l*P_l/Z_l 和 p_r = f_r*P_r/Z_r 我们可以得到点P在左右两个摄像机坐标系中的观察点 p_l和 p_r 应满足的极线约束关系为：(p_r)^TEp_l = 0。

注意到 E 是不满秩的，它的秩为2，那么 (p_r)^TEp_l = 0 表示的实际上是一条直线，也就是对极线。

（2）Fundamental Matrix

由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在图像像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像头坐标系和图像像素坐标系联系起来。在（1）中，p_l和p_r是物理坐标值，对应的像素坐标值为q_l和q_r，摄像头内参矩阵为M，则有：p=M^-1q。从而：(p_r)^TEp_l = 0 à q_r^T(M_r^-1)^TE M_l^-1q_l = 0。这里，我们就得到Fundamental Matrix：F = (M_r^-1)^TE M_l^-1。并有 q_r^TFq_l = 0。

（3）OpenCV的相关计算

由上面的分析可见，求取矩阵E和F关键在于旋转矩阵R和平移向量T的计算，而cvStereoCalibrate的代码中大部分（cvcalibration.cpp的第1886-2180行）也是计算和优化R和T的。在cvcalibration.cpp的第1913-1925行给出了计算R和T初始估计值的基本方法：

    /*       Compute initial estimate of pose       For each image, compute:          R(om) is the rotation matrix of om          om(R) is the rotation vector of R          R_ref = R(om_right) * R(om_left)'          T_ref_list = [T_ref_list; T_right - R_ref * T_left]          om_ref_list = {om_ref_list; om(R_ref)]       om = median(om_ref_list)       T = median(T_ref_list)    */

具体的计算过程比较繁杂，不好理解，这里就不讨论了，下面是计算矩阵E和F的代码：

 

    if( matE || matF )    {        double* t = T_LR.data.db;        double tx[] =        {            0, -t[2], t[1],            t[2], 0, -t[0],            -t[1], t[0], 0        };        CvMat Tx = cvMat(3, 3, CV_64F, tx);        double e[9], f[9];        CvMat E = cvMat(3, 3, CV_64F, e);        CvMat F = cvMat(3, 3, CV_64F, f);        cvMatMul( &Tx, &R_LR, &E );        if( matE )            cvConvert( &E, matE );        if( matF )        {            double ik[9];            CvMat iK = cvMat(3, 3, CV_64F, ik);            cvInvert(&K[1], &iK);            cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );            cvInvert(&K[0], &iK);            cvMatMul(&E, &iK, &F);            cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 );        }    }

 

3． 通过双目定标得出的向量T中，Tx符号为什么是负的？

“@scyscyao：这个其实我也不是很清楚。个人的解释是，双目定标得出的T向量指向是从右摄像头指向左摄像头(也就是Tx为负)，而在OpenCV坐标系中，坐标的原点是在左摄像头的。因此，用作校准的时候，要把这个向量的三个分量符号都要换一下，最后求出的距离才会是正的。

图12

但是这里还有一个问题，就是Learning OpenCV中Q的表达式，第四行第三列元素是-1/Tx，而在具体实践中，求出来的实际值是1/Tx。这里我和maxwellsdemon讨论下来的结果是，估计书上Q表达式里的这个负号就是为了抵消T向量的反方向所设的，但在实际写OpenCV代码的过程中，那位朋友却没有把这个负号加进去。”

图13

scyscyao 的分析有一定道理，不过我觉得还有另外一种解释：如上图所示，摄像机C₁（C₂）与世界坐标系相对位置的外部参数为旋转矩阵R₁（R₂）和平移向量 t₁（t₂），如果下标1代表左摄像机，2代表右摄像机，显然在平移向量的水平分量上有 t_1x > t_2x；若以左摄像机C₁为坐标原点，则可得到如上图所示的旋转矩阵R和平移向量t，由于t_1x > t_2x，则有 t_x < 0。

为了抵消T_x为负，在矩阵Q中元素(4,3)应该加上负号，但在cvStereoRectify代码中并没有添加上，这就使得我们通过 cvReprojectImageTo3D 计算得到的三维数据都与实际值反号了。

 

    if( matQ )    {        double q[] =        {            1, 0, 0, -cc_new[0].x,            0, 1, 0, -cc_new[0].y,            0, 0, 0, fc_new,            0, 0, 1./_t[idx],            (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]        };        CvMat Q = cvMat(4, 4, CV_64F, q);        cvConvert( &Q, matQ );    }

 

为了避免上述反号的情况，可以在计算得到Q矩阵后，添加以下代码更改 Q[3][2] 的值。

 

// Q 是 Mat 类型矩阵，OpenCV2.1 C++ 模式    Q.at<double>(3, 2) = -Q.at<double>(3, 2);    // Q 是 double 数组定义时    double Q[4][4];    CvMat t_Q = cvMat(4, 4, CV_64F, Q );    cvStereoRectify(…);    Q[3][2]=-Q[3][2];

 

4． 双目校正原理及cvStereoRectify 的应用。

图14

如图14所示，双目校正是根据摄像头定标后获得的单目内参数据（焦距、成像原点、畸变系数）和双目相对位置关系（旋转矩阵和平移向量），分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致（CV_CALIB_ZERO_DISPARITY 标志位设置时发生作用）、两摄像头光轴平行、左右成像平面共面、对极线行对齐。在OpenCV2.1版之前，cvStereoRectify 的主要工作就是完成上述操作，校正后的显示效果如图14(c) 所示。可以看到校正后左右视图的边角区域是不规则的，而且对后续的双目匹配求取视差会产生影响，因为这些边角区域也参与到匹配操作中，其对应的视差值是无用的、而且一般数值比较大，在三维重建和机器人避障导航等应用中会产生不利影响。

因此，OpenCV2.1 版中cvStereoRectify新增了4个参数用于调整双目校正后图像的显示效果，分别是 double alpha, CvSize newImgSize, CvRect* roi1, CvRect* roi2。下面结合图15-17简要介绍这4个参数的作用：

（1）newImgSize：校正后remap图像的分辨率。如果输入为(0,0)，则是与原图像大小一致。对于图像畸变系数比较大的，可以把newImgSize 设得大一些，以保留图像细节。

（2）alpha：图像剪裁系数，取值范围是-1、0~1。当取值为 0 时，OpenCV会对校正后的图像进行缩放和平移，使得remap图像只显示有效像素（即去除不规则的边角区域），如图17所示，适用于机器人避障导航等应用；当alpha取值为1时，remap图像将显示所有原图像中包含的像素，该取值适用于畸变系数极少的高端摄像头；alpha取值在0-1之间时，OpenCV按对应比例保留原图像的边角区域像素。Alpha取值为-1时，OpenCV自动进行缩放和平移，其显示效果如图16所示。

（3）roi1, roi2：用于标记remap图像中包含有效像素的矩形区域。对应代码如下：

 

02433     if(roi1)02434     {02435         *roi1 = cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1),02436                      cvCeil((inner1.y - cy1_0)*s + cy1),02437                      cvFloor(inner1.width*s), cvFloor(inner1.height*s))02438             & cv::Rect(0, 0, newImgSize.width, newImgSize.height);02439     }02440     02441     if(roi2)02442     {02443         *roi2 = cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2),02444                      cvCeil((inner2.y - cy2_0)*s + cy2),02445                      cvFloor(inner2.width*s), cvFloor(inner2.height*s))02446             & cv::Rect(0, 0, newImgSize.width, newImgSize.height);02447     }

 

图15

图16

图17

在cvStereoRectify 之后，一般紧接着使用 cvInitUndistortRectifyMap 来产生校正图像所需的变换参数（mapx, mapy）。

 

//////////////////////////////////////////////////////////////////////////// 执行双目校正// 利用BOUGUET方法或HARTLEY方法来校正图像    mx1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );    my1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );    mx2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );    my2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F );    double R1[3][3], R2[3][3], P1[3][4], P2[3][4], Q[4][4];    CvMat t_R1 = cvMat(3, 3, CV_64F, R1);    CvMat t_R2 = cvMat(3, 3, CV_64F, R2);    CvMat t_Q = cvMat(4, 4, CV_64F, Q );    CvRect roi1, roi2;// IF BY CALIBRATED (BOUGUET'S METHOD)        CvMat t_P1 = cvMat(3, 4, CV_64F, P1);    CvMat t_P2 = cvMat(3, 4, CV_64F, P2);    cvStereoRectify( &t_M1, &t_M2, &t_D1, &t_D2, imageSize,        &t_R, &t_T, &t_R1, &t_R2, &t_P1, &t_P2, &t_Q,        CV_CALIB_ZERO_DISPARITY,         0, imageSize, &roi1, &roi2); // Precompute maps for cvRemap()    cvInitUndistortRectifyMap(&t_M1,&t_D1,&t_R1,&t_P1, mx1, my1);    cvInitUndistortRectifyMap(&t_M2,&t_D2,&t_R2,&t_P2, mx2, my2);

 

5． 为什么cvStereoRectify求出的Q矩阵cx, cy, f都与原来的不同？

“@scyscyao：在实际测量中，由于摄像头摆放的关系，左右摄像头的f, cx, cy都是不相同的。而为了使左右视图达到完全平行对准的理想形式从而达到数学上运算的方便，立体校准所做的工作事实上就是在左右像重合区域最大的情况下，让两个摄像头光轴的前向平行，并且让左右摄像头的f, cx, cy相同。因此，Q矩阵中的值与两个instrinsic矩阵的值不一样就可以理解了。”

注：校正后得到的变换矩阵Q，Q[0][3]、Q[1][3]存储的是校正后左摄像头的原点坐标（principal point）cx和cy，Q[2][3]是焦距f。