1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #include "test_precomp.hpp" 44 #include <opencv2/ts/cuda_test.hpp> 45 #include "../src/fisheye.hpp" 46 47 class fisheyeTest : public ::testing::Test { 48 49 protected: 50 const static cv::Size imageSize; 51 const static cv::Matx33d K; 52 const static cv::Vec4d D; 53 const static cv::Matx33d R; 54 const static cv::Vec3d T; 55 std::string datasets_repository_path; 56 57 virtual void SetUp() { 58 datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye"); 59 } 60 61 protected: 62 std::string combine(const std::string& _item1, const std::string& _item2); 63 cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r); 64 }; 65 66 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 67 /// TESTS:: 68 69 TEST_F(fisheyeTest, projectPoints) 70 { 71 double cols = this->imageSize.width, 72 rows = this->imageSize.height; 73 74 const int N = 20; 75 cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2; 76 undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3)); 77 cv::Vec2d* pts = distorted0.ptr<cv::Vec2d>(); 78 79 cv::Vec2d c(this->K(0, 2), this->K(1, 2)); 80 for(int y = 0, k = 0; y < N; ++y) 81 for(int x = 0; x < N; ++x) 82 { 83 cv::Vec2d point(x*cols/(N-1.f), y*rows/(N-1.f)); 84 pts[k++] = (point - c) * 0.85 + c; 85 } 86 87 cv::fisheye::undistortPoints(distorted0, undist1, this->K, this->D); 88 89 cv::Vec2d* u1 = undist1.ptr<cv::Vec2d>(); 90 cv::Vec3d* u2 = undist2.ptr<cv::Vec3d>(); 91 for(int i = 0; i < (int)distorted0.total(); ++i) 92 u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0); 93 94 cv::fisheye::distortPoints(undist1, distorted1, this->K, this->D); 95 cv::fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D); 96 97 EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10); 98 EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10); 99 } 100 101 TEST_F(fisheyeTest, DISABLED_undistortImage) 102 { 103 cv::Matx33d K = this->K; 104 cv::Mat D = cv::Mat(this->D); 105 std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg"); 106 cv::Matx33d newK = K; 107 cv::Mat distorted = cv::imread(file), undistorted; 108 { 109 newK(0, 0) = 100; 110 newK(1, 1) = 100; 111 cv::fisheye::undistortImage(distorted, undistorted, K, D, newK); 112 cv::Mat correct = cv::imread(combine(datasets_repository_path, "new_f_100.png")); 113 if (correct.empty()) 114 CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_f_100.png"), undistorted)); 115 else 116 EXPECT_MAT_NEAR(correct, undistorted, 1e-10); 117 } 118 { 119 double balance = 1.0; 120 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance); 121 cv::fisheye::undistortImage(distorted, undistorted, K, D, newK); 122 cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_1.0.png")); 123 if (correct.empty()) 124 CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_1.0.png"), undistorted)); 125 else 126 EXPECT_MAT_NEAR(correct, undistorted, 1e-10); 127 } 128 129 { 130 double balance = 0.0; 131 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance); 132 cv::fisheye::undistortImage(distorted, undistorted, K, D, newK); 133 cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_0.0.png")); 134 if (correct.empty()) 135 CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_0.0.png"), undistorted)); 136 else 137 EXPECT_MAT_NEAR(correct, undistorted, 1e-10); 138 } 139 } 140 141 TEST_F(fisheyeTest, jacobians) 142 { 143 int n = 10; 144 cv::Mat X(1, n, CV_64FC3); 145 cv::Mat om(3, 1, CV_64F), T(3, 1, CV_64F); 146 cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F); 147 cv::Mat k(4, 1, CV_64F); 148 double alpha; 149 150 cv::RNG r; 151 152 r.fill(X, cv::RNG::NORMAL, 2, 1); 153 X = cv::abs(X) * 10; 154 155 r.fill(om, cv::RNG::NORMAL, 0, 1); 156 om = cv::abs(om); 157 158 r.fill(T, cv::RNG::NORMAL, 0, 1); 159 T = cv::abs(T); T.at<double>(2) = 4; T *= 10; 160 161 r.fill(f, cv::RNG::NORMAL, 0, 1); 162 f = cv::abs(f) * 1000; 163 164 r.fill(c, cv::RNG::NORMAL, 0, 1); 165 c = cv::abs(c) * 1000; 166 167 r.fill(k, cv::RNG::NORMAL, 0, 1); 168 k*= 0.5; 169 170 alpha = 0.01*r.gaussian(1); 171 172 cv::Mat x1, x2, xpred; 173 cv::Matx33d K(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0), 174 0, f.at<double>(1), c.at<double>(1), 175 0, 0, 1); 176 177 cv::Mat jacobians; 178 cv::fisheye::projectPoints(X, x1, om, T, K, k, alpha, jacobians); 179 180 //test on T: 181 cv::Mat dT(3, 1, CV_64FC1); 182 r.fill(dT, cv::RNG::NORMAL, 0, 1); 183 dT *= 1e-9*cv::norm(T); 184 cv::Mat T2 = T + dT; 185 cv::fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray()); 186 xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1); 187 CV_Assert (cv::norm(x2 - xpred) < 1e-10); 188 189 //test on om: 190 cv::Mat dom(3, 1, CV_64FC1); 191 r.fill(dom, cv::RNG::NORMAL, 0, 1); 192 dom *= 1e-9*cv::norm(om); 193 cv::Mat om2 = om + dom; 194 cv::fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray()); 195 xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1); 196 CV_Assert (cv::norm(x2 - xpred) < 1e-10); 197 198 //test on f: 199 cv::Mat df(2, 1, CV_64FC1); 200 r.fill(df, cv::RNG::NORMAL, 0, 1); 201 df *= 1e-9*cv::norm(f); 202 cv::Matx33d K2 = K + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0); 203 cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray()); 204 xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1); 205 CV_Assert (cv::norm(x2 - xpred) < 1e-10); 206 207 //test on c: 208 cv::Mat dc(2, 1, CV_64FC1); 209 r.fill(dc, cv::RNG::NORMAL, 0, 1); 210 dc *= 1e-9*cv::norm(c); 211 K2 = K + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0); 212 cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray()); 213 xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1); 214 CV_Assert (cv::norm(x2 - xpred) < 1e-10); 215 216 //test on k: 217 cv::Mat dk(4, 1, CV_64FC1); 218 r.fill(dk, cv::RNG::NORMAL, 0, 1); 219 dk *= 1e-9*cv::norm(k); 220 cv::Mat k2 = k + dk; 221 cv::fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray()); 222 xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1); 223 CV_Assert (cv::norm(x2 - xpred) < 1e-10); 224 225 //test on alpha: 226 cv::Mat dalpha(1, 1, CV_64FC1); 227 r.fill(dalpha, cv::RNG::NORMAL, 0, 1); 228 dalpha *= 1e-9*cv::norm(f); 229 double alpha2 = alpha + dalpha.at<double>(0); 230 K2 = K + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0); 231 cv::fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray()); 232 xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1); 233 CV_Assert (cv::norm(x2 - xpred) < 1e-10); 234 } 235 236 TEST_F(fisheyeTest, Calibration) 237 { 238 const int n_images = 34; 239 240 std::vector<std::vector<cv::Point2d> > imagePoints(n_images); 241 std::vector<std::vector<cv::Point3d> > objectPoints(n_images); 242 243 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); 244 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); 245 CV_Assert(fs_left.isOpened()); 246 for(int i = 0; i < n_images; ++i) 247 fs_left[cv::format("image_%d", i )] >> imagePoints[i]; 248 fs_left.release(); 249 250 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); 251 CV_Assert(fs_object.isOpened()); 252 for(int i = 0; i < n_images; ++i) 253 fs_object[cv::format("image_%d", i )] >> objectPoints[i]; 254 fs_object.release(); 255 256 int flag = 0; 257 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; 258 flag |= cv::fisheye::CALIB_CHECK_COND; 259 flag |= cv::fisheye::CALIB_FIX_SKEW; 260 261 cv::Matx33d K; 262 cv::Vec4d D; 263 264 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D, 265 cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6)); 266 267 EXPECT_MAT_NEAR(K, this->K, 1e-10); 268 EXPECT_MAT_NEAR(D, this->D, 1e-10); 269 } 270 271 TEST_F(fisheyeTest, Homography) 272 { 273 const int n_images = 1; 274 275 std::vector<std::vector<cv::Point2d> > imagePoints(n_images); 276 std::vector<std::vector<cv::Point3d> > objectPoints(n_images); 277 278 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); 279 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); 280 CV_Assert(fs_left.isOpened()); 281 for(int i = 0; i < n_images; ++i) 282 fs_left[cv::format("image_%d", i )] >> imagePoints[i]; 283 fs_left.release(); 284 285 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); 286 CV_Assert(fs_object.isOpened()); 287 for(int i = 0; i < n_images; ++i) 288 fs_object[cv::format("image_%d", i )] >> objectPoints[i]; 289 fs_object.release(); 290 291 cv::internal::IntrinsicParams param; 292 param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI), 293 cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5)); 294 295 cv::Mat _imagePoints (imagePoints[0]); 296 cv::Mat _objectPoints(objectPoints[0]); 297 298 cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t(); 299 _objectPoints = _objectPoints.reshape(1).t(); 300 cv::Mat objectPointsMean, covObjectPoints; 301 302 int Np = imagePointsNormalized.cols; 303 cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS); 304 cv::SVD svd(covObjectPoints); 305 cv::Mat R(svd.vt); 306 307 if (cv::norm(R(cv::Rect(2, 0, 1, 2))) < 1e-6) 308 R = cv::Mat::eye(3,3, CV_64FC1); 309 if (cv::determinant(R) < 0) 310 R = -R; 311 312 cv::Mat T = -R * objectPointsMean; 313 cv::Mat X_new = R * _objectPoints + T * cv::Mat::ones(1, Np, CV_64FC1); 314 cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2)); 315 316 cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1); 317 X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2)); 318 cv::Mat mrep = H * M; 319 320 cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep); 321 322 cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t(); 323 324 cv::Vec2d std_err; 325 cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err); 326 std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1)); 327 328 cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901); 329 EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12); 330 } 331 332 TEST_F(fisheyeTest, EtimateUncertainties) 333 { 334 const int n_images = 34; 335 336 std::vector<std::vector<cv::Point2d> > imagePoints(n_images); 337 std::vector<std::vector<cv::Point3d> > objectPoints(n_images); 338 339 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); 340 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); 341 CV_Assert(fs_left.isOpened()); 342 for(int i = 0; i < n_images; ++i) 343 fs_left[cv::format("image_%d", i )] >> imagePoints[i]; 344 fs_left.release(); 345 346 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); 347 CV_Assert(fs_object.isOpened()); 348 for(int i = 0; i < n_images; ++i) 349 fs_object[cv::format("image_%d", i )] >> objectPoints[i]; 350 fs_object.release(); 351 352 int flag = 0; 353 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; 354 flag |= cv::fisheye::CALIB_CHECK_COND; 355 flag |= cv::fisheye::CALIB_FIX_SKEW; 356 357 cv::Matx33d K; 358 cv::Vec4d D; 359 std::vector<cv::Vec3d> rvec; 360 std::vector<cv::Vec3d> tvec; 361 362 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D, 363 rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6)); 364 365 cv::internal::IntrinsicParams param, errors; 366 cv::Vec2d err_std; 367 double thresh_cond = 1e6; 368 int check_cond = 1; 369 param.Init(cv::Vec2d(K(0,0), K(1,1)), cv::Vec2d(K(0,2), K(1, 2)), D); 370 param.isEstimate = std::vector<int>(9, 1); 371 param.isEstimate[4] = 0; 372 373 errors.isEstimate = param.isEstimate; 374 375 double rms; 376 377 cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec, 378 errors, err_std, thresh_cond, check_cond, rms); 379 380 EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-10); 381 EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10); 382 EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10); 383 EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10); 384 CV_Assert(fabs(rms - 0.263782587133546) < 1e-10); 385 CV_Assert(errors.alpha == 0); 386 } 387 388 #ifdef HAVE_TEGRA_OPTIMIZATION 389 // not passing accuracy constrains 390 TEST_F(fisheyeTest, DISABLED_rectify) 391 #else 392 TEST_F(fisheyeTest, rectify) 393 #endif 394 { 395 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); 396 397 cv::Size calibration_size = this->imageSize, requested_size = calibration_size; 398 cv::Matx33d K1 = this->K, K2 = K1; 399 cv::Mat D1 = cv::Mat(this->D), D2 = D1; 400 401 cv::Vec3d T = this->T; 402 cv::Matx33d R = this->R; 403 404 double balance = 0.0, fov_scale = 1.1; 405 cv::Mat R1, R2, P1, P2, Q; 406 cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, R, T, R1, R2, P1, P2, Q, 407 cv::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale); 408 409 cv::Mat lmapx, lmapy, rmapx, rmapy; 410 //rewrite for fisheye 411 cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy); 412 cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy); 413 414 cv::Mat l, r, lundist, rundist; 415 cv::VideoCapture lcap(combine(folder, "left/stereo_pair_%03d.jpg")), 416 rcap(combine(folder, "right/stereo_pair_%03d.jpg")); 417 418 for(int i = 0;; ++i) 419 { 420 lcap >> l; rcap >> r; 421 if (l.empty() || r.empty()) 422 break; 423 424 int ndisp = 128; 425 cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255)); 426 cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255)); 427 cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), cv::Scalar(0, 0, 255)); 428 cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR); 429 cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR); 430 431 cv::Mat rectification = mergeRectification(lundist, rundist); 432 433 cv::Mat correct = cv::imread(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i))); 434 435 if (correct.empty()) 436 cv::imwrite(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i)), rectification); 437 else 438 EXPECT_MAT_NEAR(correct, rectification, 1e-10); 439 } 440 } 441 442 TEST_F(fisheyeTest, stereoCalibrate) 443 { 444 const int n_images = 34; 445 446 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); 447 448 std::vector<std::vector<cv::Point2d> > leftPoints(n_images); 449 std::vector<std::vector<cv::Point2d> > rightPoints(n_images); 450 std::vector<std::vector<cv::Point3d> > objectPoints(n_images); 451 452 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); 453 CV_Assert(fs_left.isOpened()); 454 for(int i = 0; i < n_images; ++i) 455 fs_left[cv::format("image_%d", i )] >> leftPoints[i]; 456 fs_left.release(); 457 458 cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); 459 CV_Assert(fs_right.isOpened()); 460 for(int i = 0; i < n_images; ++i) 461 fs_right[cv::format("image_%d", i )] >> rightPoints[i]; 462 fs_right.release(); 463 464 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); 465 CV_Assert(fs_object.isOpened()); 466 for(int i = 0; i < n_images; ++i) 467 fs_object[cv::format("image_%d", i )] >> objectPoints[i]; 468 fs_object.release(); 469 470 cv::Matx33d K1, K2, R; 471 cv::Vec3d T; 472 cv::Vec4d D1, D2; 473 474 int flag = 0; 475 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; 476 flag |= cv::fisheye::CALIB_CHECK_COND; 477 flag |= cv::fisheye::CALIB_FIX_SKEW; 478 // flag |= cv::fisheye::CALIB_FIX_INTRINSIC; 479 480 cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, 481 K1, D1, K2, D2, imageSize, R, T, flag, 482 cv::TermCriteria(3, 12, 0)); 483 484 cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, 485 -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, 486 -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); 487 cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); 488 cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412, 489 0, 562.849402029712, 380.555455380889, 490 0, 0, 1); 491 492 cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, 493 0, 561.90171021422, 380.401340535339, 494 0, 0, 1); 495 496 cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); 497 cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); 498 499 EXPECT_MAT_NEAR(R, R_correct, 1e-10); 500 EXPECT_MAT_NEAR(T, T_correct, 1e-10); 501 502 EXPECT_MAT_NEAR(K1, K1_correct, 1e-10); 503 EXPECT_MAT_NEAR(K2, K2_correct, 1e-10); 504 505 EXPECT_MAT_NEAR(D1, D1_correct, 1e-10); 506 EXPECT_MAT_NEAR(D2, D2_correct, 1e-10); 507 508 } 509 510 TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic) 511 { 512 const int n_images = 34; 513 514 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY"); 515 516 std::vector<std::vector<cv::Point2d> > leftPoints(n_images); 517 std::vector<std::vector<cv::Point2d> > rightPoints(n_images); 518 std::vector<std::vector<cv::Point3d> > objectPoints(n_images); 519 520 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); 521 CV_Assert(fs_left.isOpened()); 522 for(int i = 0; i < n_images; ++i) 523 fs_left[cv::format("image_%d", i )] >> leftPoints[i]; 524 fs_left.release(); 525 526 cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); 527 CV_Assert(fs_right.isOpened()); 528 for(int i = 0; i < n_images; ++i) 529 fs_right[cv::format("image_%d", i )] >> rightPoints[i]; 530 fs_right.release(); 531 532 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); 533 CV_Assert(fs_object.isOpened()); 534 for(int i = 0; i < n_images; ++i) 535 fs_object[cv::format("image_%d", i )] >> objectPoints[i]; 536 fs_object.release(); 537 538 cv::Matx33d R; 539 cv::Vec3d T; 540 541 int flag = 0; 542 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; 543 flag |= cv::fisheye::CALIB_CHECK_COND; 544 flag |= cv::fisheye::CALIB_FIX_SKEW; 545 flag |= cv::fisheye::CALIB_FIX_INTRINSIC; 546 547 cv::Matx33d K1 (561.195925927249, 0, 621.282400272412, 548 0, 562.849402029712, 380.555455380889, 549 0, 0, 1); 550 551 cv::Matx33d K2 (560.395452535348, 0, 678.971652040359, 552 0, 561.90171021422, 380.401340535339, 553 0, 0, 1); 554 555 cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); 556 cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); 557 558 cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, 559 K1, D1, K2, D2, imageSize, R, T, flag, 560 cv::TermCriteria(3, 12, 0)); 561 562 cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, 563 -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, 564 -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); 565 cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); 566 567 568 EXPECT_MAT_NEAR(R, R_correct, 1e-10); 569 EXPECT_MAT_NEAR(T, T_correct, 1e-10); 570 } 571 572 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 573 /// fisheyeTest:: 574 575 const cv::Size fisheyeTest::imageSize(1280, 800); 576 577 const cv::Matx33d fisheyeTest::K(558.478087865323, 0, 620.458515360843, 578 0, 560.506767351568, 381.939424848348, 579 0, 0, 1); 580 581 const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371); 582 583 const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03, 584 -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02, 585 -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01); 586 587 const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04); 588 589 std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2) 590 { 591 std::string item1 = _item1, item2 = _item2; 592 std::replace(item1.begin(), item1.end(), '\\', '/'); 593 std::replace(item2.begin(), item2.end(), '\\', '/'); 594 595 if (item1.empty()) 596 return item2; 597 598 if (item2.empty()) 599 return item1; 600 601 char last = item1[item1.size()-1]; 602 return item1 + (last != '/' ? "/" : "") + item2; 603 } 604 605 cv::Mat fisheyeTest::mergeRectification(const cv::Mat& l, const cv::Mat& r) 606 { 607 CV_Assert(l.type() == r.type() && l.size() == r.size()); 608 cv::Mat merged(l.rows, l.cols * 2, l.type()); 609 cv::Mat lpart = merged.colRange(0, l.cols); 610 cv::Mat rpart = merged.colRange(l.cols, merged.cols); 611 l.copyTo(lpart); 612 r.copyTo(rpart); 613 614 for(int i = 0; i < l.rows; i+=20) 615 cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), cv::Scalar(0, 255, 0)); 616 617 return merged; 618 } 619
