Merge pull request opencv#10793 from denmatfoton:master

Author: alalek

modules/calib3d/src/calibration.cpp

@@ -2296,7 +2296,7 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
                       CvRect* roi1, CvRect* roi2 )
 {
     double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
-    double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3];
+    double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3], _w3[3];
     cv::Rect_<float> inner1, inner2, outer1, outer2;
 
     CvMat om  = cvMat(3, 1, CV_64F, _om);
@@ -2305,11 +2305,13 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
     CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
     CvMat pp  = cvMat(3, 4, CV_64F, _pp);
     CvMat ww  = cvMat(3, 1, CV_64F, _ww); // temps
+    CvMat w3  = cvMat(3, 1, CV_64F, _w3); // temps
     CvMat wR  = cvMat(3, 3, CV_64F, _wr);
     CvMat Z   = cvMat(3, 1, CV_64F, _z);
     CvMat Ri  = cvMat(3, 3, CV_64F, _ri);
     double nx = imageSize.width, ny = imageSize.height;
     int i, k;
+    double nt, nw;
 
     if( matR->rows == 3 && matR->cols == 3 )
         cvRodrigues2(matR, &om);          // get vector rotation
@@ -2320,15 +2322,33 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
     cvMatMul(&r_r, matT, &t);
 
     int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
-    double c = _t[idx], nt = cvNorm(&t, 0, CV_L2);
-    _uu[idx] = c > 0 ? 1 : -1;
 
-    // calculate global Z rotation
-    cvCrossProduct(&t,&uu,&ww);
-    double nw = cvNorm(&ww, 0, CV_L2);
-    if (nw > 0.0)
-        cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);
-    cvRodrigues2(&ww, &wR);
+    // if idx == 0
+    //   e1 = T / ||T||
+    //   e2 = e1 x [0,0,1]
+
+    // if idx == 1
+    //   e2 = T / ||T||
+    //   e1 = e2 x [0,0,1]
+
+    // e3 = e1 x e2
+
+    _uu[2] = 1;
+    cvCrossProduct(&uu, &t, &ww);
+    nt = cvNorm(&t, 0, CV_L2);
+    nw = cvNorm(&ww, 0, CV_L2);
+    cvConvertScale(&ww, &ww, 1 / nw);
+    cvCrossProduct(&t, &ww, &w3);
+    nw = cvNorm(&w3, 0, CV_L2);
+    cvConvertScale(&w3, &w3, 1 / nw);
+    _uu[2] = 0;
+
+    for (i = 0; i < 3; ++i)
+    {
+        _wr[idx][i] = -_t[i] / nt;
+        _wr[idx ^ 1][i] = -_ww[i];
+        _wr[2][i] = _w3[i] * (1 - 2 * idx); // if idx == 1 -> opposite direction
+    }
 
     // apply to both views
     cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
@@ -2342,16 +2362,11 @@ void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
     double fc_new = DBL_MAX;
     CvPoint2D64f cc_new[2] = {{0,0}, {0,0}};
 
-    for( k = 0; k < 2; k++ ) {
-        const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
-        const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
-        double dk1 = Dk && Dk->data.ptr ? cvmGet(Dk, 0, 0) : 0;
-        double fc = cvmGet(A,idx^1,idx^1);
-        if( dk1 < 0 ) {
-            fc *= 1 + dk1*(nx*nx + ny*ny)/(4*fc*fc);
-        }
-        fc_new = MIN(fc_new, fc);
-    }
+    newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
+    const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
+    const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
+    const double ratio = idx == 1 ? ratio_x : ratio_y;
+    fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;
 
     for( k = 0; k < 2; k++ )
     {

modules/calib3d/test/test_cameracalibration.cpp

@@ -2116,6 +2116,44 @@ TEST(Calib3d_StereoCalibrate_CPP, extended)
     EXPECT_TRUE(err.total() == 2);
 }
 
+TEST(Calib3d_StereoCalibrate, regression_10791)
+{
+    const Matx33d M1(
+        853.1387981631528, 0, 704.154907802121,
+        0, 853.6445089162528, 520.3600712930319,
+        0, 0, 1
+    );
+    const Matx33d M2(
+        848.6090216909176, 0, 701.6162856852185,
+        0, 849.7040162357157, 509.1864036137,
+        0, 0, 1
+    );
+    const Matx<double, 14, 1> D1(-6.463598629567206, 79.00104930508179, -0.0001006144444464403, -0.0005437499822299972,
+        12.56900616588467, -6.056719942752855, 76.3842481414836, 45.57460250612659,
+        0, 0, 0, 0, 0, 0);
+    const Matx<double, 14, 1> D2(0.6123436439798265, -0.4671756923224087, -0.0001261947899033442, -0.000597334584036978,
+        -0.05660119809538371, 1.037075740629769, -0.3076042835831711, -0.2502169324283623,
+        0, 0, 0, 0, 0, 0);
+
+    const Matx33d R(
+        0.9999926627018476, -0.0001095586963765905, 0.003829169539302921,
+        0.0001021735876758584, 0.9999981346680941, 0.0019287874145156,
+        -0.003829373712065528, -0.001928382022437616, 0.9999908085776333
+    );
+    const Matx31d T(-58.9161771697128, -0.01581306249996402, -0.8492960216760961);
+
+    const Size imageSize(1280, 960);
+
+    Mat R1, R2, P1, P2, Q;
+    Rect roi1, roi2;
+    stereoRectify(M1, D1, M2, D2, imageSize, R, T,
+                  R1, R2, P1, P2, Q,
+                  CALIB_ZERO_DISPARITY, 1, imageSize, &roi1, &roi2);
+
+    EXPECT_GE(roi1.area(), 400*300) << roi1;
+    EXPECT_GE(roi2.area(), 400*300) << roi2;
+}
+
 TEST(Calib3d_Triangulate, accuracy)
 {
     // the testcase from http://code.opencv.org/issues/4334