From b085158d5999fdced91094405f8753e9275092f6 Mon Sep 17 00:00:00 2001 From: denmatfoton Date: Mon, 5 Feb 2018 15:30:26 -0500 Subject: [PATCH 1/2] Fix unstable work of stereoRectify function. Issue description: https://github.com/opencv/opencv/issues/10791 --- modules/calib3d/src/calibration.cpp | 53 ++++++++++++++++++----------- 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 1a84948e14..6e433106fb 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/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_ 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++ ) { From f378f1d585fd54b1e0f02545d91bf918808aa2ef Mon Sep 17 00:00:00 2001 From: Alexander Alekhin Date: Thu, 15 Feb 2018 15:17:42 +0300 Subject: [PATCH 2/2] calib3d: add regression test for stereoCalibrate --- .../calib3d/test/test_cameracalibration.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/modules/calib3d/test/test_cameracalibration.cpp b/modules/calib3d/test/test_cameracalibration.cpp index 52bb59d685..bf48675965 100644 --- a/modules/calib3d/test/test_cameracalibration.cpp +++ b/modules/calib3d/test/test_cameracalibration.cpp @@ -2118,6 +2118,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 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 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