From 8ae03fcd6e0e1fa895af719b394e3b4ea9d8a470 Mon Sep 17 00:00:00 2001 From: Stefan Spiss Date: Wed, 7 Sep 2022 17:03:51 +0200 Subject: [PATCH 1/4] Extended stereoCalibrate function for pinhole model to return per view rotation and translation vectors between the calibration object coordinate space and the coordinate space of the first camera of the stereo pair. Added overloaded versions of the function for downward compatibility. --- modules/calib3d/include/opencv2/calib3d.hpp | 22 +++- modules/calib3d/src/calibration.cpp | 128 ++++++++++++++++++-- 2 files changed, 140 insertions(+), 10 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 893eb22761..176a2dfc5f 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -1748,6 +1748,15 @@ second camera coordinate system. @param T Output translation vector, see description above. @param E Output essential matrix. @param F Output fundamental matrix. +@param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the +coordinate system of the first camera of the stereo pair (e.g. std::vector). More in detail, each +i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter +description) brings the calibration pattern from the object coordinate space (in which object points are +specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms, +the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space +to camera coordinate space of the first camera of the stereo pair. +@param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description +of previous output parameter ( rvecs ). @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view. @param flags Different flags that may be zero or a combination of the following values: - @ref CALIB_FIX_INTRINSIC Fix cameraMatrix? and distCoeffs? so that only R, T, E, and F @@ -1844,8 +1853,8 @@ CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArray InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, - Size imageSize, InputOutputArray R,InputOutputArray T, OutputArray E, OutputArray F, - OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, + Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); /// @overload @@ -1857,6 +1866,15 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, int flags = CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); +/// @overload +CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, + InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, + InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, + Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, + OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); + /** @brief Computes rectification transforms for each head of a calibrated stereo camera. @param cameraMatrix1 First camera intrinsic matrix. diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 43e58dad19..ba5848891c 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -1973,7 +1973,7 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i CvMat* _cameraMatrix2, CvMat* _distCoeffs2, CvSize imageSize, CvMat* matR, CvMat* matT, CvMat* matE, CvMat* matF, - CvMat* perViewErr, int flags, + CvMat* rvecs, CvMat* tvecs, CvMat* perViewErr, int flags, CvTermCriteria termCrit ) { const int NINTRINSIC = 18; @@ -2013,6 +2013,28 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i cvConvert( _objectPoints, objectPoints ); cvReshape( objectPoints, objectPoints, 3, 1 ); + if( rvecs ) + { + int cn = CV_MAT_CN(rvecs->type); + if( !CV_IS_MAT(rvecs) || + (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) || + ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) && + (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) ) + CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel " + "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" ); + } + + if( tvecs ) + { + int cn = CV_MAT_CN(tvecs->type); + if( !CV_IS_MAT(tvecs) || + (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) || + ((tvecs->rows != nimages || tvecs->cols*cn != 3) && + (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) ) + CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel " + "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" ); + } + for( k = 0; k < 2; k++ ) { const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2; @@ -2440,6 +2462,39 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i } } + CvMat tmp = cvMat(3, 3, CV_64F); + for( i = 0; i < nimages; i++ ) + { + CvMat src, dst; + + if( rvecs ) + { + src = cvMat(3, 1, CV_64F, solver.param->data.db+(i+1)*6); + if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 ) + { + dst = cvMat(3, 3, CV_MAT_DEPTH(rvecs->type), + rvecs->data.ptr + rvecs->step*i); + cvRodrigues2( &src, &tmp ); + cvConvert( &tmp, &dst ); + } + else + { + dst = cvMat(3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ? + rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) : + rvecs->data.ptr + rvecs->step*i); + cvConvert( &src, &dst ); + } + } + if( tvecs ) + { + src = cvMat(3, 1,CV_64F,solver.param->data.db+(i+1)*6+3); + dst = cvMat(3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ? + tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) : + tvecs->data.ptr + tvecs->step*i); + cvConvert( &src, &dst ); + } + } + return std::sqrt(reprojErr/(pointsTotal*2)); } double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1, @@ -2453,7 +2508,7 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1 { return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE, - matF, NULL, flags, termCrit); + matF, NULL, NULL, NULL, flags, termCrit); } static void @@ -3886,7 +3941,22 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2, Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat, OutputArray _Emat, OutputArray _Fmat, - OutputArray _perViewErrors, int flags , + OutputArray _perViewErrors, int flags, + TermCriteria criteria) +{ + return stereoCalibrate(_objectPoints, _imagePoints1, _imagePoints2, _cameraMatrix1, _distCoeffs1, + _cameraMatrix2, _distCoeffs2, imageSize, _Rmat, _Tmat, _Emat, _Fmat, + noArray(), noArray(), _perViewErrors, flags, criteria); +} + +double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, + InputArrayOfArrays _imagePoints1, + InputArrayOfArrays _imagePoints2, + InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1, + InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2, + Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat, + OutputArray _Emat, OutputArray _Fmat, + OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _perViewErrors, int flags, TermCriteria criteria) { int rtype = CV_64F; @@ -3901,19 +3971,22 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, if( !(flags & CALIB_RATIONAL_MODEL) && (!(flags & CALIB_THIN_PRISM_MODEL)) && - (!(flags & CALIB_TILTED_MODEL))) + (!(flags & CALIB_TILTED_MODEL)) ) { distCoeffs1 = distCoeffs1.rows == 1 ? distCoeffs1.colRange(0, 5) : distCoeffs1.rowRange(0, 5); distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5); } - if((flags & CALIB_USE_EXTRINSIC_GUESS) == 0) + if( (flags & CALIB_USE_EXTRINSIC_GUESS) == 0 ) { _Rmat.create(3, 3, rtype); _Tmat.create(3, 1, rtype); } - Mat objPt, imgPt, imgPt2, npoints; + int nimages = int(_objectPoints.total()); + CV_Assert( nimages > 0 ); + + Mat objPt, imgPt, imgPt2, npoints, rvecLM, tvecLM; collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2, objPt, imgPt, &imgPt2, npoints ); @@ -3923,7 +3996,7 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, Mat matR_ = _Rmat.getMat(), matT_ = _Tmat.getMat(); CvMat c_matR = cvMat(matR_), c_matT = cvMat(matT_), c_matE, c_matF, c_matErr; - bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed(); + bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(), errors_needed = _perViewErrors.needed(); Mat matE_, matF_, matErr_; if( E_needed ) @@ -3939,9 +4012,31 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, c_matF = cvMat(matF_); } + bool rvecs_mat_vec = _rvecs.isMatVector(); + bool tvecs_mat_vec = _tvecs.isMatVector(); + + if( rvecs_needed ) + { + _rvecs.create(nimages, 1, CV_64FC3); + + if( rvecs_mat_vec ) + rvecLM.create(nimages, 3, CV_64F); + else + rvecLM = _rvecs.getMat(); + } + if( tvecs_needed ) + { + _tvecs.create(nimages, 1, CV_64FC3); + + if( tvecs_mat_vec ) + tvecLM.create(nimages, 3, CV_64F); + else + tvecLM = _tvecs.getMat(); + } + CvMat c_rvecLM = cvMat(rvecLM), c_tvecLM = cvMat(tvecLM); + if( errors_needed ) { - int nimages = int(_objectPoints.total()); _perViewErrors.create(nimages, 2, CV_64F); matErr_ = _perViewErrors.getMat(); c_matErr = cvMat(matErr_); @@ -3950,6 +4045,7 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, double err = cvStereoCalibrateImpl(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1, &c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, cvSize(imageSize), &c_matR, &c_matT, E_needed ? &c_matE : NULL, F_needed ? &c_matF : NULL, + rvecs_needed ? &c_rvecLM : NULL, tvecs_needed ? &c_tvecLM : NULL, errors_needed ? &c_matErr : NULL, flags, cvTermCriteria(criteria)); cameraMatrix1.copyTo(_cameraMatrix1); @@ -3957,6 +4053,22 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints, distCoeffs1.copyTo(_distCoeffs1); distCoeffs2.copyTo(_distCoeffs2); + for(int i = 0; i < nimages; i++ ) + { + if( rvecs_needed && rvecs_mat_vec ) + { + _rvecs.create(3, 1, CV_64F, i, true); + Mat rv = _rvecs.getMat(i); + memcpy(rv.ptr(), rvecLM.ptr(i), 3*sizeof(double)); + } + if( tvecs_needed && tvecs_mat_vec ) + { + _tvecs.create(3, 1, CV_64F, i, true); + Mat tv = _tvecs.getMat(i); + memcpy(tv.ptr(), tvecLM.ptr(i), 3*sizeof(double)); + } + } + return err; } From 6fb465cb4e77d2ce658e4350b38517d287d19080 Mon Sep 17 00:00:00 2001 From: Stefan Spiss Date: Wed, 7 Sep 2022 17:11:12 +0200 Subject: [PATCH 2/4] Extended stereoCalibrate function for fisheye model to return per view rotation and translation vectors between the calibration object coordinate space and the coordinate space of the first camera of the stereo pair. Added overloaded versions of the function for downward compatibility. --- modules/calib3d/include/opencv2/calib3d.hpp | 15 +++++++++++ modules/calib3d/src/fisheye.cpp | 28 +++++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 176a2dfc5f..b8634bde60 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -3995,6 +3995,15 @@ optimization. It is the \f$max(width,height)/\pi\f$ or the provided \f$f_x\f$, \ @param imageSize Size of the image used only to initialize camera intrinsic matrix. @param R Output rotation matrix between the 1st and the 2nd camera coordinate systems. @param T Output translation vector between the coordinate systems of the cameras. + @param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the + coordinate system of the first camera of the stereo pair (e.g. std::vector). More in detail, each + i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter + description) brings the calibration pattern from the object coordinate space (in which object points are + specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms, + the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space + to camera coordinate space of the first camera of the stereo pair. + @param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description + of previous output parameter ( rvecs ). @param flags Different flags that may be zero or a combination of the following values: - @ref fisheye::CALIB_FIX_INTRINSIC Fix K1, K2? and D1, D2? so that only R, T matrices are estimated. @@ -4009,6 +4018,12 @@ optimization. It is the \f$max(width,height)/\pi\f$ or the provided \f$f_x\f$, \ zero. @param criteria Termination criteria for the iterative optimization algorithm. */ + CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = fisheye::CALIB_FIX_INTRINSIC, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); + + /// @overload CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC, diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 346638d4c1..c150d10d63 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -885,6 +885,13 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, OutputArray R, OutputArray T, int flags, TermCriteria criteria) +{ + return cv::fisheye::stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T, noArray(), noArray(), flags, criteria); +} + +double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags, TermCriteria criteria) { CV_INSTRUMENT_REGION(); @@ -1116,6 +1123,27 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type()); if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type()); if (T.needed()) cv::Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.type()); + if (rvecs.isMatVector()) + { + if(rvecs.empty()) + rvecs.create(n_images, 1, CV_64FC3); + + if(tvecs.empty()) + tvecs.create(n_images, 1, CV_64FC3); + + for(int i = 0; i < n_images; i++ ) + { + rvecs.create(3, 1, CV_64F, i, true); + tvecs.create(3, 1, CV_64F, i, true); + memcpy(rvecs.getMat(i).ptr(), rvecs1[i].val, sizeof(Vec3d)); + memcpy(tvecs.getMat(i).ptr(), tvecs1[i].val, sizeof(Vec3d)); + } + } + else + { + if (rvecs.needed()) cv::Mat(rvecs1).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type()); + if (tvecs.needed()) cv::Mat(tvecs1).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type()); + } return rms; } From 9ca3a3139a54a5aef9e08fa525c9bfd243536460 Mon Sep 17 00:00:00 2001 From: Stefan Spiss Date: Thu, 15 Sep 2022 11:54:31 +0200 Subject: [PATCH 3/4] Extended tests for stereoCalibrate function of pinhole camera model. --- .../calib3d/test/test_cameracalibration.cpp | 165 ++++++++++++++++-- 1 file changed, 155 insertions(+), 10 deletions(-) diff --git a/modules/calib3d/test/test_cameracalibration.cpp b/modules/calib3d/test/test_cameracalibration.cpp index 27e0e83b8f..ee7c2510db 100644 --- a/modules/calib3d/test/test_cameracalibration.cpp +++ b/modules/calib3d/test/test_cameracalibration.cpp @@ -1237,7 +1237,10 @@ protected: Mat& cameraMatrix1, Mat& distCoeffs1, Mat& cameraMatrix2, Mat& distCoeffs2, Size imageSize, Mat& R, Mat& T, - Mat& E, Mat& F, TermCriteria criteria, int flags ) = 0; + Mat& E, Mat& F, + std::vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ) = 0; virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1, const Mat& cameraMatrix2, const Mat& distCoeffs2, Size imageSize, const Mat& R, const Mat& T, @@ -1253,6 +1256,8 @@ protected: virtual void correct( const Mat& F, const Mat &points1, const Mat &points2, Mat &newPoints1, Mat &newPoints2 ) = 0; + int compare(double* val, double* refVal, int len, + double eps, const char* paramName); void run(int); }; @@ -1319,12 +1324,19 @@ bool CV_StereoCalibrationTest::checkPandROI( int test_case_idx, const Mat& M, co return true; } +int CV_StereoCalibrationTest::compare(double* val, double* ref_val, int len, + double eps, const char* param_name ) +{ + return cvtest::cmpEps2_64f( ts, val, ref_val, len, eps, param_name ); +} + void CV_StereoCalibrationTest::run( int ) { const int ntests = 1; const double maxReprojErr = 2; const double maxScanlineDistErr_c = 3; const double maxScanlineDistErr_uc = 4; + const double maxDiffBtwRmsErrors = 1e-4; FILE* f = 0; for(int testcase = 1; testcase <= ntests; testcase++) @@ -1401,13 +1413,23 @@ void CV_StereoCalibrationTest::run( int ) objpt[i].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f)); } + vector rotMats1(nframes); + vector transVecs1(nframes); + vector rotMats2(nframes); + vector transVecs2(nframes); + vector rmsErrorPerView1(nframes); + vector rmsErrorPerView2(nframes); + vector rmsErrorPerViewFromReprojectedImgPts1(nframes); + vector rmsErrorPerViewFromReprojectedImgPts2(nframes); + // rectify (calibrated) Mat M1 = Mat::eye(3,3,CV_64F), M2 = Mat::eye(3,3,CV_64F), D1(5,1,CV_64F), D2(5,1,CV_64F), R, T, E, F; M1.at(0,2) = M2.at(0,2)=(imgsize.width-1)*0.5; M1.at(1,2) = M2.at(1,2)=(imgsize.height-1)*0.5; D1 = Scalar::all(0); D2 = Scalar::all(0); - double err = calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R, T, E, F, + double rmsErrorFromStereoCalib = calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R, T, E, F, + rotMats1, transVecs1, rmsErrorPerView1, rmsErrorPerView2, TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6), CV_CALIB_SAME_FOCAL_LENGTH //+ CV_CALIB_FIX_ASPECT_RATIO @@ -1416,11 +1438,89 @@ void CV_StereoCalibrationTest::run( int ) + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5 //+ CV_CALIB_FIX_K6 ); - err /= nframes*npoints; - if( err > maxReprojErr ) + /* rmsErrorFromStereoCalib /= nframes*npoints; */ + if (rmsErrorFromStereoCalib > maxReprojErr) { - ts->printf( cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", err, testcase); - ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT ); + ts->printf(cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", + rmsErrorFromStereoCalib, testcase); + ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); + return; + } + + double rmsErrorFromReprojectedImgPts = 0.0f; + if (rotMats1.empty() || transVecs1.empty()) + { + rmsErrorPerViewFromReprojectedImgPts1 = rmsErrorPerView1; + rmsErrorPerViewFromReprojectedImgPts2 = rmsErrorPerView2; + rmsErrorFromReprojectedImgPts = rmsErrorFromStereoCalib; + } + else + { + vector reprojectedImgPts[2] = {vector(nframes), vector(nframes)}; + size_t totalPoints = 0; + double totalErr[2] = { 0, 0 }, viewErr[2]; + for (size_t i = 0; i < objpt.size(); ++i) { + RotMat r1 = rotMats1[i]; + Vec3d t1 = transVecs1[i]; + + RotMat r2 = Mat(R * r1); + Mat T2t = R * t1; + Vec3d t2 = Mat(T2t + T); + + projectPoints(objpt[i], r1, t1, M1, D1, reprojectedImgPts[0]); + projectPoints(objpt[i], r2, t2, M2, D2, reprojectedImgPts[1]); + + viewErr[0] = cv::norm(imgpt1[i], reprojectedImgPts[0], cv::NORM_L2SQR); + viewErr[1] = cv::norm(imgpt2[i], reprojectedImgPts[1], cv::NORM_L2SQR); + + size_t n = objpt[i].size(); + totalErr[0] += viewErr[0]; + totalErr[1] += viewErr[1]; + totalPoints += n; + + rmsErrorPerViewFromReprojectedImgPts1[i] = sqrt(viewErr[0] / n); + rmsErrorPerViewFromReprojectedImgPts2[i] = sqrt(viewErr[1] / n); + } + rmsErrorFromReprojectedImgPts = std::sqrt((totalErr[0] + totalErr[1]) / (2 * totalPoints)); + + } + + if (abs(rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts) > maxDiffBtwRmsErrors) + { + ts->printf(cvtest::TS::LOG, + "The difference of the average reprojection error from the calibration function and from the " + "reprojected image points is too big (|%g - %g| = %g), testcase %d\n", + rmsErrorFromStereoCalib, rmsErrorFromReprojectedImgPts, + (rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts), testcase); + ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); + return; + } + + /* ----- Compare per view rms re-projection errors ----- */ + CV_Assert(rmsErrorPerView1.size() == (size_t)nframes); + CV_Assert(rmsErrorPerViewFromReprojectedImgPts1.size() == (size_t)nframes); + CV_Assert(rmsErrorPerView2.size() == (size_t)nframes); + CV_Assert(rmsErrorPerViewFromReprojectedImgPts2.size() == (size_t)nframes); + int code1 = compare(&rmsErrorPerView1[0], &rmsErrorPerViewFromReprojectedImgPts1[0], nframes, + maxDiffBtwRmsErrors, "per view errors vector"); + int code2 = compare(&rmsErrorPerView2[0], &rmsErrorPerViewFromReprojectedImgPts2[0], nframes, + maxDiffBtwRmsErrors, "per view errors vector"); + if (code1 < 0) + { + ts->printf(cvtest::TS::LOG, + "Some of the per view rms reprojection errors differ between calibration function and reprojected " + "points, for the first camera, testcase %d\n", + testcase); + ts->set_failed_test_info(code1); + return; + } + if (code2 < 0) + { + ts->printf(cvtest::TS::LOG, + "Some of the per view rms reprojection errors differ between calibration function and reprojected " + "points, for the second camera, testcase %d\n", + testcase); + ts->set_failed_test_info(code2); return; } @@ -1640,7 +1740,10 @@ protected: Mat& cameraMatrix1, Mat& distCoeffs1, Mat& cameraMatrix2, Mat& distCoeffs2, Size imageSize, Mat& R, Mat& T, - Mat& E, Mat& F, TermCriteria criteria, int flags ); + Mat& E, Mat& F, + std::vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ); virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1, const Mat& cameraMatrix2, const Mat& distCoeffs2, Size imageSize, const Mat& R, const Mat& T, @@ -1664,11 +1767,53 @@ double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector& rotationMatrices, std::vector& translationVectors, + vector& perViewErrors1, vector& perViewErrors2, + TermCriteria criteria, int flags ) { - return stereoCalibrate( objectPoints, imagePoints1, imagePoints2, + vector rvecs, tvecs; + Mat perViewErrorsMat; + double avgErr = stereoCalibrate( objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, - imageSize, R, T, E, F, flags, criteria ); + imageSize, R, T, E, F, + rvecs, tvecs, perViewErrorsMat, + flags, criteria ); + + size_t numImgs = imagePoints1.size(); + + if (perViewErrors1.size() != numImgs) + { + perViewErrors1.resize(numImgs); + } + if (perViewErrors2.size() != numImgs) + { + perViewErrors2.resize(numImgs); + } + + for (size_t i = 0; i(i, 0); + perViewErrors2[i] = perViewErrorsMat.at(i, 1); + } + + if (rotationMatrices.size() != numImgs) + { + rotationMatrices.resize(numImgs); + } + if (translationVectors.size() != numImgs) + { + translationVectors.resize(numImgs); + } + + for( size_t i = 0; i < numImgs; i++ ) + { + Mat r9; + cv::Rodrigues( rvecs[i], r9 ); + r9.convertTo(rotationMatrices[i], CV_64F); + tvecs[i].convertTo(translationVectors[i], CV_64F); + } + return avgErr; } void CV_StereoCalibrationTest_CPP::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1, From 66cbb7b9111705f1b555f0fbd741c34821702209 Mon Sep 17 00:00:00 2001 From: Stefan Spiss Date: Thu, 15 Sep 2022 11:55:25 +0200 Subject: [PATCH 4/4] Extended tests for stereoCalibrate function of fisheye camera model. --- modules/calib3d/test/test_fisheye.cpp | 105 ++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index 23cfa98889..fe4100d831 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -861,6 +861,111 @@ TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber) cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6)); } +TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations) +{ + const int n_images = 34; + + const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + cv::Matx33d K1, K2, theR; + cv::Vec3d theT; + cv::Vec4d D1, D2; + + std::vector rvecs, tvecs; + + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + + double rmsErrorStereoCalib = cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, theR, theT, rvecs, tvecs, flag, + cv::TermCriteria(3, 12, 0)); + + std::vector reprojectedImgPts[2] = {std::vector(n_images), std::vector(n_images)}; + size_t totalPoints = 0; + float totalMSError[2] = { 0, 0 }, viewMSError[2]; + for( size_t i = 0; i < n_images; i++ ) + { + cv::Matx33d viewRotMat1, viewRotMat2; + cv::Vec3d viewT1, viewT2; + cv::Mat rVec; + cv::Rodrigues( rvecs[i], rVec ); + rVec.convertTo(viewRotMat1, CV_64F); + tvecs[i].convertTo(viewT1, CV_64F); + + viewRotMat2 = theR * viewRotMat1; + cv::Vec3d T2t = theR * viewT1; + viewT2 = T2t + theT; + + cv::Vec3d viewRotVec1, viewRotVec2; + cv::Rodrigues(viewRotMat1, viewRotVec1); + cv::Rodrigues(viewRotMat2, viewRotVec2); + + double alpha1 = K1(0, 1) / K1(0, 0); + double alpha2 = K2(0, 1) / K2(0, 0); + cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[0], viewRotVec1, viewT1, K1, D1, alpha1); + cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[1], viewRotVec2, viewT2, K2, D2, alpha2); + + viewMSError[0] = cv::norm(leftPoints[i], reprojectedImgPts[0], cv::NORM_L2SQR); + viewMSError[1] = cv::norm(rightPoints[i], reprojectedImgPts[1], cv::NORM_L2SQR); + + size_t n = objectPoints[i].size(); + totalMSError[0] += viewMSError[0]; + totalMSError[1] += viewMSError[1]; + totalPoints += n; + } + double rmsErrorFromReprojectedImgPts = std::sqrt((totalMSError[0] + totalMSError[1]) / (2 * totalPoints)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); + cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); + cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412, + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); + + cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); + + cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); + cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); + + EXPECT_MAT_NEAR(theR, R_correct, 1e-10); + EXPECT_MAT_NEAR(theT, T_correct, 1e-10); + + EXPECT_MAT_NEAR(K1, K1_correct, 1e-10); + EXPECT_MAT_NEAR(K2, K2_correct, 1e-10); + + EXPECT_MAT_NEAR(D1, D1_correct, 1e-10); + EXPECT_MAT_NEAR(D2, D2_correct, 1e-10); + + EXPECT_NEAR(rmsErrorStereoCalib, rmsErrorFromReprojectedImgPts, 1e-4); +} + TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify) { cv::Size size(1920, 1080);