Extended tests for stereoCalibrate function of pinhole camera model.

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<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
+        vector<double>& perViewErrors1, vector<double>& 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<RotMat> rotMats1(nframes);
+        vector<Vec3d> transVecs1(nframes);
+        vector<RotMat> rotMats2(nframes);
+        vector<Vec3d> transVecs2(nframes);
+        vector<double> rmsErrorPerView1(nframes);
+        vector<double> rmsErrorPerView2(nframes);
+        vector<double> rmsErrorPerViewFromReprojectedImgPts1(nframes);
+        vector<double> 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<double>(0,2) = M2.at<double>(0,2)=(imgsize.width-1)*0.5;
         M1.at<double>(1,2) = M2.at<double>(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;
+        /* rmsErrorFromStereoCalib /= nframes*npoints; */
-        if( err > maxReprojErr )
+        if (rmsErrorFromStereoCalib > maxReprojErr)
         {
-            ts->printf( cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", err, testcase);
+            ts->printf(cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n",
-            ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
+                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<Point2f > reprojectedImgPts[2] = {vector<Point2f>(nframes), vector<Point2f>(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<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
+        vector<double>& perViewErrors1, vector<double>& 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<vector<
                                              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<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
+                                             vector<double>& perViewErrors1, vector<double>& perViewErrors2,
+                                             TermCriteria criteria, int flags )
 {
-    return stereoCalibrate( objectPoints, imagePoints1, imagePoints2,
+    vector<Mat> 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<numImgs; i++)
+    {
+        perViewErrors1[i] = perViewErrorsMat.at<double>(i, 0);
+        perViewErrors2[i] = perViewErrorsMat.at<double>(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,