Chiebot-Mirror
/
opencv
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added stereoCalibrate for Fisheye camera model

Browse Source
pull/2726/head
Ilya Krylov 11 years ago
parent e6aa8ce932
commit c2341fd446
3 changed files with 434 additions and 1 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. 10
      modules/calib3d/include/opencv2/calib3d/calib3d.hpp
  2. 413
      modules/calib3d/src/fisheye.cpp
  3. 12
      modules/calib3d/src/fisheye.hpp

10
modules/calib3d/include/opencv2/calib3d/calib3d.hpp
Unescape View File

@ -757,7 +757,8 @@ public:
CALIB_FIX_K1 = 16,
CALIB_FIX_K2 = 32,
CALIB_FIX_K3 = 64,
CALIB_FIX_K4 = 128
CALIB_FIX_K4 = 128,
CALIB_FIX_INTRINSIC = 256
};
//! projects 3D points using fisheye model
@ -802,6 +803,13 @@ public:
static void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec,
OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(),
double balance = 0.0, double fov_scale = 1.0);
//! performs stereo calibaration
static double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2,
InputOutputArrayOfArrays rvecs1, InputOutputArrayOfArrays tvecs1,
InputOutputArrayOfArrays rvecs2, InputOutputArrayOfArrays tvecs2,
TermCriteria criteria = TermCriteria(3, 100, 1e-10));
};
}

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

