From 05ee15f10881265f1e930cc65a83951844f7828a Mon Sep 17 00:00:00 2001 From: Ilya Krylov Date: Mon, 28 Apr 2014 12:01:27 +0400 Subject: [PATCH] Added FisheyeTest --- modules/calib3d/test/test_fisheye.cpp | 337 ++++++++++++++------------ 1 file changed, 177 insertions(+), 160 deletions(-) diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index b2f47987c..e7d4b311c 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -2,138 +2,49 @@ #include #include -#define DEF_PARAM_TEST(name, ...) typedef ::perf::TestBaseWithParam< std::tr1::tuple< __VA_ARGS__ > > name -#define PARAM_TEST_CASE(name, ...) struct name : testing::TestWithParam< std::tr1::tuple< __VA_ARGS__ > > +class FisheyeTest : public ::testing::Test { -/// Change this parameter via CMake: cmake -DDATASETS_REPOSITORY_FOLDER= -//const static std::string datasets_repository_path = "DATASETS_REPOSITORY_FOLDER"; -const static std::string datasets_repository_path = "/home/krylov/data"; +protected: + const static cv::Size imageSize; + const static cv::Matx33d K; + const static cv::Vec4d D; + const static cv::Matx33d R; + const static cv::Vec3d T; + std::string datasets_repository_path; -namespace FishEye -{ - const static cv::Size imageSize(1280, 800); - - const static cv::Matx33d K(558.478087865323, 0, 620.458515360843, - 0, 560.506767351568, 381.939424848348, - 0, 0, 1); - - const static cv::Vec4d D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371); - - const static cv::Matx33d R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03, - -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02, - -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01); - - const static cv::Vec3d T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04); -} - -namespace{ -std::string combine(const std::string& _item1, const std::string& _item2) -{ - std::string item1 = _item1, item2 = _item2; - std::replace(item1.begin(), item1.end(), '\\', '/'); - std::replace(item2.begin(), item2.end(), '\\', '/'); - - if (item1.empty()) - return item2; - - if (item2.empty()) - return item1; - - char last = item1[item1.size()-1]; - return item1 + (last != '/' ? "/" : "") + item2; -} - -std::string combine_format(const std::string& item1, const std::string& item2, ...) -{ - std::string fmt = combine(item1, item2); - char buffer[1 << 16]; - va_list args; - va_start( args, item2 ); - vsprintf( buffer, fmt.c_str(), args ); - va_end( args ); - return std::string(buffer); -} - -void readPoins(std::vector >& objectPoints, - std::vector >& imagePoints, - const std::string& path, const int n_images, const int n_points) -{ - objectPoints.resize(n_images); - imagePoints.resize(n_images); - - std::vector image(n_points); - std::vector object(n_points); - - std::ifstream ipStream; - std::ifstream opStream; - - for (int image_idx = 0; image_idx < n_images; image_idx++) - { - std::stringstream ss; - ss << image_idx; - std::string idxStr = ss.str(); - - ipStream.open(combine(path, std::string(std::string("x_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in); - opStream.open(combine(path, std::string(std::string("X_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in); - CV_Assert(ipStream.is_open() && opStream.is_open()); - - for (int point_idx = 0; point_idx < n_points; point_idx++) - { - double x, y, z; - char delim; - ipStream >> x >> delim >> y; - image[point_idx] = cv::Point2d(x, y); - opStream >> x >> delim >> y >> delim >> z; - object[point_idx] = cv::Point3d(x, y, z); - } - ipStream.close(); - opStream.close(); - - imagePoints[image_idx] = image; - objectPoints[image_idx] = object; + virtual void SetUp() { + datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cameracalibration/fisheye"); } -} -void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2, - cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q) +protected: + std::string combine(const std::string& _item1, const std::string& _item2); + + std::string combine_format(const std::string& item1, const std::string& item2, ...); + + void readPoins(std::vector >& objectPoints, + std::vector >& imagePoints, + const std::string& path, const int n_images, const int n_points); + + void readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2, + cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q); + + cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r); +}; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// TESTS:: + +TEST_F(FisheyeTest, projectPoints) { - cv::FileStorage fs(file, cv::FileStorage::READ); - CV_Assert(fs.isOpened()); - - cv::Mat R, T, R1, R2, P1, P2, Q; - fs["R"] >> R; fs["T"] >> T; fs["R1"] >> R1; fs["R2"] >> R2; fs["P1"] >> P1; fs["P2"] >> P2; fs["Q"] >> Q; - if (_R.needed()) R.copyTo(_R); if(_T.needed()) T.copyTo(_T); if (_R1.needed()) R1.copyTo(_R1); if (_R2.needed()) R2.copyTo(_R2); - if(_P1.needed()) P1.copyTo(_P1); if(_P2.needed()) P2.copyTo(_P2); if(_Q.needed()) Q.copyTo(_Q); -} - -cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r) -{ - CV_Assert(l.type() == r.type() && l.size() == r.size()); - cv::Mat merged(l.rows, l.cols * 2, l.type()); - cv::Mat lpart = merged.colRange(0, l.cols); - cv::Mat rpart = merged.colRange(l.cols, merged.cols); - l.copyTo(lpart); - r.copyTo(rpart); - - for(int i = 0; i < l.rows; i+=20) - cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), CV_RGB(0, 255, 0)); - - return merged; -} - -} - -TEST(FisheyeTest, projectPoints) -{ - double cols = FishEye::imageSize.width, - rows = FishEye::imageSize.height; + double cols = this->imageSize.width, + rows = this->imageSize.height; const int N = 20; cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2; undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3)); cv::Vec2d* pts = distorted0.ptr(); - cv::Vec2d c(FishEye::K(0, 2), FishEye::K(1, 2)); + cv::Vec2d c(this->K(0, 2), this->K(1, 2)); for(int y = 0, k = 0; y < N; ++y) for(int x = 0; x < N; ++x) { @@ -141,29 +52,27 @@ TEST(FisheyeTest, projectPoints) pts[k++] = (point - c) * 0.85 + c; } - cv::Fisheye::undistortPoints(distorted0, undist1, FishEye::K, FishEye::D); + cv::Fisheye::undistortPoints(distorted0, undist1, this->K, this->D); cv::Vec2d* u1 = undist1.ptr(); cv::Vec3d* u2 = undist2.ptr(); for(int i = 0; i < (int)distorted0.total(); ++i) u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0); - cv::Fisheye::distortPoints(undist1, distorted1, FishEye::K, FishEye::D); - cv::Fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), FishEye::K, FishEye::D); + cv::Fisheye::distortPoints(undist1, distorted1, this->K, this->D); + cv::Fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D); - EXPECT_MAT_NEAR(distorted0, distorted1, 1e-5); - EXPECT_MAT_NEAR(distorted0, distorted2, 1e-5); + EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10); + EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10); } -TEST(FisheyeTest, undistortImage) +TEST_F(FisheyeTest, undistortImage) { - cv::Matx33d K = FishEye::K; - cv::Mat D = cv::Mat(FishEye::D); + cv::Matx33d K = this->K; + cv::Mat D = cv::Mat(this->D); std::string file = combine(datasets_repository_path, "image000001.png"); - cv::Matx33d newK = K; cv::Mat distorted = cv::imread(file), undistorted; - { newK(0, 0) = 100; newK(1, 1) = 100; @@ -172,7 +81,7 @@ TEST(FisheyeTest, undistortImage) if (correct.empty()) CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/new_f_100.png"), undistorted)); else - EXPECT_MAT_NEAR(correct, undistorted, 1e-15); + EXPECT_MAT_NEAR(correct, undistorted, 1e-10); } { double balance = 1.0; @@ -182,7 +91,7 @@ TEST(FisheyeTest, undistortImage) if (correct.empty()) CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/balance_1.0.png"), undistorted)); else - EXPECT_MAT_NEAR(correct, undistorted, 1e-15); + EXPECT_MAT_NEAR(correct, undistorted, 1e-10); } { @@ -193,13 +102,13 @@ TEST(FisheyeTest, undistortImage) if (correct.empty()) CV_Assert(cv::imwrite(combine(datasets_repository_path, "test_undistortImage/balance_0.0.png"), undistorted)); else - EXPECT_MAT_NEAR(correct, undistorted, 1e-15); + EXPECT_MAT_NEAR(correct, undistorted, 1e-10); } cv::waitKey(); } -TEST(FisheyeTest, jacobians) +TEST_F(FisheyeTest, jacobians) { int n = 10; cv::Mat X(1, n, CV_64FC3); @@ -245,7 +154,7 @@ TEST(FisheyeTest, jacobians) cv::Mat T2 = T + dT; cv::Fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray()); xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1); - CV_Assert (cv::norm(x2 - xpred) < 1e-12); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); //test on om: cv::Mat dom(3, 1, CV_64FC1); @@ -254,7 +163,7 @@ TEST(FisheyeTest, jacobians) cv::Mat om2 = om + dom; cv::Fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray()); xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1); - CV_Assert (cv::norm(x2 - xpred) < 1e-12); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); //test on f: cv::Mat df(2, 1, CV_64FC1); @@ -263,7 +172,7 @@ TEST(FisheyeTest, jacobians) cv::Matx33d K2 = K + cv::Matx33d(df.at(0), df.at(0) * alpha, 0, 0, df.at(1), 0, 0, 0, 0); cv::Fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray()); xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1); - CV_Assert (cv::norm(x2 - xpred) < 1e-12); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); //test on c: cv::Mat dc(2, 1, CV_64FC1); @@ -272,7 +181,7 @@ TEST(FisheyeTest, jacobians) K2 = K + cv::Matx33d(0, 0, dc.at(0), 0, 0, dc.at(1), 0, 0, 0); cv::Fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray()); xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1); - CV_Assert (cv::norm(x2 - xpred) < 1e-12); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); //test on k: cv::Mat dk(4, 1, CV_64FC1); @@ -281,7 +190,7 @@ TEST(FisheyeTest, jacobians) cv::Mat k2 = k + dk; cv::Fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray()); xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1); - CV_Assert (cv::norm(x2 - xpred) < 1e-12); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); //test on alpha: cv::Mat dalpha(1, 1, CV_64FC1); @@ -291,10 +200,10 @@ TEST(FisheyeTest, jacobians) K2 = K + cv::Matx33d(0, f.at(0) * dalpha.at(0), 0, 0, 0, 0, 0, 0, 0); cv::Fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray()); xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1); - CV_Assert (cv::norm(x2 - xpred) < 1e-12); + CV_Assert (cv::norm(x2 - xpred) < 1e-10); } -TEST(FisheyeTest, Calibration) +TEST_F(FisheyeTest, Calibration) { const int n_images = 34; const int n_points = 48; @@ -316,11 +225,11 @@ TEST(FisheyeTest, Calibration) cv::Fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D, cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6)); - EXPECT_MAT_NEAR(K, FishEye::K, 1e-11); - EXPECT_MAT_NEAR(D, FishEye::D, 1e-12); + EXPECT_MAT_NEAR(K, this->K, 1e-10); + EXPECT_MAT_NEAR(D, this->D, 1e-10); } -TEST(FisheyeTest, Homography) +TEST_F(FisheyeTest, Homography) { const int n_images = 1; const int n_points = 48; @@ -368,10 +277,10 @@ TEST(FisheyeTest, Homography) std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1)); cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901); - EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-16); + EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12); } -TEST(TestFisheye, EtimateUncertainties) +TEST_F(FisheyeTest, EtimateUncertainties) { const int n_images = 34; const int n_points = 48; @@ -393,7 +302,7 @@ TEST(TestFisheye, EtimateUncertainties) std::vector tvec; cv::Fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D, - cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6)); + rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6)); cv::internal::IntrinsicParams param, errors; cv::Vec2d err_std; @@ -410,24 +319,24 @@ TEST(TestFisheye, EtimateUncertainties) cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec, errors, err_std, thresh_cond, check_cond, rms); - EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-14); - EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-15); - EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-15); - EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-15); - CV_Assert(abs(rms - 0.263782587133546) < 1e-15); + EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-10); + EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10); + EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10); + EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10); + CV_Assert(abs(rms - 0.263782587133546) < 1e-10); CV_Assert(errors.alpha == 0); } -TEST(FisheyeTest, rectify) +TEST_F(FisheyeTest, rectify) { const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); - cv::Size calibration_size = FishEye::imageSize, requested_size = calibration_size; - cv::Matx33d K1 = FishEye::K, K2 = K1; - cv::Mat D1 = cv::Mat(FishEye::D), D2 = D1; + cv::Size calibration_size = this->imageSize, requested_size = calibration_size; + cv::Matx33d K1 = this->K, K2 = K1; + cv::Mat D1 = cv::Mat(this->D), D2 = D1; - cv::Vec3d T = FishEye::T; - cv::Matx33d R = FishEye::R; + cv::Vec3d T = this->T; + cv::Matx33d R = this->R; double balance = 0.0, fov_scale = 1.1; cv::Mat R1, R2, P1, P2, Q; @@ -462,11 +371,119 @@ TEST(FisheyeTest, rectify) if (correct.empty()) cv::imwrite(combine_format(folder, "test_rectify/rectification_AB_%03d.png", i), rectification); else - EXPECT_MAT_NEAR(correct, rectification, 1e-15); + EXPECT_MAT_NEAR(correct, rectification, 1e-10); } } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// FisheyeTest:: + +const cv::Size FisheyeTest::imageSize(1280, 800); + +const cv::Matx33d FisheyeTest::K(558.478087865323, 0, 620.458515360843, + 0, 560.506767351568, 381.939424848348, + 0, 0, 1); + +const cv::Vec4d FisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371); + +const cv::Matx33d FisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03, + -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02, + -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01); + +const cv::Vec3d FisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04); +std::string FisheyeTest::combine(const std::string& _item1, const std::string& _item2) +{ + std::string item1 = _item1, item2 = _item2; + std::replace(item1.begin(), item1.end(), '\\', '/'); + std::replace(item2.begin(), item2.end(), '\\', '/'); + if (item1.empty()) + return item2; + + if (item2.empty()) + return item1; + + char last = item1[item1.size()-1]; + return item1 + (last != '/' ? "/" : "") + item2; +} + +std::string FisheyeTest::combine_format(const std::string& item1, const std::string& item2, ...) +{ + std::string fmt = combine(item1, item2); + char buffer[1 << 16]; + va_list args; + va_start( args, item2 ); + vsprintf( buffer, fmt.c_str(), args ); + va_end( args ); + return std::string(buffer); +} + +void FisheyeTest::readPoins(std::vector >& objectPoints, + std::vector >& imagePoints, + const std::string& path, const int n_images, const int n_points) +{ + objectPoints.resize(n_images); + imagePoints.resize(n_images); + + std::vector image(n_points); + std::vector object(n_points); + + std::ifstream ipStream; + std::ifstream opStream; + + for (int image_idx = 0; image_idx < n_images; image_idx++) + { + std::stringstream ss; + ss << image_idx; + std::string idxStr = ss.str(); + + ipStream.open(combine(path, std::string(std::string("x_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in); + opStream.open(combine(path, std::string(std::string("X_") + idxStr + std::string(".csv"))).c_str(), std::ifstream::in); + CV_Assert(ipStream.is_open() && opStream.is_open()); + + for (int point_idx = 0; point_idx < n_points; point_idx++) + { + double x, y, z; + char delim; + ipStream >> x >> delim >> y; + image[point_idx] = cv::Point2d(x, y); + opStream >> x >> delim >> y >> delim >> z; + object[point_idx] = cv::Point3d(x, y, z); + } + ipStream.close(); + opStream.close(); + + imagePoints[image_idx] = image; + objectPoints[image_idx] = object; + } +} + +void FisheyeTest::readExtrinsics(const std::string& file, cv::OutputArray _R, cv::OutputArray _T, cv::OutputArray _R1, cv::OutputArray _R2, + cv::OutputArray _P1, cv::OutputArray _P2, cv::OutputArray _Q) +{ + cv::FileStorage fs(file, cv::FileStorage::READ); + CV_Assert(fs.isOpened()); + + cv::Mat R, T, R1, R2, P1, P2, Q; + fs["R"] >> R; fs["T"] >> T; fs["R1"] >> R1; fs["R2"] >> R2; fs["P1"] >> P1; fs["P2"] >> P2; fs["Q"] >> Q; + if (_R.needed()) R.copyTo(_R); if(_T.needed()) T.copyTo(_T); if (_R1.needed()) R1.copyTo(_R1); if (_R2.needed()) R2.copyTo(_R2); + if(_P1.needed()) P1.copyTo(_P1); if(_P2.needed()) P2.copyTo(_P2); if(_Q.needed()) Q.copyTo(_Q); +} + +cv::Mat FisheyeTest::mergeRectification(const cv::Mat& l, const cv::Mat& r) +{ + CV_Assert(l.type() == r.type() && l.size() == r.size()); + cv::Mat merged(l.rows, l.cols * 2, l.type()); + cv::Mat lpart = merged.colRange(0, l.cols); + cv::Mat rpart = merged.colRange(l.cols, merged.cols); + l.copyTo(lpart); + r.copyTo(rpart); + + for(int i = 0; i < l.rows; i+=20) + cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), CV_RGB(0, 255, 0)); + + return merged; +}