generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

clean up fisheye calibration code

Browse Source 
improves performance by factor 1.2 to 2.0
This commit is contained in:
Pavel Rojtberg 2015-11-16 00:58:55 +01:00
parent b4112a5878
commit 81e814d9ed
1 changed files with 26 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
modules/calib3d/src/fisheye.cpp
View File

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