雙目測距與三維重建的OpenCV實現問題集錦（二）雙目定標與雙目校正

    //////////////////////////////////////////////////////////////////////////
    // 是否首先進行單目定標？
    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

    /*
       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)
    */

    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 );
        }
    }

    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 是 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];

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     }

//////////////////////////////////////////////////////////////////////////
// 執行雙目校正
// 利用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);