diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 78c066afb5..3d737e7f46 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -53,7 +53,7 @@ namespace cv { namespace double dalpha; }; - void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows); + void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows); }} ////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -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])) @@ -899,7 +899,7 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate; intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate; - std::vector selectedParams; + std::vector selectedParams; std::vector tmp(6 * (n_images + 1), 1); selectedParams.insert(selectedParams.end(), intrinsicLeft.isEstimate.begin(), intrinsicLeft.isEstimate.end()); selectedParams.insert(selectedParams.end(), intrinsicRight.isEstimate.begin(), intrinsicRight.isEstimate.end()); @@ -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) { @@ -1000,12 +999,11 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]); //update all parameters - cv::subMatrix(J, J, selectedParams, std::vector(J.rows, 1)); - cv::Mat J2 = J.t() * J; - J2_inv = J2.inv(); + cv::subMatrix(J, J, selectedParams, std::vector(J.rows, 1)); 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)); @@ -1052,12 +1050,12 @@ 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) +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(); - - 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) { @@ -1441,12 +1435,11 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO } } - std::vector idxs(param.isEstimate); + std::vector idxs(param.isEstimate); idxs.insert(idxs.end(), 6 * n, 1); - subMatrix(JJ3, JJ3, idxs, idxs); - subMatrix(ex3, ex3, std::vector(1, 1), idxs); - JJ2_inv = JJ3.inv(); + subMatrix(JJ2, JJ2, idxs, idxs); + subMatrix(ex3, ex3, std::vector(1, 1), idxs); } 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_inv = (Mat_&)_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(0); - Mat r = 3 * s * JJ2_inv.diag(); - errors = r; + sqrt(JJ2.inv(), JJ2); - 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) diff --git a/modules/calib3d/src/fisheye.hpp b/modules/calib3d/src/fisheye.hpp index 82c9f34598..d46ac86464 100644 --- a/modules/calib3d/src/fisheye.hpp +++ b/modules/calib3d/src/fisheye.hpp @@ -10,7 +10,7 @@ struct CV_EXPORTS IntrinsicParams Vec2d c; Vec4d k; double alpha; - std::vector isEstimate; + std::vector isEstimate; IntrinsicParams(); IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0); diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index 32b77c654e..006f378912 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -368,7 +368,7 @@ TEST_F(fisheyeTest, EtimateUncertainties) double thresh_cond = 1e6; int check_cond = 1; param.Init(cv::Vec2d(theK(0,0), theK(1,1)), cv::Vec2d(theK(0,2), theK(1, 2)), theD); - param.isEstimate = std::vector(9, 1); + param.isEstimate = std::vector(9, 1); param.isEstimate[4] = 0; errors.isEstimate = param.isEstimate;