Fix unstable work of stereoRectify function. Issue description: opencv#10791

Author: denmatfoton

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++ )
     {