Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #5675 from paroj:fisheyecalib

Browse Source
pull/5781/head
Vadim Pisarevsky 9 years ago
parent 8e67f0ba84 cefa1dc5cb
commit 5d6292fc97
3 changed files with 34 additions and 54 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 84
      modules/calib3d/src/fisheye.cpp
  2. 2
      modules/calib3d/src/fisheye.hpp
  3. 2
      modules/calib3d/test/test_fisheye.cpp

84
modules/calib3d/src/fisheye.cpp
Unescape View File

@ -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);
}}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
@ -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<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());
@ -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<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));
@ -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++)
{
@ -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)
{
@ -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,
@ -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)

2
modules/calib3d/src/fisheye.hpp
Unescape View File

@ -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);

2
modules/calib3d/test/test_fisheye.cpp
Unescape View File

@ -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;

Write Preview
Loading…
Cancel
Save