diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 9bc48661e3..d24bc90d1b 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -2297,7 +2297,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], _w3[3]; + double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3]; cv::Rect_ inner1, inner2, outer1, outer2; CvMat om = cvMat(3, 1, CV_64F, _om); @@ -2306,13 +2306,11 @@ 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 @@ -2323,36 +2321,15 @@ 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; - // 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); - CV_Assert(fabs(nt) > 0); - nw = cvNorm(&ww, 0, CV_L2); - CV_Assert(fabs(nw) > 0); - cvConvertScale(&ww, &ww, 1 / nw); - cvCrossProduct(&t, &ww, &w3); - nw = cvNorm(&w3, 0, CV_L2); - CV_Assert(fabs(nw) > 0); - 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 - } + // 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); // apply to both views cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); diff --git a/modules/calib3d/test/test_cameracalibration.cpp b/modules/calib3d/test/test_cameracalibration.cpp index adf4e454be..7aeaea3021 100644 --- a/modules/calib3d/test/test_cameracalibration.cpp +++ b/modules/calib3d/test/test_cameracalibration.cpp @@ -2332,6 +2332,44 @@ TEST(Calib3d_StereoCalibrate, regression_10791) EXPECT_GE(roi2.area(), 400*300) << roi2; } +TEST(Calib3d_StereoCalibrate, regression_11131) +{ + const Matx33d M1( + 1457.572438721727, 0, 1212.945694211622, + 0, 1457.522226502963, 1007.32058848921, + 0, 0, 1 + ); + const Matx33d M2( + 1460.868570835972, 0, 1215.024068023046, + 0, 1460.791367088, 1011.107202932225, + 0, 0, 1 + ); + const Matx D1(0, 0, 0, 0, 0); + const Matx D2(0, 0, 0, 0, 0); + + const Matx33d R( + 0.9985404059825475, 0.02963547172078553, -0.04515303352041626, + -0.03103795276460111, 0.9990471552537432, -0.03068268351343364, + 0.04420071389006859, 0.03203935697372317, 0.9985087763742083 + ); + const Matx31d T(0.9995500167379527, 0.0116311595111068, 0.02764923448462666); + + const Size imageSize(2456, 2058); + + 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_GT(P1.at(0, 0), 0); + EXPECT_GT(P2.at(0, 0), 0); + EXPECT_GT(R1.at(0, 0), 0); + EXPECT_GT(R2.at(0, 0), 0); + 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