clean up fisheye calibration code

improves performance by factor 1.2 to 2.0

modules/calib3d/src/fisheye.cpp

@@ -762,12 +762,12 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
 
         double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0);
 
-        Mat JJ2_inv, ex3;
-        ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2_inv, ex3);
+        Mat JJ2, ex3;
+        ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2, ex3);
 
-        Mat G =  alpha_smooth2 * JJ2_inv * ex3;
-
-        currentParam = finalParam + G;
+        Mat G;
+        solve(JJ2, ex3, G);
+        currentParam = finalParam + alpha_smooth2*G;
 
         change = norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]) -
                 Vec4d(finalParam.f[0], finalParam.f[1], finalParam.c[0], finalParam.c[1]))
@@ -923,7 +923,6 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
 
     cv::Mat J = cv::Mat::zeros(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1),
             e = cv::Mat::zeros(4 * n_points * n_images, 1, CV_64FC1), Jkk, ekk;
-    cv::Mat J2_inv;
 
     for(int iter = 0; ; ++iter)
     {
@@ -1001,11 +1000,10 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
 
         //update all parameters
         cv::subMatrix(J, J, selectedParams, std::vector<int>(J.rows, 1));
-        cv::Mat J2 = J.t() * J;
-        J2_inv = J2.inv();
         int a = cv::countNonZero(intrinsicLeft.isEstimate);
         int b = cv::countNonZero(intrinsicRight.isEstimate);
-        cv::Mat deltas = J2_inv * J.t() * e;
+        cv::Mat deltas;
+        solve(J.t() * J, J.t()*e, deltas);
         intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a);
         intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b);
         omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3));
@@ -1054,10 +1052,10 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
 namespace cv{ namespace {
 void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows)
 {
-    CV_Assert(src.type() == CV_64FC1);
+    CV_Assert(src.channels() == 1);
 
     int nonzeros_cols = cv::countNonZero(cols);
-    Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
+    Mat tmp(src.rows, nonzeros_cols, CV_64F);
 
     for (int i = 0, j = 0; i < (int)cols.size(); i++)
     {
@@ -1068,16 +1066,14 @@ void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std
     }
 
     int nonzeros_rows  = cv::countNonZero(rows);
-    Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1);
+    dst.create(nonzeros_rows, nonzeros_cols, CV_64F);
     for (int i = 0, j = 0; i < (int)rows.size(); i++)
     {
         if (rows[i])
         {
-            tmp.row(i).copyTo(tmp1.row(j++));
+            tmp.row(i).copyTo(dst.row(j++));
         }
     }
-
-    dst = tmp1.clone();
 }
 
 }}
@@ -1386,7 +1382,7 @@ void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArr
 
 void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
                       const IntrinsicParams& param,  InputArray omc, InputArray Tc,
-                      const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3)
+                      const int& check_cond, const double& thresh_cond, Mat& JJ2, Mat& ex3)
 {
     CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
     CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
@@ -1396,7 +1392,7 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
 
     int n = (int)objectPoints.total();
 
-    Mat JJ3 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1);
+    JJ2 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1);
     ex3 = Mat::zeros(9 + 6 * n, 1, CV_64FC1 );
 
     for (int image_idx = 0; image_idx < n; ++image_idx)
@@ -1422,16 +1418,14 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
         Mat B = jacobians.colRange(8, 14).clone();
         B = B.t();
 
-        JJ3(Rect(0, 0, 9, 9)) = JJ3(Rect(0, 0, 9, 9)) + A * A.t();
-        JJ3(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t();
-
-        Mat AB = A * B.t();
-        AB.copyTo(JJ3(Rect(9 + 6 * image_idx, 0, 6, 9)));
+        JJ2(Rect(0, 0, 9, 9)) += A * A.t();
+        JJ2(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t();
 
-        JJ3(Rect(0, 9 + 6 * image_idx, 9, 6)) = AB.t();
-        ex3(Rect(0,0,1,9)) = ex3(Rect(0,0,1,9)) + A * exkk.reshape(1, 2 * exkk.rows);
+        JJ2(Rect(9 + 6 * image_idx, 0, 6, 9)) = A * B.t();
+        JJ2(Rect(0, 9 + 6 * image_idx, 9, 6)) = JJ2(Rect(9 + 6 * image_idx, 0, 6, 9)).t();
 
-        ex3(Rect(0, 9 + 6 * image_idx, 1, 6)) = B * exkk.reshape(1, 2 * exkk.rows);
+        ex3.rowRange(0, 9) += A * exkk.reshape(1, 2 * exkk.rows);
+        ex3.rowRange(9 + 6 * image_idx, 9 + 6 * (image_idx + 1)) = B * exkk.reshape(1, 2 * exkk.rows);
 
         if (check_cond)
         {
@@ -1444,9 +1438,8 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
     std::vector<int> idxs(param.isEstimate);
     idxs.insert(idxs.end(), 6 * n, 1);
 
-    subMatrix(JJ3, JJ3, idxs, idxs);
+    subMatrix(JJ2, JJ2, idxs, idxs);
     subMatrix(ex3, ex3, std::vector<int>(1, 1), idxs);
-    JJ2_inv = JJ3.inv();
 }
 
 void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
@@ -1478,30 +1471,17 @@ void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputA
     meanStdDev(ex, noArray(), std_err);
     std_err *= sqrt((double)ex.total()/((double)ex.total() - 1.0));
 
-    Mat sigma_x;
+    Vec<double, 1> sigma_x;
     meanStdDev(ex.reshape(1, 1), noArray(), sigma_x);
     sigma_x  *= sqrt(2.0 * (double)ex.total()/(2.0 * (double)ex.total() - 1.0));
 
-    Mat _JJ2_inv, ex3;
-    ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, _JJ2_inv, ex3);
-
-    Mat_<double>& JJ2_inv = (Mat_<double>&)_JJ2_inv;
-
-    sqrt(JJ2_inv, JJ2_inv);
+    Mat JJ2, ex3;
+    ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, JJ2, ex3);
 
-    double s  = sigma_x.at<double>(0);
-    Mat r = 3 * s * JJ2_inv.diag();
-    errors = r;
+    sqrt(JJ2.inv(), JJ2);
 
-    rms = 0;
-    const Vec2d* ptr_ex = ex.ptr<Vec2d>();
-    for (size_t i = 0; i < ex.total(); i++)
-    {
-        rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
-    }
-
-    rms /= (double)ex.total();
-    rms = sqrt(rms);
+    errors = 3 * sigma_x(0) * JJ2.diag();
+    rms = sqrt(norm(ex, NORM_L2SQR)/ex.total());
 }
 
 void cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB)