From 50b291995a71c7176dad21a39b7151cf81644700 Mon Sep 17 00:00:00 2001 From: Ilya Krylov Date: Mon, 5 May 2014 17:23:03 +0400 Subject: [PATCH] Added tests for stereoCalibrate --- .../include/opencv2/calib3d/calib3d.hpp | 6 +- modules/calib3d/src/fisheye.cpp | 196 ++++++--------- modules/calib3d/test/test_fisheye.cpp | 235 ++++++++++++++++++ 3 files changed, 312 insertions(+), 125 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index b495fbe2d..b6d85a095 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -806,10 +806,10 @@ public: //! 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, + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, int flags, TermCriteria criteria = TermCriteria(3, 100, 1e-10)); + }; } diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 19b30428e..50dd04528 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -764,10 +764,8 @@ double cv::Fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray /// 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) + InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, + OutputArray R, OutputArray T, int flags, TermCriteria criteria) { CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty()); CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total()); @@ -780,14 +778,13 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO 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()); + CV_Assert(((flags & CALIB_FIX_INTRINSIC) && !K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC)); //-------------------------------Initialization const int threshold = 50; + const double thresh_cond = 1e6; + const int check_cond = 1; size_t n_points = objectPoints.getMat(0).total(); size_t n_images = objectPoints.total(); @@ -800,38 +797,52 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO cv::internal::IntrinsicParams intrinsicLeft_errors; cv::internal::IntrinsicParams intrinsicRight_errors; - Matx33d _K; - Vec4d _D; + Matx33d _K1, _K2; + Vec4d _D1, _D2; + if (!K1.empty()) K1.getMat().convertTo(_K1, CV_64FC1); + if (!D1.empty()) D1.getMat().convertTo(_D1, CV_64FC1); + if (!K2.empty()) K2.getMat().convertTo(_K2, CV_64FC1); + if (!D2.empty()) D2.getMat().convertTo(_D2, CV_64FC1); - 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)); + std::vector rvecs1(n_images), tvecs1(n_images), rvecs2(n_images), tvecs2(n_images); - 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)); + if (!(flags & CALIB_FIX_INTRINSIC)) + { + calibrate(objectPoints, imagePoints1, imageSize, _K1, _D1, rvecs1, tvecs1, flags, TermCriteria(3, 20, 1e-6)); + calibrate(objectPoints, imagePoints2, imageSize, _K2, _D2, rvecs2, tvecs2, flags, TermCriteria(3, 20, 1e-6)); + } - 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; + intrinsicLeft.Init(Vec2d(_K1(0,0), _K1(1, 1)), Vec2d(_K1(0,2), _K1(1, 2)), + Vec4d(_D1[0], _D1[1], _D1[2], _D1[3]), _K1(0, 1) / _K1(0, 0)); - 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; + intrinsicRight.Init(Vec2d(_K2(0,0), _K2(1, 1)), Vec2d(_K2(0,2), _K2(1, 2)), + Vec4d(_D2[0], _D2[1], _D2[2], _D2[3]), _K2(0, 1) / _K2(0, 0)); + + if ((flags & CALIB_FIX_INTRINSIC)) + { + internal::CalibrateExtrinsics(objectPoints, imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1); + internal::CalibrateExtrinsics(objectPoints, imagePoints2, intrinsicRight, check_cond, thresh_cond, rvecs2, tvecs2); + } + + intrinsicLeft.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicLeft.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicLeft.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1; + + intrinsicRight.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; + intrinsicRight.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1; + intrinsicRight.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1; intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate; intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate; @@ -847,10 +858,10 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO 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); + cv::Rodrigues(rvecs1[image_idx], R1); + cv::Rodrigues(rvecs2[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); + T_ref = cv::Mat(tvecs2[image_idx]) - R_ref * cv::Mat(tvecs1[image_idx]); 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)); @@ -861,6 +872,7 @@ 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) { if ((criteria.type == 1 && iter >= criteria.maxCount) || @@ -885,8 +897,8 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO cv::Mat jacobians, projected; //left camera jacobian - cv::Mat rvec = rvecs1.getMat(image_idx).clone(); - cv::Mat tvec = tvecs1.getMat(image_idx).clone(); + cv::Mat rvec = cv::Mat(rvecs1[image_idx]); + cv::Mat tvec = cv::Mat(tvecs1[image_idx]); 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)); @@ -898,8 +910,8 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO //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(); + rvec = cv::Mat(rvecs2[image_idx]); + tvec = cv::Mat(tvecs2[image_idx]); 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)); @@ -952,103 +964,43 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO 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); + rvecs1[image_idx] = cv::Mat(cv::Mat(rvecs1[image_idx]) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6)); + tvecs1[image_idx] = cv::Mat(cv::Mat(tvecs1[image_idx]) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6)); } 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)); + double rms = 0; + const Vec2d* ptr_e = e.ptr(); + for (size_t i = 0; i < e.total() / 2; i++) + { + rms += ptr_e[i][0] * ptr_e[i][0] + ptr_e[i][1] * ptr_e[i][1]; + } - 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; + rms /= (e.total() / 2); + rms = sqrt(rms); - 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], + _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], + _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 (K1.needed()) cv::Mat(_K1).convertTo(K1, 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()) cv::Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.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; + return rms; } namespace cv{ namespace { diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index 41bbfea59..0caaf7f45 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -368,6 +368,241 @@ TEST_F(FisheyeTest, rectify) } } +TEST_F(FisheyeTest, stereoCalibrate) +{ + const int n_images = 34; + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + std::ofstream fs; + + for (size_t i = 0; i < leftPoints.size(); i++) + { + std::string ss = combine(folder, "left"); + ss = combine_format(ss, "%d", i); + fs.open(ss.c_str()); + CV_Assert(fs.is_open()); + for (size_t j = 0; j < leftPoints[i].size(); j++) + { + double x = leftPoints[i][j].x; + double y = leftPoints[i][j].y; + fs << std::setprecision(15) << x << "; " << y; + fs << std::endl; + } + fs.close(); + } + + for (size_t i = 0; i < rightPoints.size(); i++) + { + std::string ss = combine(folder, "right"); + ss = combine_format(ss, "%d", i); + fs.open(ss.c_str()); + CV_Assert(fs.is_open()); + for (size_t j = 0; j < rightPoints[i].size(); j++) + { + double x = rightPoints[i][j].x; + double y = rightPoints[i][j].y; + fs << std::setprecision(15) << x << "; " << y; + fs << std::endl; + } + fs.close(); + } + + for (size_t i = 0; i < objectPoints.size(); i++) + { + std::string ss = combine(folder, "object"); + ss = combine_format(ss, "%d", i); + fs.open(ss.c_str()); + CV_Assert(fs.is_open()); + for (size_t j = 0; j < objectPoints[i].size(); j++) + { + double x = objectPoints[i][j].x; + double y = objectPoints[i][j].y; + double z = objectPoints[i][j].z; + fs << std::setprecision(15) << x << "; " << y; + fs << std::setprecision(15) << "; " << z; + fs << std::endl; + } + fs.close(); + } + + cv::Matx33d K1, K2, R; + cv::Vec3d T; + cv::Vec4d D1, D2; + + int flag = 0; + flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::Fisheye::CALIB_CHECK_COND; + flag |= cv::Fisheye::CALIB_FIX_SKEW; + // flag |= cv::Fisheye::CALIB_FIX_INTRINSIC; + + cv::Fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, R, T, flag, + cv::TermCriteria(3, 12, 0)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); + cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); + cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412, + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); + + cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); + + cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); + cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); + + EXPECT_MAT_NEAR(R, R_correct, 1e-10); + EXPECT_MAT_NEAR(T, T_correct, 1e-10); + + EXPECT_MAT_NEAR(K1, K1_correct, 1e-10); + EXPECT_MAT_NEAR(K2, K2_correct, 1e-10); + + EXPECT_MAT_NEAR(D1, D1_correct, 1e-10); + EXPECT_MAT_NEAR(D2, D2_correct, 1e-10); + +} + +TEST_F(FisheyeTest, stereoCalibrateFixIntrinsic) +{ + const int n_images = 34; + + const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + + std::ofstream fs; + + for (size_t i = 0; i < leftPoints.size(); i++) + { + std::string ss = combine(folder, "left"); + ss = combine_format(ss, "%d", i); + fs.open(ss.c_str()); + CV_Assert(fs.is_open()); + for (size_t j = 0; j < leftPoints[i].size(); j++) + { + double x = leftPoints[i][j].x; + double y = leftPoints[i][j].y; + fs << std::setprecision(15) << x << "; " << y; + fs << std::endl; + } + fs.close(); + } + + for (size_t i = 0; i < rightPoints.size(); i++) + { + std::string ss = combine(folder, "right"); + ss = combine_format(ss, "%d", i); + fs.open(ss.c_str()); + CV_Assert(fs.is_open()); + for (size_t j = 0; j < rightPoints[i].size(); j++) + { + double x = rightPoints[i][j].x; + double y = rightPoints[i][j].y; + fs << std::setprecision(15) << x << "; " << y; + fs << std::endl; + } + fs.close(); + } + + for (size_t i = 0; i < objectPoints.size(); i++) + { + std::string ss = combine(folder, "object"); + ss = combine_format(ss, "%d", i); + fs.open(ss.c_str()); + CV_Assert(fs.is_open()); + for (size_t j = 0; j < objectPoints[i].size(); j++) + { + double x = objectPoints[i][j].x; + double y = objectPoints[i][j].y; + double z = objectPoints[i][j].z; + fs << std::setprecision(15) << x << "; " << y; + fs << std::setprecision(15) << "; " << z; + fs << std::endl; + } + fs.close(); + } + + cv::Matx33d R; + cv::Vec3d T; + + int flag = 0; + flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::Fisheye::CALIB_CHECK_COND; + flag |= cv::Fisheye::CALIB_FIX_SKEW; + flag |= cv::Fisheye::CALIB_FIX_INTRINSIC; + + cv::Matx33d K1 (561.195925927249, 0, 621.282400272412, + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); + + cv::Matx33d K2 (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); + + cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); + cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); + + cv::Fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, R, T, flag, + cv::TermCriteria(3, 12, 0)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); + cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); + + + EXPECT_MAT_NEAR(R, R_correct, 1e-10); + EXPECT_MAT_NEAR(T, T_correct, 1e-10); +} + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// FisheyeTest::