@ -2,6 +2,7 @@
#include "opencv2/core/affine.hpp"
#include "opencv2/core/affine.hpp"
#include "fisheye.hpp"
#include "iomanip"
namespace cv { namespace
{
@ -12,6 +13,8 @@ namespace cv { namespace
Vec3d dom, dT;
double dalpha;
};
void subMatrix(const Mat& src, Mat& dst, const vector<int>& cols, const vector<int>& rows);
}}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
@ -757,6 +760,297 @@ double cv::Fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
return rms;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// cv::Fisheye::stereoCalibrate
double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2,
InputOutputArray rvecs1, InputOutputArrayOfArrays tvecs1,
InputOutputArrayOfArrays rvecs2, InputOutputArrayOfArrays tvecs2,
TermCriteria criteria)
{
CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty());
CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total());
CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2);
CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2);
CV_Assert((!K1.empty() && K1.size() == Size(3,3)) || K1.empty());
CV_Assert((!D1.empty() && D1.total() == 4) || D1.empty());
CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty());
CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty());
CV_Assert((!rvecs1.empty() && rvecs1.channels() == 3) || rvecs1.empty());
CV_Assert((!tvecs1.empty() && tvecs1.channels() == 3) || tvecs1.empty());
CV_Assert((!rvecs2.empty() && rvecs2.channels() == 3) || rvecs2.empty());
CV_Assert((!tvecs2.empty() && tvecs2.channels() == 3) || tvecs2.empty());
//-------------------------------Initialization
const int threshold = 50;
size_t n_points = objectPoints.getMat(0).total();
size_t n_images = objectPoints.total();
double change = 1;
cv::internal::IntrinsicParams intrinsicLeft;
cv::internal::IntrinsicParams intrinsicRight;
cv::internal::IntrinsicParams intrinsicLeft_errors;
cv::internal::IntrinsicParams intrinsicRight_errors;
Matx33d _K;
Vec4d _D;
K1.getMat().convertTo(_K, CV_64FC1);
D1.getMat().convertTo(_D, CV_64FC1);
intrinsicLeft.Init(Vec2d(_K(0,0), _K(1, 1)), Vec2d(_K(0,2), _K(1, 2)),
Vec4d(_D[0], _D[1], _D[2], _D[3]), _K(0, 1) / _K(0, 0));
K2.getMat().convertTo(_K, CV_64FC1);
D2.getMat().convertTo(_D, CV_64FC1);
intrinsicRight.Init(Vec2d(_K(0,0), _K(1, 1)), Vec2d(_K(0,2), _K(1, 2)),
Vec4d(_D[0], _D[1], _D[2], _D[3]), _K(0, 1) / _K(0, 0));
intrinsicLeft.isEstimate[0] = 1;
intrinsicLeft.isEstimate[1] = 1;
intrinsicLeft.isEstimate[2] = 1;
intrinsicLeft.isEstimate[3] = 1;
intrinsicLeft.isEstimate[4] = 0;
intrinsicLeft.isEstimate[5] = 1;
intrinsicLeft.isEstimate[6] = 1;
intrinsicLeft.isEstimate[7] = 1;
intrinsicLeft.isEstimate[8] = 1;
intrinsicRight.isEstimate[0] = 1;
intrinsicRight.isEstimate[1] = 1;
intrinsicRight.isEstimate[2] = 1;
intrinsicRight.isEstimate[3] = 1;
intrinsicRight.isEstimate[4] = 0;
intrinsicRight.isEstimate[5] = 1;
intrinsicRight.isEstimate[6] = 1;
intrinsicRight.isEstimate[7] = 1;
intrinsicRight.isEstimate[8] = 1;
intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate;
intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate;
std::vector<int> 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());
selectedParams.insert(selectedParams.end(), tmp.begin(), tmp.end());
//Init values for rotation and translation between two views
cv::Mat om_list(1, n_images, CV_64FC3), T_list(1, n_images, CV_64FC3);
cv::Mat om_ref, R_ref, T_ref, R1, R2;
for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
{
cv::Rodrigues(rvecs1.getMat(image_idx), R1);
cv::Rodrigues(rvecs2.getMat(image_idx), R2);
R_ref = R2 * R1.t();
T_ref = tvecs2.getMat(image_idx).reshape(1, 3) - R_ref * tvecs1.getMat(image_idx).reshape(1, 3);
cv::Rodrigues(R_ref, om_ref);
om_ref.reshape(3, 1).copyTo(om_list.col(image_idx));
T_ref.reshape(3, 1).copyTo(T_list.col(image_idx));
}
cv::Vec3d omcur = internal::median3d(om_list);
cv::Vec3d Tcur = internal::median3d(T_list);
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)
{
if ((criteria.type == 1 && iter >= criteria.maxCount) ||
(criteria.type == 2 && change <= criteria.epsilon) ||
(criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
break;
J.create(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1);
e.create(4 * n_points * n_images, 1, CV_64FC1);
Jkk.create(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1);
ekk.create(4 * n_points, 1, CV_64FC1);
cv::Mat omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT;
for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
{
Jkk = cv::Mat::zeros(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1);
cv::Mat object = objectPoints.getMat(image_idx).clone();
cv::Mat imageLeft = imagePoints1.getMat(image_idx).clone();
cv::Mat imageRight = imagePoints2.getMat(image_idx).clone();
cv::Mat jacobians, projected;
//left camera jacobian
cv::Mat rvec = rvecs1.getMat(image_idx).clone();
cv::Mat tvec = tvecs1.getMat(image_idx).clone();
cv::internal::projectPoints(object, projected, rvec, tvec, intrinsicLeft, jacobians);
cv::Mat(cv::Mat((imageLeft - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(0, 2 * n_points));
jacobians.colRange(8, 11).copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(0, 2 * n_points));
jacobians.colRange(11, 14).copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(0, 2 * n_points));
jacobians.colRange(0, 2).copyTo(Jkk.colRange(0, 2).rowRange(0, 2 * n_points));
jacobians.colRange(2, 4).copyTo(Jkk.colRange(2, 4).rowRange(0, 2 * n_points));
jacobians.colRange(4, 8).copyTo(Jkk.colRange(5, 9).rowRange(0, 2 * n_points));
jacobians.col(14).copyTo(Jkk.col(4).rowRange(0, 2 * n_points));
//right camera jacobian
internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT);
rvec = rvecs2.getMat(image_idx).clone();
tvec = tvecs2.getMat(image_idx).clone();
cv::internal::projectPoints(object, projected, omr, Tr, intrinsicRight, jacobians);
cv::Mat(cv::Mat((imageRight - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(2 * n_points, 4 * n_points));
cv::Mat dxrdom = jacobians.colRange(8, 11) * domrdom + jacobians.colRange(11, 14) * dTrdom;
cv::Mat dxrdT = jacobians.colRange(8, 11) * domrdT + jacobians.colRange(11, 14)* dTrdT;
cv::Mat dxrdomckk = jacobians.colRange(8, 11) * domrdomckk + jacobians.colRange(11, 14) * dTrdomckk;
cv::Mat dxrdTckk = jacobians.colRange(8, 11) * domrdTckk + jacobians.colRange(11, 14) * dTrdTckk;
dxrdom.copyTo(Jkk.colRange(18, 21).rowRange(2 * n_points, 4 * n_points));
dxrdT.copyTo(Jkk.colRange(21, 24).rowRange(2 * n_points, 4 * n_points));
dxrdomckk.copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(2 * n_points, 4 * n_points));
dxrdTckk.copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(2 * n_points, 4 * n_points));
jacobians.colRange(0, 2).copyTo(Jkk.colRange(9 + 0, 9 + 2).rowRange(2 * n_points, 4 * n_points));
jacobians.colRange(2, 4).copyTo(Jkk.colRange(9 + 2, 9 + 4).rowRange(2 * n_points, 4 * n_points));
jacobians.colRange(4, 8).copyTo(Jkk.colRange(9 + 5, 9 + 9).rowRange(2 * n_points, 4 * n_points));
jacobians.col(14).copyTo(Jkk.col(9 + 4).rowRange(2 * n_points, 4 * n_points));
//check goodness of sterepair
double abs_max = 0;
for (size_t i = 0; i < 4 * n_points; i++)
{
if (fabs(ekk.at<double>(i)) > abs_max)
{
abs_max = fabs(ekk.at<double>(i));
}
}
if (abs_max < threshold)
{
Jkk.copyTo(J.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points));
ekk.copyTo(e.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points));
}
else
{
CV_Assert(!"Bad stereo pair");
}
}
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();
int a = cv::countNonZero(intrinsicLeft.isEstimate);
int b = cv::countNonZero(intrinsicRight.isEstimate);
cv::Mat deltas = J2_inv * J.t() * e;
intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a);
intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b);
omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3));
Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6));
for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
{
rvecs1.getMat(image_idx) = rvecs1.getMat(image_idx) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6).reshape(3);
tvecs1.getMat(image_idx) = tvecs1.getMat(image_idx) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6).reshape(3);
}
cv::Vec6d newTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
change = cv::norm(newTom - oldTom) / cv::norm(newTom);
}
//estimate uncertainties
cv::Mat sigma_x;
cv::meanStdDev(e, cv::noArray(), sigma_x);
sigma_x *= sqrt((double)e.total() / (e.total() - 1));
cv::sqrt(J2_inv, J2_inv);
cv::Mat errors = 3 * J2_inv.diag() * sigma_x;
int a1 = cv::countNonZero(intrinsicLeft_errors.isEstimate);
int b1 = cv::countNonZero(intrinsicRight_errors.isEstimate);
intrinsicLeft_errors = errors.rowRange(0, a1);
intrinsicRight_errors = errors.rowRange(a1, a1 + b1);
cv::Vec3d om_error = cv::Vec3d(errors.rowRange(a1 + b1, a1 + b1 + 3));
cv::Vec3d T_error = cv::Vec3d(errors.rowRange(a1 + b1 + 3, a1 + b1 + 6));
std::cout << std::setprecision(15) << "left f = " << intrinsicLeft.f << std::endl;
std::cout << std::setprecision(15) << "left c = " << intrinsicLeft.c << std::endl;
std::cout << std::setprecision(15) << "left alpha = " << intrinsicLeft.alpha << std::endl;
std::cout << std::setprecision(15) << "left k = " << intrinsicLeft.k << std::endl;
std::cout << std::setprecision(15) << "right f = " << intrinsicRight.f << std::endl;
std::cout << std::setprecision(15) << "right c = " << intrinsicRight.c << std::endl;
std::cout << std::setprecision(15) << "right alpha = " << intrinsicRight.alpha << std::endl;
std::cout << std::setprecision(15) << "right k = " << intrinsicRight.k << std::endl;
std::cout << omcur << std::endl;
std::cout << Tcur << std::endl;
std::cout << "====================================================================================" << std::endl;
std::cout.flush();
std::cout << std::setprecision(15) << "left f = " << intrinsicLeft_errors.f << std::endl;
std::cout << std::setprecision(15) << "left c = " << intrinsicLeft_errors.c << std::endl;
std::cout << std::setprecision(15) << "left alpha = " << intrinsicLeft_errors.alpha << std::endl;
std::cout << std::setprecision(15) << "left k = " << intrinsicLeft_errors.k << std::endl;
std::cout << std::setprecision(15) << "right f = " << intrinsicRight_errors.f << std::endl;
std::cout << std::setprecision(15) << "right c = " << intrinsicRight_errors.c << std::endl;
std::cout << std::setprecision(15) << "right alpha = " << intrinsicRight_errors.alpha << std::endl;
std::cout << std::setprecision(15) << "right k = " << intrinsicRight_errors.k << std::endl;
std::cout << om_error << std::endl;
std::cout << T_error << std::endl;
std::cout << "====================================================================================" << std::endl;
std::cout.flush();
CV_Assert(cv::norm(intrinsicLeft.f - cv::Vec2d(561.195925927249, 562.849402029712)) < 1e-12);
CV_Assert(cv::norm(intrinsicLeft.c - cv::Vec2d(621.282400272412, 380.555455380889)) < 1e-12);
CV_Assert(intrinsicLeft.alpha == 0);
CV_Assert(cv::norm(intrinsicLeft.k - cv::Vec4d(-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771)) < 1e-12);
CV_Assert(cv::norm(intrinsicRight.f - cv::Vec2d(560.395452535348, 561.90171021422)) < 1e-12);
CV_Assert(cv::norm(intrinsicRight.c - cv::Vec2d(678.971652040359, 380.401340535339)) < 1e-12);
CV_Assert(intrinsicRight.alpha == 0);
CV_Assert(cv::norm(intrinsicRight.k - cv::Vec4d(-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222)) < 1e-12);
CV_Assert(cv::norm(omcur - cv::Vec3d(-0.00605728469659871, 0.006287139337868821, -0.06960627514977027)) < 1e-12);
CV_Assert(cv::norm(Tcur - cv::Vec3d(-0.09940272472412097, 0.002708121392654134, 0.001293302924726987)) < 1e-12);
CV_Assert(cv::norm(intrinsicLeft_errors.f - cv::Vec2d(0.71024293066476, 0.717596249442966)) < 1e-12);
CV_Assert(cv::norm(intrinsicLeft_errors.c - cv::Vec2d(0.782164491020839, 0.538718002947604)) < 1e-12);
CV_Assert(intrinsicLeft_errors.alpha == 0);
CV_Assert(cv::norm(intrinsicLeft_errors.k - cv::Vec4d(0.00525819017878291, 0.0179451746982225, 0.0236417266063274, 0.0104757238170252)) < 1e-12);
CV_Assert(cv::norm(intrinsicRight_errors.f - cv::Vec2d(0.748707568264116, 0.745355483082726)) < 1e-12);
CV_Assert(cv::norm(intrinsicRight_errors.c - cv::Vec2d(0.788236834082615, 0.538854504490304)) < 1e-12);
CV_Assert(intrinsicRight_errors.alpha == 0);
CV_Assert(cv::norm(intrinsicRight_errors.k - cv::Vec4d(0.00534743998208779, 0.0175804116710864, 0.0226549382734192, 0.00979255348533809)) < 1e-12);
CV_Assert(cv::norm(om_error - cv::Vec3d(0.0005903298904975326, 0.001048251127879415, 0.0001775640833531587)) < 1e-12);
CV_Assert(cv::norm(T_error - cv::Vec3d(6.691282702437657e-05, 5.566841336891827e-05, 0.0001954901454589594)) < 1e-12);
Matx33d _K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
0, intrinsicLeft.f[1], intrinsicLeft.c[1],
0, 0, 1);
Matx33d _K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0],
0, intrinsicRight.f[1], intrinsicRight.c[1],
0, 0, 1);
Mat _R;
Rodrigues(omcur, _R);
// if (K1.needed()) cv::Mat(_K1).convertTo(K2, K1.empty() ? CV_64FC1 : K1.type());
// if (K2.needed()) cv::Mat(_K2).convertTo(K2, K2.empty() ? CV_64FC1 : K2.type());
// if (D1.needed()) cv::Mat(intrinsicLeft.k).convertTo(D1, D1.empty() ? CV_64FC1 : D1.type());
// if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type());
// if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type());
// if (T.needed()) Tcur.convertTo(R, R.empty() ? CV_64FC1 : R.type());
// if (rvecs1.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
// if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
return 0;
}
namespace cv{ namespace {
void subMatrix(const Mat& src, Mat& dst, const vector<int>& cols, const vector<int>& rows)
{
@ -1216,3 +1510,122 @@ void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputA
rms /= ex.total();
rms = sqrt(rms);
}
void cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB)
{
CV_Assert(A.getMat().cols == B.getMat().rows);
CV_Assert(A.type() == CV_64FC1 && B.type() == CV_64FC1);
size_t p = A.getMat().rows;
size_t n = A.getMat().cols;
size_t q = B.getMat().cols;
dABdA.create(p * q, p * n, CV_64FC1);
dABdB.create(p * q, q * n, CV_64FC1);
dABdA.getMat() = Mat::zeros(p * q, p * n, CV_64FC1);
dABdB.getMat() = Mat::zeros(p * q, q * n, CV_64FC1);
for (size_t i = 0; i < q; ++i)
{
for (size_t j = 0; j < p; ++j)
{
size_t ij = j + i * p;
for (size_t k = 0; k < n; ++k)
{
size_t kj = j + k * p;
dABdA.getMat().at<double>(ij, kj) = B.getMat().at<double>(k, i);
}
}
}
for (size_t i = 0; i < q; ++i)
{
A.getMat().copyTo(dABdB.getMat().rowRange(i * p, i * p + p).colRange(i * n, i * n + n));
}
}
void cv::internal::JRodriguesMatlab(const Mat& src, Mat& dst)
{
Mat tmp(src.cols, src.rows, src.type());
if (src.rows == 9)
{
Mat(src.row(0).t()).copyTo(tmp.col(0));
Mat(src.row(1).t()).copyTo(tmp.col(3));
Mat(src.row(2).t()).copyTo(tmp.col(6));
Mat(src.row(3).t()).copyTo(tmp.col(1));
Mat(src.row(4).t()).copyTo(tmp.col(4));
Mat(src.row(5).t()).copyTo(tmp.col(7));
Mat(src.row(6).t()).copyTo(tmp.col(2));
Mat(src.row(7).t()).copyTo(tmp.col(5));
Mat(src.row(8).t()).copyTo(tmp.col(8));
}
else
{
Mat(src.col(0).t()).copyTo(tmp.row(0));
Mat(src.col(1).t()).copyTo(tmp.row(3));
Mat(src.col(2).t()).copyTo(tmp.row(6));
Mat(src.col(3).t()).copyTo(tmp.row(1));
Mat(src.col(4).t()).copyTo(tmp.row(4));
Mat(src.col(5).t()).copyTo(tmp.row(7));
Mat(src.col(6).t()).copyTo(tmp.row(2));
Mat(src.col(7).t()).copyTo(tmp.row(5));
Mat(src.col(8).t()).copyTo(tmp.row(8));
}
dst = tmp.clone();
}
void cv::internal::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2,
Mat& om3, Mat& T3, Mat& dom3dom1, Mat& dom3dT1, Mat& dom3dom2,
Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2)
{
Mat om1 = _om1.getMat();
Mat om2 = _om2.getMat();
Mat T1 = _T1.getMat().reshape(1, 3);
Mat T2 = _T2.getMat().reshape(1, 3);
//% Rotations:
Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2;
Rodrigues(om1, R1, dR1dom1);
Rodrigues(om2, R2, dR2dom2);
JRodriguesMatlab(dR1dom1, dR1dom1);
JRodriguesMatlab(dR2dom2, dR2dom2);
R3 = R2 * R1;
Mat dR3dR2, dR3dR1;
dAB(R2, R1, dR3dR2, dR3dR1);
Mat dom3dR3;
Rodrigues(R3, om3, dom3dR3);
JRodriguesMatlab(dom3dR3, dom3dR3);
dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;
dom3dT1 = Mat::zeros(3, 3, CV_64FC1);
dom3dT2 = Mat::zeros(3, 3, CV_64FC1);
//% Translations:
Mat T3t = R2 * T1;
Mat dT3tdR2, dT3tdT1;
dAB(R2, T1, dT3tdR2, dT3tdT1);
Mat dT3tdom2 = dT3tdR2 * dR2dom2;
T3 = T3t + T2;
dT3dT1 = dT3tdT1;
dT3dT2 = Mat::eye(3, 3, CV_64FC1);
dT3dom2 = dT3tdom2;
dT3dom1 = Mat::zeros(3, 3, CV_64FC1);
}
double cv::internal::median(const Mat& row)
{
CV_Assert(row.type() == CV_64FC1);
CV_Assert(!row.empty() && row.rows == 1);
Mat tmp = row.clone();
sort(tmp, tmp, 0);
if (tmp.total() % 2) return tmp.at<double>(tmp.total() / 2);
else return 0.5 *(tmp.at<double>(tmp.total() / 2) + tmp.at<double>(tmp.total() / 2 - 1));
}
cv::Vec3d cv::internal::median3d(InputArray m)
{
CV_Assert(m.depth() == CV_64F && m.getMat().rows == 1);
Mat M = Mat(m.getMat().t()).reshape(1).t();
return Vec3d(median(M.row(0)), median(M.row(1)), median(M.row(2)));
}

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

@ -43,6 +43,18 @@ void EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays i
const IntrinsicParams& params, InputArray omc, InputArray Tc,
IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms);
void dAB(cv::InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB);
void JRodriguesMatlab(const Mat& src, Mat& dst);
void compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2,
Mat& om3, Mat& T3, Mat& dom3dom1, Mat& dom3dT1, Mat& dom3dom2,
Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2);
double median(const Mat& row);
Vec3d median3d(InputArray m);
}}
#endif

Write Preview
Loading…
Cancel
Save