modules/calib3d/test/test_fisheye.cpp

   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> // EXPECT_MAT_NEAR
  45 #include "../src/fisheye.hpp"
  46 #include "opencv2/videoio.hpp"
  47
  48 namespace opencv_test { namespace {
  49
  50 class fisheyeTest : public ::testing::Test {
  51
  52 protected:
  53     const static cv::Size imageSize;
  54     const static cv::Matx33d K;
  55     const static cv::Vec4d D;
  56     const static cv::Matx33d R;
  57     const static cv::Vec3d T;
  58     std::string datasets_repository_path;
  59
  60     virtual void SetUp() {
  61         datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye");
  62     }
  63
  64 protected:
  65     std::string combine(const std::string& _item1, const std::string& _item2);
  66     static void merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged);
  67 };
  68
  69 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  70 ///  TESTS::
  71
  72 TEST_F(fisheyeTest, projectPoints)
  73 {
  74     double cols = this->imageSize.width,
  75            rows = this->imageSize.height;
  76
  77     const int N = 20;
  78     cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2;
  79     undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3));
  80     cv::Vec2d* pts = distorted0.ptr<cv::Vec2d>();
  81
  82     cv::Vec2d c(this->K(0, 2), this->K(1, 2));
  83     for(int y = 0, k = 0; y < N; ++y)
  84         for(int x = 0; x < N; ++x)
  85         {
  86             cv::Vec2d point(x*cols/(N-1.f), y*rows/(N-1.f));
  87             pts[k++] = (point - c) * 0.85 + c;
  88         }
  89
  90     cv::fisheye::undistortPoints(distorted0, undist1, this->K, this->D);
  91
  92     cv::Vec2d* u1 = undist1.ptr<cv::Vec2d>();
  93     cv::Vec3d* u2 = undist2.ptr<cv::Vec3d>();
  94     for(int i = 0; i  < (int)distorted0.total(); ++i)
  95         u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0);
  96
  97     cv::fisheye::distortPoints(undist1, distorted1, this->K, this->D);
  98     cv::fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D);
  99
 100     EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10);
 101     EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10);
 102 }
 103
 104 TEST_F(fisheyeTest, undistortImage)
 105 {
 106     cv::Matx33d theK = this->K;
 107     cv::Mat theD = cv::Mat(this->D);
 108     std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg");
 109     cv::Matx33d newK = theK;
 110     cv::Mat distorted = cv::imread(file), undistorted;
 111     {
 112         newK(0, 0) = 100;
 113         newK(1, 1) = 100;
 114         cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
 115         cv::Mat correct = cv::imread(combine(datasets_repository_path, "new_f_100.png"));
 116         if (correct.empty())
 117             CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_f_100.png"), undistorted));
 118         else
 119             EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
 120     }
 121     {
 122         double balance = 1.0;
 123         cv::fisheye::estimateNewCameraMatrixForUndistortRectify(theK, theD, distorted.size(), cv::noArray(), newK, balance);
 124         cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
 125         cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_1.0.png"));
 126         if (correct.empty())
 127             CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_1.0.png"), undistorted));
 128         else
 129             EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
 130     }
 131
 132     {
 133         double balance = 0.0;
 134         cv::fisheye::estimateNewCameraMatrixForUndistortRectify(theK, theD, distorted.size(), cv::noArray(), newK, balance);
 135         cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
 136         cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_0.0.png"));
 137         if (correct.empty())
 138             CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_0.0.png"), undistorted));
 139         else
 140             EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
 141     }
 142 }
 143
 144 TEST_F(fisheyeTest, undistortAndDistortImage)
 145 {
 146     cv::Matx33d K_src = this->K;
 147     cv::Mat D_src = cv::Mat(this->D);
 148     std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg");
 149     cv::Matx33d K_dst = K_src;
 150     cv::Mat image = cv::imread(file), image_projected;
 151     cv::Vec4d D_dst_vec (-1.0, 0.0, 0.0, 0.0);
 152     cv::Mat D_dst = cv::Mat(D_dst_vec);
 153
 154     int imageWidth = (int)this->imageSize.width;
 155     int imageHeight = (int)this->imageSize.height;
 156
 157     cv::Mat imagePoints(imageHeight, imageWidth, CV_32FC2), undPoints, distPoints;
 158     cv::Vec2f* pts = imagePoints.ptr<cv::Vec2f>();
 159
 160     for(int y = 0, k = 0; y < imageHeight; ++y)
 161     {
 162         for(int x = 0; x < imageWidth; ++x)
 163         {
 164             cv::Vec2f point((float)x, (float)y);
 165             pts[k++] = point;
 166         }
 167     }
 168
 169     cv::fisheye::undistortPoints(imagePoints, undPoints, K_dst, D_dst);
 170     cv::fisheye::distortPoints(undPoints, distPoints, K_src, D_src);
 171     cv::remap(image, image_projected, distPoints, cv::noArray(), cv::INTER_LINEAR);
 172
 173     float dx, dy, r_sq;
 174     float R_MAX = 250;
 175     float imageCenterX = (float)imageWidth / 2;
 176     float imageCenterY = (float)imageHeight / 2;
 177
 178     cv::Mat undPointsGt(imageHeight, imageWidth, CV_32FC2);
 179     cv::Mat imageGt(imageHeight, imageWidth, CV_8UC3);
 180
 181     for(int y = 0, k = 0; y < imageHeight; ++y)
 182     {
 183         for(int x = 0; x < imageWidth; ++x)
 184         {
 185             dx = x - imageCenterX;
 186             dy = y - imageCenterY;
 187             r_sq = dy * dy + dx * dx;
 188
 189             Vec2f & und_vec = undPoints.at<Vec2f>(y,x);
 190             Vec3b & pixel = image_projected.at<Vec3b>(y,x);
 191
 192             Vec2f & undist_vec_gt = undPointsGt.at<Vec2f>(y,x);
 193             Vec3b & pixel_gt = imageGt.at<Vec3b>(y,x);
 194
 195             if (r_sq > R_MAX * R_MAX)
 196             {
 197
 198                 undist_vec_gt[0] = -1e6;
 199                 undist_vec_gt[1] = -1e6;
 200
 201                 pixel_gt[0] = 0;
 202                 pixel_gt[1] = 0;
 203                 pixel_gt[2] = 0;
 204             }
 205             else
 206             {
 207                 undist_vec_gt[0] = und_vec[0];
 208                 undist_vec_gt[1] = und_vec[1];
 209
 210                 pixel_gt[0] = pixel[0];
 211                 pixel_gt[1] = pixel[1];
 212                 pixel_gt[2] = pixel[2];
 213             }
 214
 215             k++;
 216         }
 217     }
 218
 219     EXPECT_MAT_NEAR(undPoints, undPointsGt, 1e-10);
 220     EXPECT_MAT_NEAR(image_projected, imageGt, 1e-10);
 221
 222     Vec2f dist_point_1 = distPoints.at<Vec2f>(400, 640);
 223     Vec2f dist_point_1_gt(640.044f, 400.041f);
 224
 225     Vec2f dist_point_2 = distPoints.at<Vec2f>(400, 440);
 226     Vec2f dist_point_2_gt(409.731f, 403.029f);
 227
 228     Vec2f dist_point_3 = distPoints.at<Vec2f>(200, 640);
 229     Vec2f dist_point_3_gt(643.341f, 168.896f);
 230
 231     Vec2f dist_point_4 = distPoints.at<Vec2f>(300, 480);
 232     Vec2f dist_point_4_gt(463.402f, 290.317f);
 233
 234     Vec2f dist_point_5 = distPoints.at<Vec2f>(550, 750);
 235     Vec2f dist_point_5_gt(797.51f, 611.637f);
 236
 237     EXPECT_MAT_NEAR(dist_point_1, dist_point_1_gt, 1e-2);
 238     EXPECT_MAT_NEAR(dist_point_2, dist_point_2_gt, 1e-2);
 239     EXPECT_MAT_NEAR(dist_point_3, dist_point_3_gt, 1e-2);
 240     EXPECT_MAT_NEAR(dist_point_4, dist_point_4_gt, 1e-2);
 241     EXPECT_MAT_NEAR(dist_point_5, dist_point_5_gt, 1e-2);
 242
 243     CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_distortion.png"), image_projected));
 244 }
 245
 246 TEST_F(fisheyeTest, jacobians)
 247 {
 248     int n = 10;
 249     cv::Mat X(1, n, CV_64FC3);
 250     cv::Mat om(3, 1, CV_64F), theT(3, 1, CV_64F);
 251     cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
 252     cv::Mat k(4, 1, CV_64F);
 253     double alpha;
 254
 255     cv::RNG r;
 256
 257     r.fill(X, cv::RNG::NORMAL, 2, 1);
 258     X = cv::abs(X) * 10;
 259
 260     r.fill(om, cv::RNG::NORMAL, 0, 1);
 261     om = cv::abs(om);
 262
 263     r.fill(theT, cv::RNG::NORMAL, 0, 1);
 264     theT = cv::abs(theT); theT.at<double>(2) = 4; theT *= 10;
 265
 266     r.fill(f, cv::RNG::NORMAL, 0, 1);
 267     f = cv::abs(f) * 1000;
 268
 269     r.fill(c, cv::RNG::NORMAL, 0, 1);
 270     c = cv::abs(c) * 1000;
 271
 272     r.fill(k, cv::RNG::NORMAL, 0, 1);
 273     k*= 0.5;
 274
 275     alpha = 0.01*r.gaussian(1);
 276
 277     cv::Mat x1, x2, xpred;
 278     cv::Matx33d theK(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0),
 279                      0,            f.at<double>(1), c.at<double>(1),
 280                      0,            0,    1);
 281
 282     cv::Mat jacobians;
 283     cv::fisheye::projectPoints(X, x1, om, theT, theK, k, alpha, jacobians);
 284
 285     //test on T:
 286     cv::Mat dT(3, 1, CV_64FC1);
 287     r.fill(dT, cv::RNG::NORMAL, 0, 1);
 288     dT *= 1e-9*cv::norm(theT);
 289     cv::Mat T2 = theT + dT;
 290     cv::fisheye::projectPoints(X, x2, om, T2, theK, k, alpha, cv::noArray());
 291     xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
 292     CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 293
 294     //test on om:
 295     cv::Mat dom(3, 1, CV_64FC1);
 296     r.fill(dom, cv::RNG::NORMAL, 0, 1);
 297     dom *= 1e-9*cv::norm(om);
 298     cv::Mat om2 = om + dom;
 299     cv::fisheye::projectPoints(X, x2, om2, theT, theK, k, alpha, cv::noArray());
 300     xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
 301     CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 302
 303     //test on f:
 304     cv::Mat df(2, 1, CV_64FC1);
 305     r.fill(df, cv::RNG::NORMAL, 0, 1);
 306     df *= 1e-9*cv::norm(f);
 307     cv::Matx33d K2 = theK + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0);
 308     cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
 309     xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
 310     CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 311
 312     //test on c:
 313     cv::Mat dc(2, 1, CV_64FC1);
 314     r.fill(dc, cv::RNG::NORMAL, 0, 1);
 315     dc *= 1e-9*cv::norm(c);
 316     K2 = theK + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0);
 317     cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
 318     xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
 319     CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 320
 321     //test on k:
 322     cv::Mat dk(4, 1, CV_64FC1);
 323     r.fill(dk, cv::RNG::NORMAL, 0, 1);
 324     dk *= 1e-9*cv::norm(k);
 325     cv::Mat k2 = k + dk;
 326     cv::fisheye::projectPoints(X, x2, om, theT, theK, k2, alpha, cv::noArray());
 327     xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
 328     CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 329
 330     //test on alpha:
 331     cv::Mat dalpha(1, 1, CV_64FC1);
 332     r.fill(dalpha, cv::RNG::NORMAL, 0, 1);
 333     dalpha *= 1e-9*cv::norm(f);
 334     double alpha2 = alpha + dalpha.at<double>(0);
 335     K2 = theK + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0);
 336     cv::fisheye::projectPoints(X, x2, om, theT, theK, k, alpha2, cv::noArray());
 337     xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
 338     CV_Assert (cv::norm(x2 - xpred) < 1e-10);
 339 }
 340
 341 TEST_F(fisheyeTest, Calibration)
 342 {
 343     const int n_images = 34;
 344
 345     std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
 346     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 347
 348     const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
 349     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
 350     CV_Assert(fs_left.isOpened());
 351     for(int i = 0; i < n_images; ++i)
 352         fs_left[cv::format("image_%d", i )] >> imagePoints[i];
 353     fs_left.release();
 354
 355     cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
 356     CV_Assert(fs_object.isOpened());
 357     for(int i = 0; i < n_images; ++i)
 358         fs_object[cv::format("image_%d", i )] >> objectPoints[i];
 359     fs_object.release();
 360
 361     int flag = 0;
 362     flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
 363     flag |= cv::fisheye::CALIB_CHECK_COND;
 364     flag |= cv::fisheye::CALIB_FIX_SKEW;
 365
 366     cv::Matx33d theK;
 367     cv::Vec4d theD;
 368
 369     cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
 370                            cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
 371
 372     EXPECT_MAT_NEAR(theK, this->K, 1e-10);
 373     EXPECT_MAT_NEAR(theD, this->D, 1e-10);
 374 }
 375
 376 TEST_F(fisheyeTest, Homography)
 377 {
 378     const int n_images = 1;
 379
 380     std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
 381     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 382
 383     const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
 384     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
 385     CV_Assert(fs_left.isOpened());
 386     for(int i = 0; i < n_images; ++i)
 387         fs_left[cv::format("image_%d", i )] >> imagePoints[i];
 388     fs_left.release();
 389
 390     cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
 391     CV_Assert(fs_object.isOpened());
 392     for(int i = 0; i < n_images; ++i)
 393         fs_object[cv::format("image_%d", i )] >> objectPoints[i];
 394     fs_object.release();
 395
 396     cv::internal::IntrinsicParams param;
 397     param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI),
 398                cv::Vec2d(imageSize.width  / 2.0 - 0.5, imageSize.height / 2.0 - 0.5));
 399
 400     cv::Mat _imagePoints (imagePoints[0]);
 401     cv::Mat _objectPoints(objectPoints[0]);
 402
 403     cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t();
 404     _objectPoints = _objectPoints.reshape(1).t();
 405     cv::Mat objectPointsMean, covObjectPoints;
 406
 407     int Np = imagePointsNormalized.cols;
 408     cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS);
 409     cv::SVD svd(covObjectPoints);
 410     cv::Mat theR(svd.vt);
 411
 412     if (cv::norm(theR(cv::Rect(2, 0, 1, 2))) < 1e-6)
 413         theR = cv::Mat::eye(3,3, CV_64FC1);
 414     if (cv::determinant(theR) < 0)
 415         theR = -theR;
 416
 417     cv::Mat theT = -theR * objectPointsMean;
 418     cv::Mat X_new = theR * _objectPoints + theT * cv::Mat::ones(1, Np, CV_64FC1);
 419     cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2));
 420
 421     cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1);
 422     X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2));
 423     cv::Mat mrep = H * M;
 424
 425     cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep);
 426
 427     cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t();
 428
 429     cv::Vec2d std_err;
 430     cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err);
 431     std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1));
 432
 433     cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901);
 434     EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12);
 435 }
 436
 437 TEST_F(fisheyeTest, EstimateUncertainties)
 438 {
 439     const int n_images = 34;
 440
 441     std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
 442     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 443
 444     const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
 445     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
 446     CV_Assert(fs_left.isOpened());
 447     for(int i = 0; i < n_images; ++i)
 448         fs_left[cv::format("image_%d", i )] >> imagePoints[i];
 449     fs_left.release();
 450
 451     cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
 452     CV_Assert(fs_object.isOpened());
 453     for(int i = 0; i < n_images; ++i)
 454         fs_object[cv::format("image_%d", i )] >> objectPoints[i];
 455     fs_object.release();
 456
 457     int flag = 0;
 458     flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
 459     flag |= cv::fisheye::CALIB_CHECK_COND;
 460     flag |= cv::fisheye::CALIB_FIX_SKEW;
 461
 462     cv::Matx33d theK;
 463     cv::Vec4d theD;
 464     std::vector<cv::Vec3d> rvec;
 465     std::vector<cv::Vec3d> tvec;
 466
 467     cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
 468                            rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6));
 469
 470     cv::internal::IntrinsicParams param, errors;
 471     cv::Vec2d err_std;
 472     double thresh_cond = 1e6;
 473     int check_cond = 1;
 474     param.Init(cv::Vec2d(theK(0,0), theK(1,1)), cv::Vec2d(theK(0,2), theK(1, 2)), theD);
 475     param.isEstimate = std::vector<uchar>(9, 1);
 476     param.isEstimate[4] = 0;
 477
 478     errors.isEstimate = param.isEstimate;
 479
 480     double rms;
 481
 482     cv::internal::EstimateUncertainties(objectPoints, imagePoints, param,  rvec, tvec,
 483                                         errors, err_std, thresh_cond, check_cond, rms);
 484
 485     EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046,  1.31565641071524), 1e-10);
 486     EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10);
 487     EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10);
 488     EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10);
 489     CV_Assert(fabs(rms - 0.263782587133546) < 1e-10);
 490     CV_Assert(errors.alpha == 0);
 491 }
 492
 493 TEST_F(fisheyeTest, stereoRectify)
 494 {
 495     const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
 496
 497     cv::Size calibration_size = this->imageSize, requested_size = calibration_size;
 498     cv::Matx33d K1 = this->K, K2 = K1;
 499     cv::Mat D1 = cv::Mat(this->D), D2 = D1;
 500
 501     cv::Vec3d theT = this->T;
 502     cv::Matx33d theR = this->R;
 503
 504     double balance = 0.0, fov_scale = 1.1;
 505     cv::Mat R1, R2, P1, P2, Q;
 506     cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, theR, theT, R1, R2, P1, P2, Q,
 507                       cv::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale);
 508
 509     // Collected with these CMake flags: -DWITH_IPP=OFF -DCV_ENABLE_INTRINSICS=OFF -DCV_DISABLE_OPTIMIZATION=ON -DCMAKE_BUILD_TYPE=Debug
 510     cv::Matx33d R1_ref(
 511         0.9992853269091279, 0.03779164101000276, -0.0007920188690205426,
 512         -0.03778569762983931, 0.9992646472015868, 0.006511981857667881,
 513         0.001037534936357442, -0.006477400933964018, 0.9999784831677112
 514     );
 515     cv::Matx33d R2_ref(
 516         0.9994868963898833, -0.03197579751378937, -0.001868774538573449,
 517         0.03196298186616116, 0.9994677442608699, -0.0065265589947392,
 518         0.002076471801477729, 0.006463478587068991, 0.9999769555891836
 519     );
 520     cv::Matx34d P1_ref(
 521         420.8551870450913, 0, 586.501617798451, 0,
 522         0, 420.8551870450913, 374.7667511986098, 0,
 523         0, 0, 1, 0
 524     );
 525     cv::Matx34d P2_ref(
 526         420.8551870450913, 0, 586.501617798451, -41.77758076597302,
 527         0, 420.8551870450913, 374.7667511986098, 0,
 528         0, 0, 1, 0
 529     );
 530     cv::Matx44d Q_ref(
 531         1, 0, 0, -586.501617798451,
 532         0, 1, 0, -374.7667511986098,
 533         0, 0, 0, 420.8551870450913,
 534         0, 0, 10.07370889670733, -0
 535     );
 536
 537     const double eps = 1e-10;
 538     EXPECT_MAT_NEAR(R1_ref, R1, eps);
 539     EXPECT_MAT_NEAR(R2_ref, R2, eps);
 540     EXPECT_MAT_NEAR(P1_ref, P1, eps);
 541     EXPECT_MAT_NEAR(P2_ref, P2, eps);
 542     EXPECT_MAT_NEAR(Q_ref, Q, eps);
 543
 544     if (::testing::Test::HasFailure())
 545     {
 546         std::cout << "Actual values are:" << std::endl
 547             << "R1 =" << std::endl << R1 << std::endl
 548             << "R2 =" << std::endl << R2 << std::endl
 549             << "P1 =" << std::endl << P1 << std::endl
 550             << "P2 =" << std::endl << P2 << std::endl
 551             << "Q =" << std::endl << Q << std::endl;
 552     }
 553
 554     if (cvtest::debugLevel == 0)
 555         return;
 556     // DEBUG code is below
 557
 558     cv::Mat lmapx, lmapy, rmapx, rmapy;
 559     //rewrite for fisheye
 560     cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy);
 561     cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy);
 562
 563     cv::Mat l, r, lundist, rundist;
 564     for (int i = 0; i < 34; ++i)
 565     {
 566         SCOPED_TRACE(cv::format("image %d", i));
 567         l = imread(combine(folder, cv::format("left/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
 568         r = imread(combine(folder, cv::format("right/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
 569         ASSERT_FALSE(l.empty());
 570         ASSERT_FALSE(r.empty());
 571
 572         int ndisp = 128;
 573         cv::rectangle(l, cv::Rect(255,       0, 829,       l.rows-1), cv::Scalar(0, 0, 255));
 574         cv::rectangle(r, cv::Rect(255,       0, 829,       l.rows-1), cv::Scalar(0, 0, 255));
 575         cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), cv::Scalar(0, 0, 255));
 576         cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
 577         cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);
 578
 579         for (int ii = 0; ii < lundist.rows; ii += 20)
 580         {
 581             cv::line(lundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
 582             cv::line(rundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
 583         }
 584
 585         cv::Mat rectification;
 586         merge4(l, r, lundist, rundist, rectification);
 587
 588         cv::imwrite(cv::format("fisheye_rectification_AB_%03d.png", i), rectification);
 589     }
 590 }
 591
 592 TEST_F(fisheyeTest, stereoCalibrate)
 593 {
 594     const int n_images = 34;
 595
 596     const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
 597
 598     std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
 599     std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
 600     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 601
 602     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
 603     CV_Assert(fs_left.isOpened());
 604     for(int i = 0; i < n_images; ++i)
 605         fs_left[cv::format("image_%d", i )] >> leftPoints[i];
 606     fs_left.release();
 607
 608     cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
 609     CV_Assert(fs_right.isOpened());
 610     for(int i = 0; i < n_images; ++i)
 611         fs_right[cv::format("image_%d", i )] >> rightPoints[i];
 612     fs_right.release();
 613
 614     cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
 615     CV_Assert(fs_object.isOpened());
 616     for(int i = 0; i < n_images; ++i)
 617         fs_object[cv::format("image_%d", i )] >> objectPoints[i];
 618     fs_object.release();
 619
 620     cv::Matx33d K1, K2, theR;
 621     cv::Vec3d theT;
 622     cv::Vec4d D1, D2;
 623
 624     int flag = 0;
 625     flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
 626     flag |= cv::fisheye::CALIB_CHECK_COND;
 627     flag |= cv::fisheye::CALIB_FIX_SKEW;
 628
 629     cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
 630                     K1, D1, K2, D2, imageSize, theR, theT, flag,
 631                     cv::TermCriteria(3, 12, 0));
 632
 633     cv::Matx33d R_correct(   0.9975587205950972,   0.06953016383322372, 0.006492709911733523,
 634                            -0.06956823121068059,    0.9975601387249519, 0.005833595226966235,
 635                           -0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
 636     cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
 637     cv::Matx33d K1_correct (561.195925927249,                0, 621.282400272412,
 638                                    0, 562.849402029712, 380.555455380889,
 639                                    0,                0,                1);
 640
 641     cv::Matx33d K2_correct (560.395452535348,                0, 678.971652040359,
 642                                    0,  561.90171021422, 380.401340535339,
 643                                    0,                0,                1);
 644
 645     cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
 646     cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
 647
 648     EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
 649     EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
 650
 651     EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
 652     EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
 653
 654     EXPECT_MAT_NEAR(D1, D1_correct, 1e-10);
 655     EXPECT_MAT_NEAR(D2, D2_correct, 1e-10);
 656
 657 }
 658
 659 TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
 660 {
 661     const int n_images = 34;
 662
 663     const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
 664
 665     std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
 666     std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
 667     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 668
 669     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
 670     CV_Assert(fs_left.isOpened());
 671     for(int i = 0; i < n_images; ++i)
 672         fs_left[cv::format("image_%d", i )] >> leftPoints[i];
 673     fs_left.release();
 674
 675     cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
 676     CV_Assert(fs_right.isOpened());
 677     for(int i = 0; i < n_images; ++i)
 678         fs_right[cv::format("image_%d", i )] >> rightPoints[i];
 679     fs_right.release();
 680
 681     cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
 682     CV_Assert(fs_object.isOpened());
 683     for(int i = 0; i < n_images; ++i)
 684         fs_object[cv::format("image_%d", i )] >> objectPoints[i];
 685     fs_object.release();
 686
 687     cv::Matx33d theR;
 688     cv::Vec3d theT;
 689
 690     int flag = 0;
 691     flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
 692     flag |= cv::fisheye::CALIB_CHECK_COND;
 693     flag |= cv::fisheye::CALIB_FIX_SKEW;
 694     flag |= cv::fisheye::CALIB_FIX_INTRINSIC;
 695
 696     cv::Matx33d K1 (561.195925927249,                0, 621.282400272412,
 697                                    0, 562.849402029712, 380.555455380889,
 698                                    0,                0,                1);
 699
 700     cv::Matx33d K2 (560.395452535348,                0, 678.971652040359,
 701                                    0,  561.90171021422, 380.401340535339,
 702                                    0,                0,                1);
 703
 704     cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
 705     cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
 706
 707     cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
 708                     K1, D1, K2, D2, imageSize, theR, theT, flag,
 709                     cv::TermCriteria(3, 12, 0));
 710
 711     cv::Matx33d R_correct(   0.9975587205950972,   0.06953016383322372, 0.006492709911733523,
 712                            -0.06956823121068059,    0.9975601387249519, 0.005833595226966235,
 713                           -0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
 714     cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
 715
 716
 717     EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
 718     EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
 719 }
 720
 721 TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber)
 722 {
 723     const int n_images = 2;
 724
 725     std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
 726     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 727
 728     std::vector<cv::Point2d> imgPoints1(10);
 729     std::vector<cv::Point2d> imgPoints2(15);
 730
 731     std::vector<cv::Point3d> objectPoints1(imgPoints1.size());
 732     std::vector<cv::Point3d> objectPoints2(imgPoints2.size());
 733
 734     for (size_t i = 0; i < imgPoints1.size(); i++)
 735     {
 736         imgPoints1[i] = cv::Point2d((double)i, (double)i);
 737         objectPoints1[i] = cv::Point3d((double)i, (double)i, 10.0);
 738     }
 739
 740     for (size_t i = 0; i < imgPoints2.size(); i++)
 741     {
 742         imgPoints2[i] = cv::Point2d(i + 0.5, i + 0.5);
 743         objectPoints2[i] = cv::Point3d(i + 0.5, i + 0.5, 10.0);
 744     }
 745
 746     imagePoints[0] = imgPoints1;
 747     imagePoints[1] = imgPoints2;
 748     objectPoints[0] = objectPoints1;
 749     objectPoints[1] = objectPoints2;
 750
 751     cv::Matx33d theK = cv::Matx33d::eye();
 752     cv::Vec4d theD;
 753
 754     int flag = 0;
 755     flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
 756     flag |= cv::fisheye::CALIB_USE_INTRINSIC_GUESS;
 757     flag |= cv::fisheye::CALIB_FIX_SKEW;
 758
 759     cv::fisheye::calibrate(objectPoints, imagePoints, cv::Size(100, 100), theK, theD,
 760         cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
 761 }
 762
 763 TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify)
 764 {
 765     cv::Size size(1920, 1080);
 766
 767     cv::Mat K_fullhd(3, 3, cv::DataType<double>::type);
 768     K_fullhd.at<double>(0, 0) = 600.44477382;
 769     K_fullhd.at<double>(0, 1) = 0.0;
 770     K_fullhd.at<double>(0, 2) = 992.06425788;
 771
 772     K_fullhd.at<double>(1, 0) = 0.0;
 773     K_fullhd.at<double>(1, 1) = 578.99298055;
 774     K_fullhd.at<double>(1, 2) = 549.26826242;
 775
 776     K_fullhd.at<double>(2, 0) = 0.0;
 777     K_fullhd.at<double>(2, 1) = 0.0;
 778     K_fullhd.at<double>(2, 2) = 1.0;
 779
 780     cv::Mat K_new_truth(3, 3, cv::DataType<double>::type);
 781
 782     K_new_truth.at<double>(0, 0) = 387.4809086880343;
 783     K_new_truth.at<double>(0, 1) = 0.0;
 784     K_new_truth.at<double>(0, 2) = 1036.669802754649;
 785
 786     K_new_truth.at<double>(1, 0) = 0.0;
 787     K_new_truth.at<double>(1, 1) = 373.6375700303157;
 788     K_new_truth.at<double>(1, 2) = 538.8373261247601;
 789
 790     K_new_truth.at<double>(2, 0) = 0.0;
 791     K_new_truth.at<double>(2, 1) = 0.0;
 792     K_new_truth.at<double>(2, 2) = 1.0;
 793
 794     cv::Mat D_fullhd(4, 1, cv::DataType<double>::type);
 795     D_fullhd.at<double>(0, 0) = -0.05090103223466704;
 796     D_fullhd.at<double>(1, 0) = 0.030944413642173308;
 797     D_fullhd.at<double>(2, 0) = -0.021509225493198905;
 798     D_fullhd.at<double>(3, 0) = 0.0043378096628297145;
 799     cv::Mat E = cv::Mat::eye(3, 3, cv::DataType<double>::type);
 800
 801     cv::Mat K_new(3, 3, cv::DataType<double>::type);
 802
 803     cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K_fullhd, D_fullhd, size, E, K_new, 0.0, size);
 804
 805     EXPECT_MAT_NEAR(K_new, K_new_truth, 1e-6);
 806 }
 807
 808 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 809 ///  fisheyeTest::
 810
 811 const cv::Size fisheyeTest::imageSize(1280, 800);
 812
 813 const cv::Matx33d fisheyeTest::K(558.478087865323,               0, 620.458515360843,
 814                               0, 560.506767351568, 381.939424848348,
 815                               0,               0,                1);
 816
 817 const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
 818
 819 const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
 820                             -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
 821                             -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
 822
 823 const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
 824
 825 std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2)
 826 {
 827     std::string item1 = _item1, item2 = _item2;
 828     std::replace(item1.begin(), item1.end(), '\\', '/');
 829     std::replace(item2.begin(), item2.end(), '\\', '/');
 830
 831     if (item1.empty())
 832         return item2;
 833
 834     if (item2.empty())
 835         return item1;
 836
 837     char last = item1[item1.size()-1];
 838     return item1 + (last != '/' ? "/" : "") + item2;
 839 }
 840
 841 void fisheyeTest::merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged)
 842 {
 843     int type = tl.type();
 844     cv::Size sz = tl.size();
 845     ASSERT_EQ(type, tr.type()); ASSERT_EQ(type, bl.type()); ASSERT_EQ(type, br.type());
 846     ASSERT_EQ(sz.width, tr.cols); ASSERT_EQ(sz.width, bl.cols); ASSERT_EQ(sz.width, br.cols);
 847     ASSERT_EQ(sz.height, tr.rows); ASSERT_EQ(sz.height, bl.rows); ASSERT_EQ(sz.height, br.rows);
 848
 849     merged.create(cv::Size(sz.width * 2, sz.height * 2), type);
 850     tl.copyTo(merged(cv::Rect(0, 0, sz.width, sz.height)));
 851     tr.copyTo(merged(cv::Rect(sz.width, 0, sz.width, sz.height)));
 852     bl.copyTo(merged(cv::Rect(0, sz.height, sz.width, sz.height)));
 853     br.copyTo(merged(cv::Rect(sz.width, sz.height, sz.width, sz.height)));
 854 }
 855
 856 }} // namespace