diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 78c066afb..fcc6ad83a 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/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(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& cols, const std::vector& 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& 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(); + JJ2(Rect(0, 0, 9, 9)) += A * A.t(); + JJ2(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(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(); - 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); - - 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 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(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 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 JJ2, ex3; + ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, JJ2, ex3); - Mat_& JJ2_inv = (Mat_&)_JJ2_inv; + sqrt(JJ2.inv(), JJ2); - sqrt(JJ2_inv, JJ2_inv); - - double s = sigma_x.at(0); - Mat r = 3 * s * JJ2_inv.diag(); - errors = r; - - rms = 0; - const Vec2d* ptr_ex = ex.ptr(); - 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)