diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 8caebd363..b495fbe2d 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -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)); }; } diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index a9172c9b2..19b30428e 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -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& cols, const vector& 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 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()); + 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(i)) > abs_max) + { + abs_max = fabs(ekk.at(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(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& cols, const vector& 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(ij, kj) = B.getMat().at(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(tmp.total() / 2); + else return 0.5 *(tmp.at(tmp.total() / 2) + tmp.at(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))); +} diff --git a/modules/calib3d/src/fisheye.hpp b/modules/calib3d/src/fisheye.hpp index e000e635a..fa4bfdb38 100644 --- a/modules/calib3d/src/fisheye.hpp +++ b/modules/calib3d/src/fisheye.hpp @@ -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