Skip to content

Commit

Permalink
Merge pull request opencv#5675 from paroj:fisheyecalib
Browse files Browse the repository at this point in the history
  • Loading branch information
@vpisarev
vpisarev committed Dec 10, 2015
2 parents 8e67f0b + cefa1dc commit 5d6292f
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 54 deletions.
84 changes: 32 additions & 52 deletions modules/calib3d/src/fisheye.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ namespace cv { namespace
double dalpha;
};

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<uchar>& cols, const std::vector<uchar>& rows);
}}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -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]))
Expand Down Expand Up @@ -899,7 +899,7 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate;
intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate;

std::vector<int> selectedParams;
std::vector<uchar> selectedParams;
std::vector<int> 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());
Expand All @@ -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)
{
Expand Down Expand Up @@ -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<int>(J.rows, 1));
cv::Mat J2 = J.t() * J;
J2_inv = J2.inv();
cv::subMatrix(J, J, selectedParams, std::vector<uchar>(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));
Expand Down Expand Up @@ -1052,12 +1050,12 @@ 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)
void subMatrix(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& 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++)
{
Expand All @@ -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();
}

}}
Expand Down Expand Up @@ -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));
Expand All @@ -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)
Expand All @@ -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)
{
Expand All @@ -1441,12 +1435,11 @@ void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayO
}
}

std::vector<int> idxs(param.isEstimate);
std::vector<uchar> idxs(param.isEstimate);
idxs.insert(idxs.end(), 6 * n, 1);

subMatrix(JJ3, JJ3, idxs, idxs);
subMatrix(ex3, ex3, std::vector<int>(1, 1), idxs);
JJ2_inv = JJ3.inv();
subMatrix(JJ2, JJ2, idxs, idxs);
subMatrix(ex3, ex3, std::vector<uchar>(1, 1), idxs);
}

void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
Expand Down Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion modules/calib3d/src/fisheye.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ struct CV_EXPORTS IntrinsicParams
Vec2d c;
Vec4d k;
double alpha;
std::vector<int> isEstimate;
std::vector<uchar> isEstimate;

IntrinsicParams();
IntrinsicParams(Vec2d f, Vec2d c, Vec4d k, double alpha = 0);
Expand Down
2 changes: 1 addition & 1 deletion modules/calib3d/test/test_fisheye.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>(9, 1);
param.isEstimate = std::vector<uchar>(9, 1);
param.isEstimate[4] = 0;

errors.isEstimate = param.isEstimate;
Expand Down

0 comments on commit 5d6292f

Please sign in to comment.