From 9ca3a3139a54a5aef9e08fa525c9bfd243536460 Mon Sep 17 00:00:00 2001 From: Stefan Spiss Date: Thu, 15 Sep 2022 11:54:31 +0200 Subject: [PATCH] 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,