modules/calib3d/test/test_cameracalibration.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 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "test_precomp.hpp"
  43 #include "opencv2/calib3d/calib3d_c.h"
  44
  45 namespace opencv_test { namespace {
  46
  47 #if 0
  48 class CV_ProjectPointsTest : public cvtest::ArrayTest
  49 {
  50 public:
  51     CV_ProjectPointsTest();
  52
  53 protected:
  54     int read_params( const cv::FileStorage& fs );
  55     void fill_array( int test_case_idx, int i, int j, Mat& arr );
  56     int prepare_test_case( int test_case_idx );
  57     void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
  58     double get_success_error_level( int test_case_idx, int i, int j );
  59     void run_func();
  60     void prepare_to_validation( int );
  61
  62     bool calc_jacobians;
  63 };
  64
  65
  66 CV_ProjectPointsTest::CV_ProjectPointsTest()
  67     : cvtest::ArrayTest( "3d-ProjectPoints", "cvProjectPoints2", "" )
  68 {
  69     test_array[INPUT].push_back(NULL);  // rotation vector
  70     test_array[OUTPUT].push_back(NULL); // rotation matrix
  71     test_array[OUTPUT].push_back(NULL); // jacobian (J)
  72     test_array[OUTPUT].push_back(NULL); // rotation vector (backward transform result)
  73     test_array[OUTPUT].push_back(NULL); // inverse transform jacobian (J1)
  74     test_array[OUTPUT].push_back(NULL); // J*J1 (or J1*J) == I(3x3)
  75     test_array[REF_OUTPUT].push_back(NULL);
  76     test_array[REF_OUTPUT].push_back(NULL);
  77     test_array[REF_OUTPUT].push_back(NULL);
  78     test_array[REF_OUTPUT].push_back(NULL);
  79     test_array[REF_OUTPUT].push_back(NULL);
  80
  81     element_wise_relative_error = false;
  82     calc_jacobians = false;
  83 }
  84
  85
  86 int CV_ProjectPointsTest::read_params( const cv::FileStorage& fs )
  87 {
  88     int code = cvtest::ArrayTest::read_params( fs );
  89     return code;
  90 }
  91
  92
  93 void CV_ProjectPointsTest::get_test_array_types_and_sizes(
  94     int /*test_case_idx*/, vector<vector<Size> >& sizes, vector<vector<int> >& types )
  95 {
  96     RNG& rng = ts->get_rng();
  97     int depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F;
  98     int i, code;
  99
 100     code = cvtest::randInt(rng) % 3;
 101     types[INPUT][0] = CV_MAKETYPE(depth, 1);
 102
 103     if( code == 0 )
 104     {
 105         sizes[INPUT][0] = cvSize(1,1);
 106         types[INPUT][0] = CV_MAKETYPE(depth, 3);
 107     }
 108     else if( code == 1 )
 109         sizes[INPUT][0] = cvSize(3,1);
 110     else
 111         sizes[INPUT][0] = cvSize(1,3);
 112
 113     sizes[OUTPUT][0] = cvSize(3, 3);
 114     types[OUTPUT][0] = CV_MAKETYPE(depth, 1);
 115
 116     types[OUTPUT][1] = CV_MAKETYPE(depth, 1);
 117
 118     if( cvtest::randInt(rng) % 2 )
 119         sizes[OUTPUT][1] = cvSize(3,9);
 120     else
 121         sizes[OUTPUT][1] = cvSize(9,3);
 122
 123     types[OUTPUT][2] = types[INPUT][0];
 124     sizes[OUTPUT][2] = sizes[INPUT][0];
 125
 126     types[OUTPUT][3] = types[OUTPUT][1];
 127     sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width);
 128
 129     types[OUTPUT][4] = types[OUTPUT][1];
 130     sizes[OUTPUT][4] = cvSize(3,3);
 131
 132     calc_jacobians = 1;//cvtest::randInt(rng) % 3 != 0;
 133     if( !calc_jacobians )
 134         sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0);
 135
 136     for( i = 0; i < 5; i++ )
 137     {
 138         types[REF_OUTPUT][i] = types[OUTPUT][i];
 139         sizes[REF_OUTPUT][i] = sizes[OUTPUT][i];
 140     }
 141 }
 142
 143
 144 double CV_ProjectPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j )
 145 {
 146     return j == 4 ? 1e-2 : 1e-2;
 147 }
 148
 149
 150 void CV_ProjectPointsTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr )
 151 {
 152     double r[3], theta0, theta1, f;
 153     CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r );
 154     RNG& rng = ts->get_rng();
 155
 156     r[0] = cvtest::randReal(rng)*CV_PI*2;
 157     r[1] = cvtest::randReal(rng)*CV_PI*2;
 158     r[2] = cvtest::randReal(rng)*CV_PI*2;
 159
 160     theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
 161     theta1 = fmod(theta0, CV_PI*2);
 162
 163     if( theta1 > CV_PI )
 164         theta1 = -(CV_PI*2 - theta1);
 165
 166     f = theta1/(theta0 ? theta0 : 1);
 167     r[0] *= f;
 168     r[1] *= f;
 169     r[2] *= f;
 170
 171     cvTsConvert( &_r, arr );
 172 }
 173
 174
 175 int CV_ProjectPointsTest::prepare_test_case( int test_case_idx )
 176 {
 177     int code = cvtest::ArrayTest::prepare_test_case( test_case_idx );
 178     return code;
 179 }
 180
 181
 182 void CV_ProjectPointsTest::run_func()
 183 {
 184     CvMat *v2m_jac = 0, *m2v_jac = 0;
 185     if( calc_jacobians )
 186     {
 187         v2m_jac = &test_mat[OUTPUT][1];
 188         m2v_jac = &test_mat[OUTPUT][3];
 189     }
 190
 191     cvProjectPoints2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac );
 192     cvProjectPoints2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac );
 193 }
 194
 195
 196 void CV_ProjectPointsTest::prepare_to_validation( int /*test_case_idx*/ )
 197 {
 198     const CvMat* vec = &test_mat[INPUT][0];
 199     CvMat* m = &test_mat[REF_OUTPUT][0];
 200     CvMat* vec2 = &test_mat[REF_OUTPUT][2];
 201     CvMat* v2m_jac = 0, *m2v_jac = 0;
 202     double theta0, theta1;
 203
 204     if( calc_jacobians )
 205     {
 206         v2m_jac = &test_mat[REF_OUTPUT][1];
 207         m2v_jac = &test_mat[REF_OUTPUT][3];
 208     }
 209
 210
 211     cvTsProjectPoints( vec, m, v2m_jac );
 212     cvTsProjectPoints( m, vec2, m2v_jac );
 213     cvTsCopy( vec, vec2 );
 214
 215     theta0 = cvtest::norm( cvarrtomat(vec2), 0, CV_L2 );
 216     theta1 = fmod( theta0, CV_PI*2 );
 217
 218     if( theta1 > CV_PI )
 219         theta1 = -(CV_PI*2 - theta1);
 220     cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) );
 221
 222     if( calc_jacobians )
 223     {
 224         //cvInvert( v2m_jac, m2v_jac, CV_SVD );
 225         if( cvtest::norm(cvarrtomat(&test_mat[OUTPUT][3]), 0, CV_C) < 1000 )
 226         {
 227             cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3],
 228                       1, 0, 0, &test_mat[OUTPUT][4],
 229                       v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T );
 230         }
 231         else
 232         {
 233             cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) );
 234             cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] );
 235         }
 236         cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) );
 237     }
 238 }
 239
 240
 241 CV_ProjectPointsTest ProjectPoints_test;
 242
 243 #endif
 244
 245 // --------------------------------- CV_CameraCalibrationTest --------------------------------------------
 246
 247 typedef Matx33d RotMat;
 248
 249 class CV_CameraCalibrationTest : public cvtest::BaseTest
 250 {
 251 public:
 252     CV_CameraCalibrationTest();
 253     ~CV_CameraCalibrationTest();
 254     void clear();
 255 protected:
 256     int compare(double* val, double* refVal, int len,
 257                 double eps, const char* paramName);
 258     virtual void calibrate(Size imageSize,
 259         const std::vector<std::vector<Point2d> >& imagePoints,
 260         const std::vector<std::vector<Point3d> >& objectPoints,
 261         int iFixedPoint, Mat& distortionCoeffs, Mat& cameraMatrix, std::vector<Vec3d>& translationVectors,
 262         std::vector<RotMat>& rotationMatrices, std::vector<Point3d>& newObjPoints,
 263         std::vector<double>& stdDevs, std::vector<double>& perViewErrors,
 264         int flags ) = 0;
 265     virtual void project( const std::vector<Point3d>& objectPoints,
 266         const RotMat& rotationMatrix, const Vec3d& translationVector,
 267         const Mat& cameraMatrix, const Mat& distortion,
 268         std::vector<Point2d>& imagePoints ) = 0;
 269
 270     void run(int);
 271 };
 272
 273 CV_CameraCalibrationTest::CV_CameraCalibrationTest()
 274 {
 275 }
 276
 277 CV_CameraCalibrationTest::~CV_CameraCalibrationTest()
 278 {
 279     clear();
 280 }
 281
 282 void CV_CameraCalibrationTest::clear()
 283 {
 284     cvtest::BaseTest::clear();
 285 }
 286
 287 int CV_CameraCalibrationTest::compare(double* val, double* ref_val, int len,
 288                                       double eps, const char* param_name )
 289 {
 290     return cvtest::cmpEps2_64f( ts, val, ref_val, len, eps, param_name );
 291 }
 292
 293 void CV_CameraCalibrationTest::run( int start_from )
 294 {
 295     int code = cvtest::TS::OK;
 296     cv::String            filepath;
 297     cv::String            filename;
 298
 299     std::vector<std::vector<Point2d> >  imagePoints;
 300     std::vector<std::vector<Point3d> >  objectPoints;
 301     std::vector<std::vector<Point2d> >  reprojectPoints;
 302
 303     std::vector<Vec3d>        transVects;
 304     std::vector<RotMat>       rotMatrs;
 305     std::vector<Point3d>      newObjPoints;
 306     std::vector<double>       stdDevs;
 307     std::vector<double>       perViewErrors;
 308
 309     std::vector<Vec3d>        goodTransVects;
 310     std::vector<RotMat>       goodRotMatrs;
 311     std::vector<Point3d>      goodObjPoints;
 312     std::vector<double>       goodPerViewErrors;
 313     std::vector<double>       goodStdDevs;
 314
 315     Mat             cameraMatrix;
 316     Mat             distortion = Mat::zeros(1, 5, CV_64F);
 317     Mat             goodDistortion = Mat::zeros(1, 5, CV_64F);
 318
 319     FILE*           file = 0;
 320     FILE*           datafile = 0;
 321     int             i,j;
 322     int             currImage;
 323     int             currPoint;
 324     char            i_dat_file[100];
 325
 326     int progress = 0;
 327     int values_read = -1;
 328
 329     filepath = cv::format("%scv/cameracalibration/", ts->get_data_path().c_str() );
 330     filename = cv::format("%sdatafiles.txt", filepath.c_str() );
 331     datafile = fopen( filename.c_str(), "r" );
 332     if( datafile == 0 )
 333     {
 334         ts->printf( cvtest::TS::LOG, "Could not open file with list of test files: %s\n", filename.c_str() );
 335         code = cvtest::TS::FAIL_MISSING_TEST_DATA;
 336         ts->set_failed_test_info( code );
 337         return;
 338     }
 339
 340     int numTests = 0;
 341     values_read = fscanf(datafile,"%d",&numTests);
 342     CV_Assert(values_read == 1);
 343
 344     for( int currTest = start_from; currTest < numTests; currTest++ )
 345     {
 346         values_read = fscanf(datafile,"%s",i_dat_file);
 347         CV_Assert(values_read == 1);
 348         filename = cv::format("%s%s", filepath.c_str(), i_dat_file);
 349         file = fopen(filename.c_str(),"r");
 350
 351         ts->update_context( this, currTest, true );
 352
 353         if( file == 0 )
 354         {
 355             ts->printf( cvtest::TS::LOG,
 356                 "Can't open current test file: %s\n",filename.c_str());
 357             if( numTests == 1 )
 358             {
 359                 code = cvtest::TS::FAIL_MISSING_TEST_DATA;
 360                 break;
 361             }
 362             continue; // if there is more than one test, just skip the test
 363         }
 364
 365         Size imageSize;
 366         values_read = fscanf(file,"%d %d\n",&(imageSize.width),&(imageSize.height));
 367         CV_Assert(values_read == 2);
 368         if( imageSize.width <= 0 || imageSize.height <= 0 )
 369         {
 370             ts->printf( cvtest::TS::LOG, "Image size in test file is incorrect\n" );
 371             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 372             break;
 373         }
 374
 375         /* Read etalon size */
 376         Size etalonSize;
 377         values_read = fscanf(file,"%d %d\n",&(etalonSize.width),&(etalonSize.height));
 378         CV_Assert(values_read == 2);
 379         if( etalonSize.width <= 0 || etalonSize.height <= 0 )
 380         {
 381             ts->printf( cvtest::TS::LOG, "Pattern size in test file is incorrect\n" );
 382             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 383             break;
 384         }
 385
 386         int numPoints = etalonSize.width * etalonSize.height;
 387
 388         /* Read number of images */
 389         int numImages = 0;
 390         values_read = fscanf(file,"%d\n",&numImages);
 391         CV_Assert(values_read == 1);
 392         if( numImages <=0 )
 393         {
 394             ts->printf( cvtest::TS::LOG, "Number of images in test file is incorrect\n");
 395             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 396             break;
 397         }
 398
 399         /* Read calibration flags */
 400         int calibFlags = 0;
 401         values_read = fscanf(file,"%d\n",&calibFlags);
 402         CV_Assert(values_read == 1);
 403
 404         /* Read index of the fixed point */
 405         int iFixedPoint;
 406         values_read = fscanf(file,"%d\n",&iFixedPoint);
 407         CV_Assert(values_read == 1);
 408
 409         /* Need to allocate memory */
 410         imagePoints.resize(numImages);
 411         objectPoints.resize(numImages);
 412         reprojectPoints.resize(numImages);
 413         for( currImage = 0; currImage < numImages; currImage++ )
 414         {
 415             imagePoints[currImage].resize(numPoints);
 416             objectPoints[currImage].resize(numPoints);
 417             reprojectPoints[currImage].resize(numPoints);
 418         }
 419
 420         transVects.resize(numImages);
 421         rotMatrs.resize(numImages);
 422         newObjPoints.resize(numPoints);
 423         stdDevs.resize(CALIB_NINTRINSIC + 6*numImages + 3*numPoints);
 424         perViewErrors.resize(numImages);
 425
 426         goodTransVects.resize(numImages);
 427         goodRotMatrs.resize(numImages);
 428         goodObjPoints.resize(numPoints);
 429         goodPerViewErrors.resize(numImages);
 430
 431         int nstddev = CALIB_NINTRINSIC + 6*numImages + 3*numPoints;
 432         goodStdDevs.resize(nstddev);
 433
 434         for( currImage = 0; currImage < numImages; currImage++ )
 435         {
 436             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 437             {
 438                 double x,y,z;
 439                 values_read = fscanf(file,"%lf %lf %lf\n",&x,&y,&z);
 440                 CV_Assert(values_read == 3);
 441
 442                 objectPoints[currImage][currPoint].x = x;
 443                 objectPoints[currImage][currPoint].y = y;
 444                 objectPoints[currImage][currPoint].z = z;
 445             }
 446         }
 447
 448         /* Read image points */
 449         for( currImage = 0; currImage < numImages; currImage++ )
 450         {
 451             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 452             {
 453                 double x,y;
 454                 values_read = fscanf(file,"%lf %lf\n",&x,&y);
 455                 CV_Assert(values_read == 2);
 456
 457                 imagePoints[currImage][currPoint].x = x;
 458                 imagePoints[currImage][currPoint].y = y;
 459             }
 460         }
 461
 462         /* Read good data computed before */
 463
 464         /* Focal lengths */
 465         double goodFcx,goodFcy;
 466         values_read = fscanf(file,"%lf %lf",&goodFcx,&goodFcy);
 467         CV_Assert(values_read == 2);
 468
 469         /* Principal points */
 470         double goodCx,goodCy;
 471         values_read = fscanf(file,"%lf %lf",&goodCx,&goodCy);
 472         CV_Assert(values_read == 2);
 473
 474         /* Read distortion */
 475
 476         for( i = 0; i < 4; i++ )
 477         {
 478             values_read = fscanf(file,"%lf",&goodDistortion.at<double>(i)); CV_Assert(values_read == 1);
 479         }
 480
 481         /* Read good Rot matrices */
 482         for( currImage = 0; currImage < numImages; currImage++ )
 483         {
 484             for( i = 0; i < 3; i++ )
 485                 for( j = 0; j < 3; j++ )
 486                 {
 487                     values_read = fscanf(file, "%lf", &goodRotMatrs[currImage].val[i*3+j]);
 488                     CV_Assert(values_read == 1);
 489                 }
 490         }
 491
 492         /* Read good Trans vectors */
 493         for( currImage = 0; currImage < numImages; currImage++ )
 494         {
 495             for( i = 0; i < 3; i++ )
 496             {
 497                 values_read = fscanf(file, "%lf", &goodTransVects[currImage].val[i]);
 498                 CV_Assert(values_read == 1);
 499             }
 500         }
 501
 502         bool releaseObject = iFixedPoint > 0 && iFixedPoint < numPoints - 1;
 503         /* Read good refined 3D object points */
 504         if( releaseObject )
 505         {
 506             for( i = 0; i < numPoints; i++ )
 507             {
 508                 for( j = 0; j < 3; j++ )
 509                 {
 510                     values_read = fscanf(file, "%lf", &goodObjPoints[i].x + j);
 511                     CV_Assert(values_read == 1);
 512                 }
 513             }
 514         }
 515
 516         /* Read good stdDeviations */
 517         for (i = 0; i < CALIB_NINTRINSIC + numImages*6; i++)
 518         {
 519             values_read = fscanf(file, "%lf", &goodStdDevs[i]);
 520             CV_Assert(values_read == 1);
 521         }
 522         for( ; i < nstddev; i++ )
 523         {
 524             if( releaseObject )
 525             {
 526                 values_read = fscanf(file, "%lf", &goodStdDevs[i]);
 527                 CV_Assert(values_read == 1);
 528             }
 529             else
 530                 goodStdDevs[i] = 0.0;
 531         }
 532
 533         cameraMatrix = Mat::zeros(3, 3, CV_64F);
 534         cameraMatrix.at<double>(0, 0) = cameraMatrix.at<double>(1, 1) = 807.;
 535         cameraMatrix.at<double>(0, 2) = (imageSize.width - 1)*0.5;
 536         cameraMatrix.at<double>(1, 2) = (imageSize.height - 1)*0.5;
 537         cameraMatrix.at<double>(2, 2) = 1.;
 538
 539         /* Now we can calibrate camera */
 540         calibrate(  imageSize,
 541                     imagePoints,
 542                     objectPoints,
 543                     iFixedPoint,
 544                     distortion,
 545                     cameraMatrix,
 546                     transVects,
 547                     rotMatrs,
 548                     newObjPoints,
 549                     stdDevs,
 550                     perViewErrors,
 551                     calibFlags );
 552
 553         /* ---- Reproject points to the image ---- */
 554         for( currImage = 0; currImage < numImages; currImage++ )
 555         {
 556             if( releaseObject )
 557             {
 558                 objectPoints[currImage] = newObjPoints;
 559             }
 560             project(  objectPoints[currImage],
 561                       rotMatrs[currImage],
 562                       transVects[currImage],
 563                       cameraMatrix,
 564                       distortion,
 565                       reprojectPoints[currImage]);
 566         }
 567
 568         /* ----- Compute reprojection error ----- */
 569         double dx,dy;
 570         double rx,ry;
 571         double meanDx,meanDy;
 572         double maxDx = 0.0;
 573         double maxDy = 0.0;
 574
 575         meanDx = 0;
 576         meanDy = 0;
 577         for( currImage = 0; currImage < numImages; currImage++ )
 578         {
 579             double imageMeanDx = 0;
 580             double imageMeanDy = 0;
 581             for( currPoint = 0; currPoint < etalonSize.width * etalonSize.height; currPoint++ )
 582             {
 583                 rx = reprojectPoints[currImage][currPoint].x;
 584                 ry = reprojectPoints[currImage][currPoint].y;
 585                 dx = rx - imagePoints[currImage][currPoint].x;
 586                 dy = ry - imagePoints[currImage][currPoint].y;
 587
 588                 meanDx += dx;
 589                 meanDy += dy;
 590
 591                 imageMeanDx += dx*dx;
 592                 imageMeanDy += dy*dy;
 593
 594                 dx = fabs(dx);
 595                 dy = fabs(dy);
 596
 597                 if( dx > maxDx )
 598                     maxDx = dx;
 599
 600                 if( dy > maxDy )
 601                     maxDy = dy;
 602             }
 603             goodPerViewErrors[currImage] = sqrt( (imageMeanDx + imageMeanDy) /
 604                                            (etalonSize.width * etalonSize.height));
 605
 606             //only for c-version of test (it does not provides evaluation of perViewErrors
 607             //and returns zeros)
 608             if(perViewErrors[currImage] == 0.0)
 609                 perViewErrors[currImage] = goodPerViewErrors[currImage];
 610         }
 611
 612         meanDx /= numImages * etalonSize.width * etalonSize.height;
 613         meanDy /= numImages * etalonSize.width * etalonSize.height;
 614
 615         /* ========= Compare parameters ========= */
 616         CV_Assert(cameraMatrix.type() == CV_64F && cameraMatrix.size() == Size(3, 3));
 617         CV_Assert(distortion.type() == CV_64F);
 618
 619         Size dsz = distortion.size();
 620         CV_Assert(dsz == Size(4, 1) || dsz == Size(1, 4) || dsz == Size(5, 1) || dsz == Size(1, 5));
 621
 622         /*std::cout << "cameraMatrix: " << cameraMatrix << "\n";
 623         std::cout << "curr distCoeffs: " << distortion << "\n";
 624         std::cout << "good distCoeffs: " << goodDistortion << "\n";*/
 625
 626         /* ----- Compare focal lengths ----- */
 627         code = compare(&cameraMatrix.at<double>(0, 0), &goodFcx, 1, 0.1, "fx");
 628         if( code < 0 )
 629             break;
 630
 631         code = compare(&cameraMatrix.at<double>(1, 1),&goodFcy, 1, 0.1, "fy");
 632         if( code < 0 )
 633             break;
 634
 635         /* ----- Compare principal points ----- */
 636         code = compare(&cameraMatrix.at<double>(0,2), &goodCx, 1, 0.1, "cx");
 637         if( code < 0 )
 638             break;
 639
 640         code = compare(&cameraMatrix.at<double>(1,2), &goodCy, 1, 0.1, "cy");
 641         if( code < 0 )
 642             break;
 643
 644         /* ----- Compare distortion ----- */
 645         code = compare(&distortion.at<double>(0), &goodDistortion.at<double>(0), 4, 0.1, "[k1,k2,p1,p2]");
 646         if( code < 0 )
 647             break;
 648
 649         /* ----- Compare rot matrixs ----- */
 650         CV_Assert(rotMatrs.size() == (size_t)numImages);
 651         CV_Assert(transVects.size() == (size_t)numImages);
 652
 653         //code = compare(rotMatrs[0].val, goodRotMatrs[0].val, 9*numImages, 0.05, "rotation matrices");
 654         for( i = 0; i < numImages; i++ )
 655         {
 656             if( cv::norm(rotMatrs[i], goodRotMatrs[i], NORM_INF) > 0.05 )
 657             {
 658                 printf("rot mats for frame #%d are very different\n", i);
 659                 std::cout << "curr:\n" << rotMatrs[i] << std::endl;
 660                 std::cout << "good:\n" << goodRotMatrs[i] << std::endl;
 661
 662                 code = TS::FAIL_BAD_ACCURACY;
 663                 break;
 664             }
 665         }
 666         if( code < 0 )
 667             break;
 668
 669         /* ----- Compare rot matrixs ----- */
 670         code = compare(transVects[0].val, goodTransVects[0].val, 3*numImages, 0.1, "translation vectors");
 671         if( code < 0 )
 672             break;
 673
 674         /* ----- Compare refined 3D object points ----- */
 675         if( releaseObject )
 676         {
 677             code = compare(&newObjPoints[0].x, &goodObjPoints[0].x, 3*numPoints, 0.1, "refined 3D object points");
 678             if( code < 0 )
 679                 break;
 680         }
 681
 682         /* ----- Compare per view re-projection errors ----- */
 683         CV_Assert(perViewErrors.size() == (size_t)numImages);
 684         code = compare(&perViewErrors[0], &goodPerViewErrors[0], numImages, 1.1, "per view errors vector");
 685         if( code < 0 )
 686             break;
 687
 688         /* ----- Compare standard deviations of parameters ----- */
 689         if( stdDevs.size() < (size_t)nstddev )
 690             stdDevs.resize(nstddev);
 691         for ( i = 0; i < nstddev; i++)
 692         {
 693             if(stdDevs[i] == 0.0)
 694                 stdDevs[i] = goodStdDevs[i];
 695         }
 696         code = compare(&stdDevs[0], &goodStdDevs[0], nstddev, .5,
 697                        "stdDevs vector");
 698         if( code < 0 )
 699             break;
 700
 701         /*if( maxDx > 1.0 )
 702         {
 703             ts->printf( cvtest::TS::LOG,
 704                       "Error in reprojection maxDx=%f > 1.0\n",maxDx);
 705             code = cvtest::TS::FAIL_BAD_ACCURACY; break;
 706         }
 707
 708         if( maxDy > 1.0 )
 709         {
 710             ts->printf( cvtest::TS::LOG,
 711                       "Error in reprojection maxDy=%f > 1.0\n",maxDy);
 712             code = cvtest::TS::FAIL_BAD_ACCURACY; break;
 713         }*/
 714
 715         progress = update_progress( progress, currTest, numTests, 0 );
 716
 717         fclose(file);
 718         file = 0;
 719     }
 720
 721     if( file )
 722         fclose(file);
 723
 724     if( datafile )
 725         fclose(datafile);
 726
 727     if( code < 0 )
 728         ts->set_failed_test_info( code );
 729 }
 730
 731 // --------------------------------- CV_CameraCalibrationTest_CPP --------------------------------------------
 732
 733 class CV_CameraCalibrationTest_CPP : public CV_CameraCalibrationTest
 734 {
 735 public:
 736     CV_CameraCalibrationTest_CPP(){}
 737 protected:
 738     virtual void calibrate(Size imageSize,
 739                            const std::vector<std::vector<Point2d> >& imagePoints,
 740                            const std::vector<std::vector<Point3d> >& objectPoints,
 741                            int iFixedPoint, Mat& distortionCoeffs, Mat& cameraMatrix, std::vector<Vec3d>& translationVectors,
 742                            std::vector<RotMat>& rotationMatrices, std::vector<Point3d>& newObjPoints,
 743                            std::vector<double>& stdDevs, std::vector<double>& perViewErrors,
 744                            int flags );
 745     virtual void project( const std::vector<Point3d>& objectPoints,
 746                          const RotMat& rotationMatrix, const Vec3d& translationVector,
 747                          const Mat& cameraMatrix, const Mat& distortion,
 748                          std::vector<Point2d>& imagePoints );
 749 };
 750
 751 void CV_CameraCalibrationTest_CPP::calibrate(Size imageSize,
 752     const std::vector<std::vector<Point2d> >& _imagePoints,
 753     const std::vector<std::vector<Point3d> >& _objectPoints,
 754     int iFixedPoint, Mat& _distCoeffs, Mat& _cameraMatrix, std::vector<Vec3d>& translationVectors,
 755     std::vector<RotMat>& rotationMatrices, std::vector<Point3d>& newObjPoints,
 756     std::vector<double>& stdDevs, std::vector<double>& perViewErrors,
 757     int flags )
 758 {
 759     int pointCount = (int)_imagePoints[0].size();
 760     size_t i, imageCount = _imagePoints.size();
 761     vector<vector<Point3f> > objectPoints( imageCount );
 762     vector<vector<Point2f> > imagePoints( imageCount );
 763     Mat cameraMatrix, distCoeffs(1,4,CV_64F,Scalar::all(0));
 764     vector<Mat> rvecs, tvecs;
 765     Mat newObjMat;
 766     Mat stdDevsMatInt, stdDevsMatExt;
 767     Mat stdDevsMatObj;
 768     Mat perViewErrorsMat;
 769
 770     for( i = 0; i < imageCount; i++ )
 771     {
 772         Mat(_imagePoints[i]).convertTo(imagePoints[i], CV_32F);
 773         Mat(_objectPoints[i]).convertTo(objectPoints[i], CV_32F);
 774     }
 775
 776     size_t nstddev0 = CV_CALIB_NINTRINSIC + imageCount*6, nstddev1 = nstddev0 + _imagePoints[0].size()*3;
 777     for( i = nstddev0; i < nstddev1; i++ )
 778     {
 779         stdDevs[i] = 0.0;
 780     }
 781
 782     calibrateCameraRO( objectPoints,
 783                        imagePoints,
 784                        imageSize,
 785                        iFixedPoint,
 786                        cameraMatrix,
 787                        distCoeffs,
 788                        rvecs,
 789                        tvecs,
 790                        newObjMat,
 791                        stdDevsMatInt,
 792                        stdDevsMatExt,
 793                        stdDevsMatObj,
 794                        perViewErrorsMat,
 795                        flags );
 796
 797     bool releaseObject = iFixedPoint > 0 && iFixedPoint < pointCount - 1;
 798     if( releaseObject )
 799     {
 800         newObjMat.convertTo( newObjPoints, CV_64F );
 801     }
 802
 803     Mat stdDevMats[] = {stdDevsMatInt, stdDevsMatExt, stdDevsMatObj}, stdDevsMat;
 804     vconcat(stdDevMats, releaseObject ? 3 : 2, stdDevsMat);
 805     stdDevsMat.convertTo(stdDevs, CV_64F);
 806
 807     perViewErrorsMat.convertTo(perViewErrors, CV_64F);
 808     cameraMatrix.convertTo(_cameraMatrix, CV_64F);
 809     distCoeffs.convertTo(_distCoeffs, CV_64F);
 810
 811     for( i = 0; i < imageCount; i++ )
 812     {
 813         Mat r9;
 814         cvtest::Rodrigues( rvecs[i], r9 );
 815         cv::transpose(r9, r9);
 816         r9.convertTo(rotationMatrices[i], CV_64F);
 817         tvecs[i].convertTo(translationVectors[i], CV_64F);
 818     }
 819 }
 820
 821
 822 void CV_CameraCalibrationTest_CPP::project( const std::vector<Point3d>& objectPoints,
 823                          const RotMat& rotationMatrix, const Vec3d& translationVector,
 824                          const Mat& cameraMatrix, const Mat& distortion,
 825                          std::vector<Point2d>& imagePoints )
 826 {
 827     projectPoints(objectPoints, rotationMatrix, translationVector, cameraMatrix, distortion, imagePoints );
 828     /*Mat objectPoints( pointCount, 3, CV_64FC1, _objectPoints );
 829     Mat rmat( 3, 3, CV_64FC1, rotationMatrix ),
 830         rvec( 1, 3, CV_64FC1 ),
 831         tvec( 1, 3, CV_64FC1, translationVector );
 832     Mat cameraMatrix( 3, 3, CV_64FC1, _cameraMatrix );
 833     Mat distCoeffs( 1, 4, CV_64FC1, distortion );
 834     vector<Point2f> imagePoints;
 835     cvtest::Rodrigues( rmat, rvec );
 836
 837     objectPoints.convertTo( objectPoints, CV_32FC1 );
 838     projectPoints( objectPoints, rvec, tvec,
 839                    cameraMatrix, distCoeffs, imagePoints );
 840     vector<Point2f>::const_iterator it = imagePoints.begin();
 841     for( int i = 0; it != imagePoints.end(); ++it, i++ )
 842     {
 843         _imagePoints[i] = cvPoint2D64f( it->x, it->y );
 844     }*/
 845 }
 846
 847
 848 //----------------------------------------- CV_CalibrationMatrixValuesTest --------------------------------
 849
 850 class CV_CalibrationMatrixValuesTest : public cvtest::BaseTest
 851 {
 852 public:
 853     CV_CalibrationMatrixValuesTest() {}
 854 protected:
 855     void run(int);
 856     virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 857         double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 858         Point2d& principalPoint, double& aspectRatio ) = 0;
 859 };
 860
 861 void CV_CalibrationMatrixValuesTest::run(int)
 862 {
 863     int code = cvtest::TS::OK;
 864     const double fcMinVal = 1e-5;
 865     const double fcMaxVal = 1000;
 866     const double apertureMaxVal = 0.01;
 867
 868     RNG rng = ts->get_rng();
 869
 870     double fx, fy, cx, cy, nx, ny;
 871     Mat cameraMatrix( 3, 3, CV_64FC1 );
 872     cameraMatrix.setTo( Scalar(0) );
 873     fx = cameraMatrix.at<double>(0,0) = rng.uniform( fcMinVal, fcMaxVal );
 874     fy = cameraMatrix.at<double>(1,1) = rng.uniform( fcMinVal, fcMaxVal );
 875     cx = cameraMatrix.at<double>(0,2) = rng.uniform( fcMinVal, fcMaxVal );
 876     cy = cameraMatrix.at<double>(1,2) = rng.uniform( fcMinVal, fcMaxVal );
 877     cameraMatrix.at<double>(2,2) = 1;
 878
 879     Size imageSize( 600, 400 );
 880
 881     double apertureWidth = (double)rng * apertureMaxVal,
 882            apertureHeight = (double)rng * apertureMaxVal;
 883
 884     double fovx, fovy, focalLength, aspectRatio,
 885            goodFovx, goodFovy, goodFocalLength, goodAspectRatio;
 886     Point2d principalPoint, goodPrincipalPoint;
 887
 888
 889     calibMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
 890         fovx, fovy, focalLength, principalPoint, aspectRatio );
 891
 892     // calculate calibration matrix values
 893     goodAspectRatio = fy / fx;
 894
 895     if( apertureWidth != 0.0 && apertureHeight != 0.0 )
 896     {
 897         nx = imageSize.width / apertureWidth;
 898         ny = imageSize.height / apertureHeight;
 899     }
 900     else
 901     {
 902         nx = 1.0;
 903         ny = goodAspectRatio;
 904     }
 905
 906     goodFovx = (atan2(cx, fx) + atan2(imageSize.width  - cx, fx)) * 180.0 / CV_PI;
 907     goodFovy = (atan2(cy, fy) + atan2(imageSize.height - cy, fy)) * 180.0 / CV_PI;
 908
 909     goodFocalLength = fx / nx;
 910
 911     goodPrincipalPoint.x = cx / nx;
 912     goodPrincipalPoint.y = cy / ny;
 913
 914     // check results
 915     if( fabs(fovx - goodFovx) > FLT_EPSILON )
 916     {
 917         ts->printf( cvtest::TS::LOG, "bad fovx (real=%f, good = %f\n", fovx, goodFovx );
 918         code = cvtest::TS::FAIL_BAD_ACCURACY;
 919         goto _exit_;
 920     }
 921     if( fabs(fovy - goodFovy) > FLT_EPSILON )
 922     {
 923         ts->printf( cvtest::TS::LOG, "bad fovy (real=%f, good = %f\n", fovy, goodFovy );
 924         code = cvtest::TS::FAIL_BAD_ACCURACY;
 925         goto _exit_;
 926     }
 927     if( fabs(focalLength - goodFocalLength) > FLT_EPSILON )
 928     {
 929         ts->printf( cvtest::TS::LOG, "bad focalLength (real=%f, good = %f\n", focalLength, goodFocalLength );
 930         code = cvtest::TS::FAIL_BAD_ACCURACY;
 931         goto _exit_;
 932     }
 933     if( fabs(aspectRatio - goodAspectRatio) > FLT_EPSILON )
 934     {
 935         ts->printf( cvtest::TS::LOG, "bad aspectRatio (real=%f, good = %f\n", aspectRatio, goodAspectRatio );
 936         code = cvtest::TS::FAIL_BAD_ACCURACY;
 937         goto _exit_;
 938     }
 939     if( cv::norm(principalPoint - goodPrincipalPoint) > FLT_EPSILON ) // Point2d
 940     {
 941         ts->printf( cvtest::TS::LOG, "bad principalPoint\n" );
 942         code = cvtest::TS::FAIL_BAD_ACCURACY;
 943         goto _exit_;
 944     }
 945
 946 _exit_:
 947     RNG& _rng = ts->get_rng();
 948     _rng = rng;
 949     ts->set_failed_test_info( code );
 950 }
 951
 952 //----------------------------------------- CV_CalibrationMatrixValuesTest_CPP --------------------------------
 953
 954 class CV_CalibrationMatrixValuesTest_CPP : public CV_CalibrationMatrixValuesTest
 955 {
 956 public:
 957     CV_CalibrationMatrixValuesTest_CPP() {}
 958 protected:
 959     virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 960         double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 961         Point2d& principalPoint, double& aspectRatio );
 962 };
 963
 964 void CV_CalibrationMatrixValuesTest_CPP::calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 965                                                          double apertureWidth, double apertureHeight,
 966                                                          double& fovx, double& fovy, double& focalLength,
 967                                                          Point2d& principalPoint, double& aspectRatio )
 968 {
 969     calibrationMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
 970         fovx, fovy, focalLength, principalPoint, aspectRatio );
 971 }
 972
 973
 974 //----------------------------------------- CV_ProjectPointsTest --------------------------------
 975 void calcdfdx( const vector<vector<Point2f> >& leftF, const vector<vector<Point2f> >& rightF, double eps, Mat& dfdx )
 976 {
 977     const int fdim = 2;
 978     CV_Assert( !leftF.empty() && !rightF.empty() && !leftF[0].empty() && !rightF[0].empty() );
 979     CV_Assert( leftF[0].size() ==  rightF[0].size() );
 980     CV_Assert( fabs(eps) > std::numeric_limits<double>::epsilon() );
 981     int fcount = (int)leftF[0].size(), xdim = (int)leftF.size();
 982
 983     dfdx.create( fcount*fdim, xdim, CV_64FC1 );
 984
 985     vector<vector<Point2f> >::const_iterator arrLeftIt = leftF.begin();
 986     vector<vector<Point2f> >::const_iterator arrRightIt = rightF.begin();
 987     for( int xi = 0; xi < xdim; xi++, ++arrLeftIt, ++arrRightIt )
 988     {
 989         CV_Assert( (int)arrLeftIt->size() ==  fcount );
 990         CV_Assert( (int)arrRightIt->size() ==  fcount );
 991         vector<Point2f>::const_iterator lIt = arrLeftIt->begin();
 992         vector<Point2f>::const_iterator rIt = arrRightIt->begin();
 993         for( int fi = 0; fi < dfdx.rows; fi+=fdim, ++lIt, ++rIt )
 994         {
 995             dfdx.at<double>(fi, xi )   = 0.5 * ((double)(rIt->x - lIt->x)) / eps;
 996             dfdx.at<double>(fi+1, xi ) = 0.5 * ((double)(rIt->y - lIt->y)) / eps;
 997         }
 998     }
 999 }
1000
1001 class CV_ProjectPointsTest : public cvtest::BaseTest
1002 {
1003 public:
1004     CV_ProjectPointsTest() {}
1005 protected:
1006     void run(int);
1007     virtual void project( const Mat& objectPoints,
1008         const Mat& rvec, const Mat& tvec,
1009         const Mat& cameraMatrix,
1010         const Mat& distCoeffs,
1011         vector<Point2f>& imagePoints,
1012         Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1013         Mat& dpdc, Mat& dpddist,
1014         double aspectRatio=0 ) = 0;
1015 };
1016
1017 void CV_ProjectPointsTest::run(int)
1018 {
1019     //typedef float matType;
1020
1021     int code = cvtest::TS::OK;
1022     const int pointCount = 100;
1023
1024     const float zMinVal = 10.0f, zMaxVal = 100.0f,
1025                 rMinVal = -0.3f, rMaxVal = 0.3f,
1026                 tMinVal = -2.0f, tMaxVal = 2.0f;
1027
1028     const float imgPointErr = 1e-3f,
1029                 dEps = 1e-3f;
1030
1031     double err;
1032
1033     Size imgSize( 600, 800 );
1034     Mat_<float> objPoints( pointCount, 3), rvec( 1, 3), rmat, tvec( 1, 3 ), cameraMatrix( 3, 3 ), distCoeffs( 1, 4 ),
1035       leftRvec, rightRvec, leftTvec, rightTvec, leftCameraMatrix, rightCameraMatrix, leftDistCoeffs, rightDistCoeffs;
1036
1037     RNG rng = ts->get_rng();
1038
1039     // generate data
1040     cameraMatrix << 300.f,  0.f,    imgSize.width/2.f,
1041                     0.f,    300.f,  imgSize.height/2.f,
1042                     0.f,    0.f,    1.f;
1043     distCoeffs << 0.1, 0.01, 0.001, 0.001;
1044
1045     rvec(0,0) = rng.uniform( rMinVal, rMaxVal );
1046     rvec(0,1) = rng.uniform( rMinVal, rMaxVal );
1047     rvec(0,2) = rng.uniform( rMinVal, rMaxVal );
1048     rmat = cv::Mat_<float>::zeros(3, 3);
1049     cvtest::Rodrigues( rvec, rmat );
1050
1051     tvec(0,0) = rng.uniform( tMinVal, tMaxVal );
1052     tvec(0,1) = rng.uniform( tMinVal, tMaxVal );
1053     tvec(0,2) = rng.uniform( tMinVal, tMaxVal );
1054
1055     for( int y = 0; y < objPoints.rows; y++ )
1056     {
1057         Mat point(1, 3, CV_32FC1, objPoints.ptr(y) );
1058         float z = rng.uniform( zMinVal, zMaxVal );
1059         point.at<float>(0,2) = z;
1060         point.at<float>(0,0) = (rng.uniform(2.f,(float)(imgSize.width-2)) - cameraMatrix(0,2)) / cameraMatrix(0,0) * z;
1061         point.at<float>(0,1) = (rng.uniform(2.f,(float)(imgSize.height-2)) - cameraMatrix(1,2)) / cameraMatrix(1,1) * z;
1062         point = (point - tvec) * rmat;
1063     }
1064
1065     vector<Point2f> imgPoints;
1066     vector<vector<Point2f> > leftImgPoints;
1067     vector<vector<Point2f> > rightImgPoints;
1068     Mat dpdrot, dpdt, dpdf, dpdc, dpddist,
1069         valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist;
1070
1071     project( objPoints, rvec, tvec, cameraMatrix, distCoeffs,
1072         imgPoints, dpdrot, dpdt, dpdf, dpdc, dpddist, 0 );
1073
1074     // calculate and check image points
1075     assert( (int)imgPoints.size() == pointCount );
1076     vector<Point2f>::const_iterator it = imgPoints.begin();
1077     for( int i = 0; i < pointCount; i++, ++it )
1078     {
1079         Point3d p( objPoints(i,0), objPoints(i,1), objPoints(i,2) );
1080         double z = p.x*rmat(2,0) + p.y*rmat(2,1) + p.z*rmat(2,2) + tvec(0,2),
1081                x = (p.x*rmat(0,0) + p.y*rmat(0,1) + p.z*rmat(0,2) + tvec(0,0)) / z,
1082                y = (p.x*rmat(1,0) + p.y*rmat(1,1) + p.z*rmat(1,2) + tvec(0,1)) / z,
1083                r2 = x*x + y*y,
1084                r4 = r2*r2;
1085         Point2f validImgPoint;
1086         double a1 = 2*x*y,
1087                a2 = r2 + 2*x*x,
1088                a3 = r2 + 2*y*y,
1089                cdist = 1+distCoeffs(0,0)*r2+distCoeffs(0,1)*r4;
1090         validImgPoint.x = static_cast<float>((double)cameraMatrix(0,0)*(x*cdist + (double)distCoeffs(0,2)*a1 + (double)distCoeffs(0,3)*a2)
1091             + (double)cameraMatrix(0,2));
1092         validImgPoint.y = static_cast<float>((double)cameraMatrix(1,1)*(y*cdist + (double)distCoeffs(0,2)*a3 + distCoeffs(0,3)*a1)
1093             + (double)cameraMatrix(1,2));
1094
1095         if( fabs(it->x - validImgPoint.x) > imgPointErr ||
1096             fabs(it->y - validImgPoint.y) > imgPointErr )
1097         {
1098             ts->printf( cvtest::TS::LOG, "bad image point\n" );
1099             code = cvtest::TS::FAIL_BAD_ACCURACY;
1100             goto _exit_;
1101         }
1102     }
1103
1104     // check derivatives
1105     // 1. rotation
1106     leftImgPoints.resize(3);
1107     rightImgPoints.resize(3);
1108     for( int i = 0; i < 3; i++ )
1109     {
1110         rvec.copyTo( leftRvec ); leftRvec(0,i) -= dEps;
1111         project( objPoints, leftRvec, tvec, cameraMatrix, distCoeffs,
1112             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1113         rvec.copyTo( rightRvec ); rightRvec(0,i) += dEps;
1114         project( objPoints, rightRvec, tvec, cameraMatrix, distCoeffs,
1115             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1116     }
1117     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdrot );
1118     err = cvtest::norm( dpdrot, valDpdrot, NORM_INF );
1119     if( err > 3 )
1120     {
1121         ts->printf( cvtest::TS::LOG, "bad dpdrot: too big difference = %g\n", err );
1122         code = cvtest::TS::FAIL_BAD_ACCURACY;
1123     }
1124
1125     // 2. translation
1126     for( int i = 0; i < 3; i++ )
1127     {
1128         tvec.copyTo( leftTvec ); leftTvec(0,i) -= dEps;
1129         project( objPoints, rvec, leftTvec, cameraMatrix, distCoeffs,
1130             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1131         tvec.copyTo( rightTvec ); rightTvec(0,i) += dEps;
1132         project( objPoints, rvec, rightTvec, cameraMatrix, distCoeffs,
1133             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1134     }
1135     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdt );
1136     if( cvtest::norm( dpdt, valDpdt, NORM_INF ) > 0.2 )
1137     {
1138         ts->printf( cvtest::TS::LOG, "bad dpdtvec\n" );
1139         code = cvtest::TS::FAIL_BAD_ACCURACY;
1140     }
1141
1142     // 3. camera matrix
1143     // 3.1. focus
1144     leftImgPoints.resize(2);
1145     rightImgPoints.resize(2);
1146     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,0) -= dEps;
1147     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1148         leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1149     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,1) -= dEps;
1150     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1151         leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1152     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,0) += dEps;
1153     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1154         rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1155     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,1) += dEps;
1156     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1157         rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1158     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdf );
1159     if ( cvtest::norm( dpdf, valDpdf, NORM_L2 ) > 0.2 )
1160     {
1161         ts->printf( cvtest::TS::LOG, "bad dpdf\n" );
1162         code = cvtest::TS::FAIL_BAD_ACCURACY;
1163     }
1164     // 3.2. principal point
1165     leftImgPoints.resize(2);
1166     rightImgPoints.resize(2);
1167     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,2) -= dEps;
1168     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1169         leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1170     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,2) -= dEps;
1171     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1172         leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1173     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,2) += dEps;
1174     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1175         rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1176     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,2) += dEps;
1177     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1178         rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1179     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdc );
1180     if ( cvtest::norm( dpdc, valDpdc, NORM_L2 ) > 0.2 )
1181     {
1182         ts->printf( cvtest::TS::LOG, "bad dpdc\n" );
1183         code = cvtest::TS::FAIL_BAD_ACCURACY;
1184     }
1185
1186     // 4. distortion
1187     leftImgPoints.resize(distCoeffs.cols);
1188     rightImgPoints.resize(distCoeffs.cols);
1189     for( int i = 0; i < distCoeffs.cols; i++ )
1190     {
1191         distCoeffs.copyTo( leftDistCoeffs ); leftDistCoeffs(0,i) -= dEps;
1192         project( objPoints, rvec, tvec, cameraMatrix, leftDistCoeffs,
1193             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1194         distCoeffs.copyTo( rightDistCoeffs ); rightDistCoeffs(0,i) += dEps;
1195         project( objPoints, rvec, tvec, cameraMatrix, rightDistCoeffs,
1196             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1197     }
1198     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpddist );
1199     if( cvtest::norm( dpddist, valDpddist, NORM_L2 ) > 0.3 )
1200     {
1201         ts->printf( cvtest::TS::LOG, "bad dpddist\n" );
1202         code = cvtest::TS::FAIL_BAD_ACCURACY;
1203     }
1204
1205 _exit_:
1206     RNG& _rng = ts->get_rng();
1207     _rng = rng;
1208     ts->set_failed_test_info( code );
1209 }
1210
1211 //----------------------------------------- CV_ProjectPointsTest_CPP --------------------------------
1212 class CV_ProjectPointsTest_CPP : public CV_ProjectPointsTest
1213 {
1214 public:
1215     CV_ProjectPointsTest_CPP() {}
1216 protected:
1217     virtual void project( const Mat& objectPoints,
1218         const Mat& rvec, const Mat& tvec,
1219         const Mat& cameraMatrix,
1220         const Mat& distCoeffs,
1221         vector<Point2f>& imagePoints,
1222         Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1223         Mat& dpdc, Mat& dpddist,
1224         double aspectRatio=0 );
1225 };
1226
1227 void CV_ProjectPointsTest_CPP::project( const Mat& objectPoints, const Mat& rvec, const Mat& tvec,
1228                                        const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& imagePoints,
1229                                        Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio)
1230 {
1231     Mat J;
1232     projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, J, aspectRatio);
1233     J.colRange(0, 3).copyTo(dpdrot);
1234     J.colRange(3, 6).copyTo(dpdt);
1235     J.colRange(6, 8).copyTo(dpdf);
1236     J.colRange(8, 10).copyTo(dpdc);
1237     J.colRange(10, J.cols).copyTo(dpddist);
1238 }
1239
1240 ///////////////////////////////// Stereo Calibration /////////////////////////////////////
1241
1242 class CV_StereoCalibrationTest : public cvtest::BaseTest
1243 {
1244 public:
1245     CV_StereoCalibrationTest();
1246     ~CV_StereoCalibrationTest();
1247     void clear();
1248 protected:
1249     bool checkPandROI( int test_case_idx,
1250         const Mat& M, const Mat& D, const Mat& R,
1251         const Mat& P, Size imgsize, Rect roi );
1252
1253     // covers of tested functions
1254     virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1255         const vector<vector<Point2f> >& imagePoints1,
1256         const vector<vector<Point2f> >& imagePoints2,
1257         Mat& cameraMatrix1, Mat& distCoeffs1,
1258         Mat& cameraMatrix2, Mat& distCoeffs2,
1259         Size imageSize, Mat& R, Mat& T,
1260         Mat& E, Mat& F, TermCriteria criteria, int flags ) = 0;
1261     virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1262         const Mat& cameraMatrix2, const Mat& distCoeffs2,
1263         Size imageSize, const Mat& R, const Mat& T,
1264         Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1265         double alpha, Size newImageSize,
1266         Rect* validPixROI1, Rect* validPixROI2, int flags ) = 0;
1267     virtual bool rectifyUncalibrated( const Mat& points1,
1268         const Mat& points2, const Mat& F, Size imgSize,
1269         Mat& H1, Mat& H2, double threshold=5 ) = 0;
1270     virtual void triangulate( const Mat& P1, const Mat& P2,
1271         const Mat &points1, const Mat &points2,
1272         Mat &points4D ) = 0;
1273     virtual void correct( const Mat& F,
1274         const Mat &points1, const Mat &points2,
1275         Mat &newPoints1, Mat &newPoints2 ) = 0;
1276
1277     void run(int);
1278 };
1279
1280
1281 CV_StereoCalibrationTest::CV_StereoCalibrationTest()
1282 {
1283 }
1284
1285
1286 CV_StereoCalibrationTest::~CV_StereoCalibrationTest()
1287 {
1288     clear();
1289 }
1290
1291 void CV_StereoCalibrationTest::clear()
1292 {
1293     cvtest::BaseTest::clear();
1294 }
1295
1296 bool CV_StereoCalibrationTest::checkPandROI( int test_case_idx, const Mat& M, const Mat& D, const Mat& R,
1297                                             const Mat& P, Size imgsize, Rect roi )
1298 {
1299     const double eps = 0.05;
1300     const int N = 21;
1301     int x, y, k;
1302     vector<Point2f> pts, upts;
1303
1304     // step 1. check that all the original points belong to the destination image
1305     for( y = 0; y < N; y++ )
1306         for( x = 0; x < N; x++ )
1307             pts.push_back(Point2f((float)x*imgsize.width/(N-1), (float)y*imgsize.height/(N-1)));
1308
1309     undistortPoints(pts, upts, M, D, R, P );
1310     for( k = 0; k < N*N; k++ )
1311         if( upts[k].x < -imgsize.width*eps || upts[k].x > imgsize.width*(1+eps) ||
1312             upts[k].y < -imgsize.height*eps || upts[k].y > imgsize.height*(1+eps) )
1313         {
1314             ts->printf(cvtest::TS::LOG, "Test #%d. The point (%g, %g) was mapped to (%g, %g) which is out of image\n",
1315                 test_case_idx, pts[k].x, pts[k].y, upts[k].x, upts[k].y);
1316             return false;
1317         }
1318
1319     // step 2. check that all the points inside ROI belong to the original source image
1320     Mat temp(imgsize, CV_8U), utemp, map1, map2;
1321     temp = Scalar::all(1);
1322     initUndistortRectifyMap(M, D, R, P, imgsize, CV_16SC2, map1, map2);
1323     remap(temp, utemp, map1, map2, INTER_LINEAR);
1324
1325     if(roi.x < 0 || roi.y < 0 || roi.x + roi.width > imgsize.width || roi.y + roi.height > imgsize.height)
1326     {
1327             ts->printf(cvtest::TS::LOG, "Test #%d. The ROI=(%d, %d, %d, %d) is outside of the imge rectangle\n",
1328                             test_case_idx, roi.x, roi.y, roi.width, roi.height);
1329             return false;
1330     }
1331     double s = sum(utemp(roi))[0];
1332     if( s > roi.area() || roi.area() - s > roi.area()*(1-eps) )
1333     {
1334             ts->printf(cvtest::TS::LOG, "Test #%d. The ratio of black pixels inside the valid ROI (~%g%%) is too large\n",
1335                             test_case_idx, s*100./roi.area());
1336             return false;
1337     }
1338
1339     return true;
1340 }
1341
1342 void CV_StereoCalibrationTest::run( int )
1343 {
1344     const int ntests = 1;
1345     const double maxReprojErr = 2;
1346     const double maxScanlineDistErr_c = 3;
1347     const double maxScanlineDistErr_uc = 4;
1348     FILE* f = 0;
1349
1350     for(int testcase = 1; testcase <= ntests; testcase++)
1351     {
1352         cv::String filepath;
1353         char buf[1000];
1354         filepath = cv::format("%scv/stereo/case%d/stereo_calib.txt", ts->get_data_path().c_str(), testcase );
1355         f = fopen(filepath.c_str(), "rt");
1356         Size patternSize;
1357         vector<string> imglist;
1358
1359         if( !f || !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &patternSize.width, &patternSize.height) != 2 )
1360         {
1361             ts->printf( cvtest::TS::LOG, "The file %s can not be opened or has invalid content\n", filepath.c_str() );
1362             ts->set_failed_test_info( f ? cvtest::TS::FAIL_INVALID_TEST_DATA : cvtest::TS::FAIL_MISSING_TEST_DATA );
1363             if (f)
1364                 fclose(f);
1365             return;
1366         }
1367
1368         for(;;)
1369         {
1370             if( !fgets( buf, sizeof(buf)-3, f ))
1371                 break;
1372             size_t len = strlen(buf);
1373             while( len > 0 && isspace(buf[len-1]))
1374                 buf[--len] = '\0';
1375             if( buf[0] == '#')
1376                 continue;
1377             filepath = cv::format("%scv/stereo/case%d/%s", ts->get_data_path().c_str(), testcase, buf );
1378             imglist.push_back(string(filepath));
1379         }
1380         fclose(f);
1381
1382         if( imglist.size() == 0 || imglist.size() % 2 != 0 )
1383         {
1384             ts->printf( cvtest::TS::LOG, "The number of images is 0 or an odd number in the case #%d\n", testcase );
1385             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
1386             return;
1387         }
1388
1389         int nframes = (int)(imglist.size()/2);
1390         int npoints = patternSize.width*patternSize.height;
1391         vector<vector<Point3f> > objpt(nframes);
1392         vector<vector<Point2f> > imgpt1(nframes);
1393         vector<vector<Point2f> > imgpt2(nframes);
1394         Size imgsize;
1395         int total = 0;
1396
1397         for( int i = 0; i < nframes; i++ )
1398         {
1399             Mat left = imread(imglist[i*2]);
1400             Mat right = imread(imglist[i*2+1]);
1401             if(left.empty() || right.empty())
1402             {
1403                 ts->printf( cvtest::TS::LOG, "Can not load images %s and %s, testcase %d\n",
1404                     imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
1405                 ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
1406                 return;
1407             }
1408             imgsize = left.size();
1409             bool found1 = findChessboardCorners(left, patternSize, imgpt1[i]);
1410             bool found2 = findChessboardCorners(right, patternSize, imgpt2[i]);
1411             if(!found1 || !found2)
1412             {
1413                 ts->printf( cvtest::TS::LOG, "The function could not detect boards (%d x %d) on the images %s and %s, testcase %d\n",
1414                     patternSize.width, patternSize.height,
1415                     imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
1416                 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1417                 return;
1418             }
1419             total += (int)imgpt1[i].size();
1420             for( int j = 0; j < npoints; j++ )
1421                 objpt[i].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f));
1422         }
1423
1424         // rectify (calibrated)
1425         Mat M1 = Mat::eye(3,3,CV_64F), M2 = Mat::eye(3,3,CV_64F), D1(5,1,CV_64F), D2(5,1,CV_64F), R, T, E, F;
1426         M1.at<double>(0,2) = M2.at<double>(0,2)=(imgsize.width-1)*0.5;
1427         M1.at<double>(1,2) = M2.at<double>(1,2)=(imgsize.height-1)*0.5;
1428         D1 = Scalar::all(0);
1429         D2 = Scalar::all(0);
1430         double err = calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R, T, E, F,
1431             TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6),
1432             CV_CALIB_SAME_FOCAL_LENGTH
1433             //+ CV_CALIB_FIX_ASPECT_RATIO
1434             + CV_CALIB_FIX_PRINCIPAL_POINT
1435             + CV_CALIB_ZERO_TANGENT_DIST
1436             + CV_CALIB_FIX_K3
1437             + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5 //+ CV_CALIB_FIX_K6
1438             );
1439         err /= nframes*npoints;
1440         if( err > maxReprojErr )
1441         {
1442             ts->printf( cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", err, testcase);
1443             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1444             return;
1445         }
1446
1447         Mat R1, R2, P1, P2, Q;
1448         Rect roi1, roi2;
1449         rectify(M1, D1, M2, D2, imgsize, R, T, R1, R2, P1, P2, Q, 1, imgsize, &roi1, &roi2, 0);
1450         Mat eye33 = Mat::eye(3,3,CV_64F);
1451         Mat R1t = R1.t(), R2t = R2.t();
1452
1453         if( cvtest::norm(R1t*R1 - eye33, NORM_L2) > 0.01 ||
1454             cvtest::norm(R2t*R2 - eye33, NORM_L2) > 0.01 ||
1455             abs(determinant(F)) > 0.01)
1456         {
1457             ts->printf( cvtest::TS::LOG, "The computed (by rectify) R1 and R2 are not orthogonal,"
1458                 "or the computed (by calibrate) F is not singular, testcase %d\n", testcase);
1459             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1460             return;
1461         }
1462
1463         if(!checkPandROI(testcase, M1, D1, R1, P1, imgsize, roi1))
1464         {
1465             ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1466             return;
1467         }
1468
1469         if(!checkPandROI(testcase, M2, D2, R2, P2, imgsize, roi2))
1470         {
1471             ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1472             return;
1473         }
1474
1475         //check that Tx after rectification is equal to distance between cameras
1476         double tx = fabs(P2.at<double>(0, 3) / P2.at<double>(0, 0));
1477         if (fabs(tx - cvtest::norm(T, NORM_L2)) > 1e-5)
1478         {
1479             ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1480             return;
1481         }
1482
1483         //check that Q reprojects points before the camera
1484         double testPoint[4] = {0.0, 0.0, 100.0, 1.0};
1485         Mat reprojectedTestPoint = Q * Mat_<double>(4, 1, testPoint);
1486         CV_Assert(reprojectedTestPoint.type() == CV_64FC1);
1487         if( reprojectedTestPoint.at<double>(2) / reprojectedTestPoint.at<double>(3) < 0 )
1488         {
1489             ts->printf( cvtest::TS::LOG, "A point after rectification is reprojected behind the camera, testcase %d\n", testcase);
1490             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1491         }
1492
1493         //check that Q reprojects the same points as reconstructed by triangulation
1494         const float minCoord = -300.0f;
1495         const float maxCoord = 300.0f;
1496         const float minDisparity = 0.1f;
1497         const float maxDisparity = 60.0f;
1498         const int pointsCount = 500;
1499         const float requiredAccuracy = 1e-3f;
1500         const float allowToFail = 0.2f; // 20%
1501         RNG& rng = ts->get_rng();
1502
1503         Mat projectedPoints_1(2, pointsCount, CV_32FC1);
1504         Mat projectedPoints_2(2, pointsCount, CV_32FC1);
1505         Mat disparities(1, pointsCount, CV_32FC1);
1506
1507         rng.fill(projectedPoints_1, RNG::UNIFORM, minCoord, maxCoord);
1508         rng.fill(disparities, RNG::UNIFORM, minDisparity, maxDisparity);
1509         projectedPoints_2.row(0) = projectedPoints_1.row(0) - disparities;
1510         Mat ys_2 = projectedPoints_2.row(1);
1511         projectedPoints_1.row(1).copyTo(ys_2);
1512
1513         Mat points4d;
1514         triangulate(P1, P2, projectedPoints_1, projectedPoints_2, points4d);
1515         Mat homogeneousPoints4d = points4d.t();
1516         const int dimension = 4;
1517         homogeneousPoints4d = homogeneousPoints4d.reshape(dimension);
1518         Mat triangulatedPoints;
1519         convertPointsFromHomogeneous(homogeneousPoints4d, triangulatedPoints);
1520
1521         Mat sparsePoints;
1522         sparsePoints.push_back(projectedPoints_1);
1523         sparsePoints.push_back(disparities);
1524         sparsePoints = sparsePoints.t();
1525         sparsePoints = sparsePoints.reshape(3);
1526         Mat reprojectedPoints;
1527         perspectiveTransform(sparsePoints, reprojectedPoints, Q);
1528
1529         Mat diff;
1530         absdiff(triangulatedPoints, reprojectedPoints, diff);
1531         Mat mask = diff > requiredAccuracy;
1532         mask = mask.reshape(1);
1533         mask = mask.col(0) | mask.col(1) | mask.col(2);
1534         int numFailed = countNonZero(mask);
1535 #if 0
1536         std::cout << "numFailed=" << numFailed << std::endl;
1537         for (int i = 0; i < triangulatedPoints.rows; i++)
1538         {
1539             if (mask.at<uchar>(i))
1540             {
1541                 // failed points usually have 'w'~0 (points4d[3])
1542                 std::cout << "i=" << i << " triangulatePoints=" << triangulatedPoints.row(i) << " reprojectedPoints=" << reprojectedPoints.row(i) << std::endl <<
1543                     "     points4d=" << points4d.col(i).t() << " projectedPoints_1=" << projectedPoints_1.col(i).t() << " disparities=" << disparities.col(i).t() << std::endl;
1544             }
1545         }
1546 #endif
1547
1548         if (numFailed >= allowToFail * pointsCount)
1549         {
1550             ts->printf( cvtest::TS::LOG, "Points reprojected with a matrix Q and points reconstructed by triangulation are different (tolerance=%g, failed=%d), testcase %d\n",
1551                 requiredAccuracy, numFailed, testcase);
1552             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1553         }
1554
1555         //check correctMatches
1556         const float constraintAccuracy = 1e-5f;
1557         Mat newPoints1, newPoints2;
1558         Mat points1 = projectedPoints_1.t();
1559         points1 = points1.reshape(2, 1);
1560         Mat points2 = projectedPoints_2.t();
1561         points2 = points2.reshape(2, 1);
1562         correctMatches(F, points1, points2, newPoints1, newPoints2);
1563         Mat newHomogeneousPoints1, newHomogeneousPoints2;
1564         convertPointsToHomogeneous(newPoints1, newHomogeneousPoints1);
1565         convertPointsToHomogeneous(newPoints2, newHomogeneousPoints2);
1566         newHomogeneousPoints1 = newHomogeneousPoints1.reshape(1);
1567         newHomogeneousPoints2 = newHomogeneousPoints2.reshape(1);
1568         Mat typedF;
1569         F.convertTo(typedF, newHomogeneousPoints1.type());
1570         for (int i = 0; i < newHomogeneousPoints1.rows; ++i)
1571         {
1572             Mat error = newHomogeneousPoints2.row(i) * typedF * newHomogeneousPoints1.row(i).t();
1573             CV_Assert(error.rows == 1 && error.cols == 1);
1574             if (cvtest::norm(error, NORM_L2) > constraintAccuracy)
1575             {
1576                 ts->printf( cvtest::TS::LOG, "Epipolar constraint is violated after correctMatches, testcase %d\n", testcase);
1577                 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1578             }
1579         }
1580
1581         // rectifyUncalibrated
1582         CV_Assert( imgpt1.size() == imgpt2.size() );
1583         Mat _imgpt1( total, 1, CV_32FC2 ), _imgpt2( total, 1, CV_32FC2 );
1584         vector<vector<Point2f> >::const_iterator iit1 = imgpt1.begin();
1585         vector<vector<Point2f> >::const_iterator iit2 = imgpt2.begin();
1586         for( int pi = 0; iit1 != imgpt1.end(); ++iit1, ++iit2 )
1587         {
1588             vector<Point2f>::const_iterator pit1 = iit1->begin();
1589             vector<Point2f>::const_iterator pit2 = iit2->begin();
1590             CV_Assert( iit1->size() == iit2->size() );
1591             for( ; pit1 != iit1->end(); ++pit1, ++pit2, pi++ )
1592             {
1593                 _imgpt1.at<Point2f>(pi,0) = Point2f( pit1->x, pit1->y );
1594                 _imgpt2.at<Point2f>(pi,0) = Point2f( pit2->x, pit2->y );
1595             }
1596         }
1597
1598         Mat _M1, _M2, _D1, _D2;
1599         vector<Mat> _R1, _R2, _T1, _T2;
1600         calibrateCamera( objpt, imgpt1, imgsize, _M1, _D1, _R1, _T1, 0 );
1601         calibrateCamera( objpt, imgpt2, imgsize, _M2, _D2, _R2, _T2, 0 );
1602         undistortPoints( _imgpt1, _imgpt1, _M1, _D1, Mat(), _M1 );
1603         undistortPoints( _imgpt2, _imgpt2, _M2, _D2, Mat(), _M2 );
1604
1605         Mat matF, _H1, _H2;
1606         matF = findFundamentalMat( _imgpt1, _imgpt2 );
1607         rectifyUncalibrated( _imgpt1, _imgpt2, matF, imgsize, _H1, _H2 );
1608
1609         Mat rectifPoints1, rectifPoints2;
1610         perspectiveTransform( _imgpt1, rectifPoints1, _H1 );
1611         perspectiveTransform( _imgpt2, rectifPoints2, _H2 );
1612
1613         bool verticalStereo = abs(P2.at<double>(0,3)) < abs(P2.at<double>(1,3));
1614         double maxDiff_c = 0, maxDiff_uc = 0;
1615         for( int i = 0, k = 0; i < nframes; i++ )
1616         {
1617             vector<Point2f> temp[2];
1618             undistortPoints(imgpt1[i], temp[0], M1, D1, R1, P1);
1619             undistortPoints(imgpt2[i], temp[1], M2, D2, R2, P2);
1620
1621             for( int j = 0; j < npoints; j++, k++ )
1622             {
1623                 double diff_c = verticalStereo ? abs(temp[0][j].x - temp[1][j].x) : abs(temp[0][j].y - temp[1][j].y);
1624                 Point2f d = rectifPoints1.at<Point2f>(k,0) - rectifPoints2.at<Point2f>(k,0);
1625                 double diff_uc = verticalStereo ? abs(d.x) : abs(d.y);
1626                 maxDiff_c = max(maxDiff_c, diff_c);
1627                 maxDiff_uc = max(maxDiff_uc, diff_uc);
1628                 if( maxDiff_c > maxScanlineDistErr_c )
1629                 {
1630                     ts->printf( cvtest::TS::LOG, "The distance between %s coordinates is too big(=%g) (used calibrated stereo), testcase %d\n",
1631                         verticalStereo ? "x" : "y", diff_c, testcase);
1632                     ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1633                     return;
1634                 }
1635                 if( maxDiff_uc > maxScanlineDistErr_uc )
1636                 {
1637                     ts->printf( cvtest::TS::LOG, "The distance between %s coordinates is too big(=%g) (used uncalibrated stereo), testcase %d\n",
1638                         verticalStereo ? "x" : "y", diff_uc, testcase);
1639                     ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1640                     return;
1641                 }
1642             }
1643         }
1644
1645         ts->printf( cvtest::TS::LOG, "Testcase %d. Max distance (calibrated) =%g\n"
1646             "Max distance (uncalibrated) =%g\n", testcase, maxDiff_c, maxDiff_uc );
1647     }
1648 }
1649
1650 //-------------------------------- CV_StereoCalibrationTest_CPP ------------------------------
1651
1652 class CV_StereoCalibrationTest_CPP : public CV_StereoCalibrationTest
1653 {
1654 public:
1655     CV_StereoCalibrationTest_CPP() {}
1656 protected:
1657     virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1658         const vector<vector<Point2f> >& imagePoints1,
1659         const vector<vector<Point2f> >& imagePoints2,
1660         Mat& cameraMatrix1, Mat& distCoeffs1,
1661         Mat& cameraMatrix2, Mat& distCoeffs2,
1662         Size imageSize, Mat& R, Mat& T,
1663         Mat& E, Mat& F, TermCriteria criteria, int flags );
1664     virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1665         const Mat& cameraMatrix2, const Mat& distCoeffs2,
1666         Size imageSize, const Mat& R, const Mat& T,
1667         Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1668         double alpha, Size newImageSize,
1669         Rect* validPixROI1, Rect* validPixROI2, int flags );
1670     virtual bool rectifyUncalibrated( const Mat& points1,
1671         const Mat& points2, const Mat& F, Size imgSize,
1672         Mat& H1, Mat& H2, double threshold=5 );
1673     virtual void triangulate( const Mat& P1, const Mat& P2,
1674         const Mat &points1, const Mat &points2,
1675         Mat &points4D );
1676     virtual void correct( const Mat& F,
1677         const Mat &points1, const Mat &points2,
1678         Mat &newPoints1, Mat &newPoints2 );
1679 };
1680
1681 double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1682                                              const vector<vector<Point2f> >& imagePoints1,
1683                                              const vector<vector<Point2f> >& imagePoints2,
1684                                              Mat& cameraMatrix1, Mat& distCoeffs1,
1685                                              Mat& cameraMatrix2, Mat& distCoeffs2,
1686                                              Size imageSize, Mat& R, Mat& T,
1687                                              Mat& E, Mat& F, TermCriteria criteria, int flags )
1688 {
1689     return stereoCalibrate( objectPoints, imagePoints1, imagePoints2,
1690                     cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
1691                     imageSize, R, T, E, F, flags, criteria );
1692 }
1693
1694 void CV_StereoCalibrationTest_CPP::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1695                                          const Mat& cameraMatrix2, const Mat& distCoeffs2,
1696                                          Size imageSize, const Mat& R, const Mat& T,
1697                                          Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1698                                          double alpha, Size newImageSize,
1699                                          Rect* validPixROI1, Rect* validPixROI2, int flags )
1700 {
1701     stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
1702                 imageSize, R, T, R1, R2, P1, P2, Q, flags, alpha, newImageSize,validPixROI1, validPixROI2 );
1703 }
1704
1705 bool CV_StereoCalibrationTest_CPP::rectifyUncalibrated( const Mat& points1,
1706                        const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold )
1707 {
1708     return stereoRectifyUncalibrated( points1, points2, F, imgSize, H1, H2, threshold );
1709 }
1710
1711 void CV_StereoCalibrationTest_CPP::triangulate( const Mat& P1, const Mat& P2,
1712         const Mat &points1, const Mat &points2,
1713         Mat &points4D )
1714 {
1715     triangulatePoints(P1, P2, points1, points2, points4D);
1716 }
1717
1718 void CV_StereoCalibrationTest_CPP::correct( const Mat& F,
1719         const Mat &points1, const Mat &points2,
1720         Mat &newPoints1, Mat &newPoints2 )
1721 {
1722     correctMatches(F, points1, points2, newPoints1, newPoints2);
1723 }
1724
1725 ///////////////////////////////////////////////////////////////////////////////////////////////////
1726
1727 TEST(Calib3d_CalibrateCamera_CPP, regression) { CV_CameraCalibrationTest_CPP test; test.safe_run(); }
1728 TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); }
1729 TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); }
1730
1731 TEST(Calib3d_ProjectPoints_CPP, inputShape)
1732 {
1733     Matx31d rvec = Matx31d::zeros();
1734     Matx31d tvec(0, 0, 1);
1735     Matx33d cameraMatrix = Matx33d::eye();
1736     const float L = 0.1f;
1737     {
1738         //3xN 1-channel
1739         Mat objectPoints = (Mat_<float>(3, 2) << -L,  L,
1740                                                   L,  L,
1741                                                   0,  0);
1742         vector<Point2f> imagePoints;
1743         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1744         EXPECT_EQ(objectPoints.cols, static_cast<int>(imagePoints.size()));
1745         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
1746         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<float>::epsilon());
1747         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<float>::epsilon());
1748         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<float>::epsilon());
1749     }
1750     {
1751         //Nx2 1-channel
1752         Mat objectPoints = (Mat_<float>(2, 3) << -L,  L, 0,
1753                                                   L,  L, 0);
1754         vector<Point2f> imagePoints;
1755         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1756         EXPECT_EQ(objectPoints.rows, static_cast<int>(imagePoints.size()));
1757         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
1758         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<float>::epsilon());
1759         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<float>::epsilon());
1760         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<float>::epsilon());
1761     }
1762     {
1763         //1xN 3-channel
1764         Mat objectPoints(1, 2, CV_32FC3);
1765         objectPoints.at<Vec3f>(0,0) = Vec3f(-L, L, 0);
1766         objectPoints.at<Vec3f>(0,1) = Vec3f(L, L, 0);
1767
1768         vector<Point2f> imagePoints;
1769         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1770         EXPECT_EQ(objectPoints.cols, static_cast<int>(imagePoints.size()));
1771         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
1772         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<float>::epsilon());
1773         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<float>::epsilon());
1774         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<float>::epsilon());
1775     }
1776     {
1777         //Nx1 3-channel
1778         Mat objectPoints(2, 1, CV_32FC3);
1779         objectPoints.at<Vec3f>(0,0) = Vec3f(-L, L, 0);
1780         objectPoints.at<Vec3f>(1,0) = Vec3f(L, L, 0);
1781
1782         vector<Point2f> imagePoints;
1783         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1784         EXPECT_EQ(objectPoints.rows, static_cast<int>(imagePoints.size()));
1785         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
1786         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<float>::epsilon());
1787         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<float>::epsilon());
1788         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<float>::epsilon());
1789     }
1790     {
1791         //vector<Point3f>
1792         vector<Point3f> objectPoints;
1793         objectPoints.push_back(Point3f(-L, L, 0));
1794         objectPoints.push_back(Point3f(L, L, 0));
1795
1796         vector<Point2f> imagePoints;
1797         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1798         EXPECT_EQ(objectPoints.size(), imagePoints.size());
1799         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
1800         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<float>::epsilon());
1801         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<float>::epsilon());
1802         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<float>::epsilon());
1803     }
1804     {
1805         //vector<Point3d>
1806         vector<Point3d> objectPoints;
1807         objectPoints.push_back(Point3d(-L, L, 0));
1808         objectPoints.push_back(Point3d(L, L, 0));
1809
1810         vector<Point2d> imagePoints;
1811         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1812         EXPECT_EQ(objectPoints.size(), imagePoints.size());
1813         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<double>::epsilon());
1814         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<double>::epsilon());
1815         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<double>::epsilon());
1816         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<double>::epsilon());
1817     }
1818 }
1819
1820 TEST(Calib3d_ProjectPoints_CPP, outputShape)
1821 {
1822     Matx31d rvec = Matx31d::zeros();
1823     Matx31d tvec(0, 0, 1);
1824     Matx33d cameraMatrix = Matx33d::eye();
1825     const float L = 0.1f;
1826     {
1827         vector<Point3f> objectPoints;
1828         objectPoints.push_back(Point3f(-L,  L, 0));
1829         objectPoints.push_back(Point3f( L,  L, 0));
1830         objectPoints.push_back(Point3f( L, -L, 0));
1831
1832         //Mat --> will be Nx1 2-channel
1833         Mat imagePoints;
1834         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1835         EXPECT_EQ(static_cast<int>(objectPoints.size()), imagePoints.rows);
1836         EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(0), -L, std::numeric_limits<float>::epsilon());
1837         EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(1),  L, std::numeric_limits<float>::epsilon());
1838         EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(0),  L, std::numeric_limits<float>::epsilon());
1839         EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(1),  L, std::numeric_limits<float>::epsilon());
1840         EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(0),  L, std::numeric_limits<float>::epsilon());
1841         EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(1), -L, std::numeric_limits<float>::epsilon());
1842     }
1843     {
1844         vector<Point3f> objectPoints;
1845         objectPoints.push_back(Point3f(-L,  L, 0));
1846         objectPoints.push_back(Point3f( L,  L, 0));
1847         objectPoints.push_back(Point3f( L, -L, 0));
1848
1849         //Nx1 2-channel
1850         Mat imagePoints(3,1,CV_32FC2);
1851         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1852         EXPECT_EQ(static_cast<int>(objectPoints.size()), imagePoints.rows);
1853         EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(0), -L, std::numeric_limits<float>::epsilon());
1854         EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(1),  L, std::numeric_limits<float>::epsilon());
1855         EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(0),  L, std::numeric_limits<float>::epsilon());
1856         EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(1),  L, std::numeric_limits<float>::epsilon());
1857         EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(0),  L, std::numeric_limits<float>::epsilon());
1858         EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(1), -L, std::numeric_limits<float>::epsilon());
1859     }
1860     {
1861         vector<Point3f> objectPoints;
1862         objectPoints.push_back(Point3f(-L,  L, 0));
1863         objectPoints.push_back(Point3f( L,  L, 0));
1864         objectPoints.push_back(Point3f( L, -L, 0));
1865
1866         //1xN 2-channel
1867         Mat imagePoints(1,3,CV_32FC2);
1868         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1869         EXPECT_EQ(static_cast<int>(objectPoints.size()), imagePoints.cols);
1870         EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(0), -L, std::numeric_limits<float>::epsilon());
1871         EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(1),  L, std::numeric_limits<float>::epsilon());
1872         EXPECT_NEAR(imagePoints.at<Vec2f>(0,1)(0),  L, std::numeric_limits<float>::epsilon());
1873         EXPECT_NEAR(imagePoints.at<Vec2f>(0,1)(1),  L, std::numeric_limits<float>::epsilon());
1874         EXPECT_NEAR(imagePoints.at<Vec2f>(0,2)(0),  L, std::numeric_limits<float>::epsilon());
1875         EXPECT_NEAR(imagePoints.at<Vec2f>(0,2)(1), -L, std::numeric_limits<float>::epsilon());
1876     }
1877     {
1878         vector<Point3f> objectPoints;
1879         objectPoints.push_back(Point3f(-L, L, 0));
1880         objectPoints.push_back(Point3f(L, L, 0));
1881
1882         //vector<Point2f>
1883         vector<Point2f> imagePoints;
1884         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1885         EXPECT_EQ(objectPoints.size(), imagePoints.size());
1886         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
1887         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<float>::epsilon());
1888         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<float>::epsilon());
1889         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<float>::epsilon());
1890     }
1891     {
1892         vector<Point3d> objectPoints;
1893         objectPoints.push_back(Point3d(-L, L, 0));
1894         objectPoints.push_back(Point3d(L, L, 0));
1895
1896         //vector<Point2d>
1897         vector<Point2d> imagePoints;
1898         projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
1899         EXPECT_EQ(objectPoints.size(), imagePoints.size());
1900         EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<double>::epsilon());
1901         EXPECT_NEAR(imagePoints[0].y,  L, std::numeric_limits<double>::epsilon());
1902         EXPECT_NEAR(imagePoints[1].x,  L, std::numeric_limits<double>::epsilon());
1903         EXPECT_NEAR(imagePoints[1].y,  L, std::numeric_limits<double>::epsilon());
1904     }
1905 }
1906
1907 TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); }
1908
1909
1910 TEST(Calib3d_StereoCalibrate_CPP, extended)
1911 {
1912     cvtest::TS* ts = cvtest::TS::ptr();
1913     String filepath = cv::format("%scv/stereo/case%d/", ts->get_data_path().c_str(), 1 );
1914
1915     Mat left = imread(filepath+"left01.png");
1916     Mat right = imread(filepath+"right01.png");
1917     if(left.empty() || right.empty())
1918     {
1919         ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
1920         return;
1921     }
1922
1923     vector<vector<Point2f> > imgpt1(1), imgpt2(1);
1924     vector<vector<Point3f> > objpt(1);
1925     Size patternSize(9, 6), imageSize(640, 480);
1926     bool found1 = findChessboardCorners(left, patternSize, imgpt1[0]);
1927     bool found2 = findChessboardCorners(right, patternSize, imgpt2[0]);
1928
1929     if(!found1 || !found2)
1930     {
1931         ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1932         return;
1933     }
1934
1935     for( int j = 0; j < patternSize.width*patternSize.height; j++ )
1936         objpt[0].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f));
1937
1938     Mat K1, K2, c1, c2, R, T, E, F, err;
1939     int flags = 0;
1940     double res0 = stereoCalibrate( objpt, imgpt1, imgpt2,
1941                     K1, c1, K2, c2,
1942                     imageSize, R, T, E, F, err, flags);
1943
1944     flags = CALIB_USE_EXTRINSIC_GUESS;
1945     double res1 = stereoCalibrate( objpt, imgpt1, imgpt2,
1946                     K1, c1, K2, c2,
1947                     imageSize, R, T, E, F, err, flags);
1948     EXPECT_LE(res1, res0);
1949     EXPECT_TRUE(err.total() == 2);
1950 }
1951
1952 TEST(Calib3d_StereoCalibrate, regression_10791)
1953 {
1954     const Matx33d M1(
1955         853.1387981631528, 0, 704.154907802121,
1956         0, 853.6445089162528, 520.3600712930319,
1957         0, 0, 1
1958     );
1959     const Matx33d M2(
1960         848.6090216909176, 0, 701.6162856852185,
1961         0, 849.7040162357157, 509.1864036137,
1962         0, 0, 1
1963     );
1964     const Matx<double, 14, 1> D1(-6.463598629567206, 79.00104930508179, -0.0001006144444464403, -0.0005437499822299972,
1965         12.56900616588467, -6.056719942752855, 76.3842481414836, 45.57460250612659,
1966         0, 0, 0, 0, 0, 0);
1967     const Matx<double, 14, 1> D2(0.6123436439798265, -0.4671756923224087, -0.0001261947899033442, -0.000597334584036978,
1968         -0.05660119809538371, 1.037075740629769, -0.3076042835831711, -0.2502169324283623,
1969         0, 0, 0, 0, 0, 0);
1970
1971     const Matx33d R(
1972         0.9999926627018476, -0.0001095586963765905, 0.003829169539302921,
1973         0.0001021735876758584, 0.9999981346680941, 0.0019287874145156,
1974         -0.003829373712065528, -0.001928382022437616, 0.9999908085776333
1975     );
1976     const Matx31d T(-58.9161771697128, -0.01581306249996402, -0.8492960216760961);
1977
1978     const Size imageSize(1280, 960);
1979
1980     Mat R1, R2, P1, P2, Q;
1981     Rect roi1, roi2;
1982     stereoRectify(M1, D1, M2, D2, imageSize, R, T,
1983                   R1, R2, P1, P2, Q,
1984                   CALIB_ZERO_DISPARITY, 1, imageSize, &roi1, &roi2);
1985
1986     EXPECT_GE(roi1.area(), 400*300) << roi1;
1987     EXPECT_GE(roi2.area(), 400*300) << roi2;
1988 }
1989
1990 TEST(Calib3d_StereoCalibrate, regression_11131)
1991 {
1992     const Matx33d M1(
1993         1457.572438721727, 0, 1212.945694211622,
1994         0, 1457.522226502963, 1007.32058848921,
1995         0, 0, 1
1996     );
1997     const Matx33d M2(
1998         1460.868570835972, 0, 1215.024068023046,
1999         0, 1460.791367088, 1011.107202932225,
2000         0, 0, 1
2001     );
2002     const Matx<double, 5, 1> D1(0, 0, 0, 0, 0);
2003     const Matx<double, 5, 1> D2(0, 0, 0, 0, 0);
2004
2005     const Matx33d R(
2006         0.9985404059825475, 0.02963547172078553, -0.04515303352041626,
2007         -0.03103795276460111, 0.9990471552537432, -0.03068268351343364,
2008         0.04420071389006859, 0.03203935697372317, 0.9985087763742083
2009     );
2010     const Matx31d T(0.9995500167379527, 0.0116311595111068, 0.02764923448462666);
2011
2012     const Size imageSize(2456, 2058);
2013
2014     Mat R1, R2, P1, P2, Q;
2015     Rect roi1, roi2;
2016     stereoRectify(M1, D1, M2, D2, imageSize, R, T,
2017                   R1, R2, P1, P2, Q,
2018                   CALIB_ZERO_DISPARITY, 1, imageSize, &roi1, &roi2);
2019
2020     EXPECT_GT(P1.at<double>(0, 0), 0);
2021     EXPECT_GT(P2.at<double>(0, 0), 0);
2022     EXPECT_GT(R1.at<double>(0, 0), 0);
2023     EXPECT_GT(R2.at<double>(0, 0), 0);
2024     EXPECT_GE(roi1.area(), 400*300) << roi1;
2025     EXPECT_GE(roi2.area(), 400*300) << roi2;
2026 }
2027
2028 TEST(Calib3d_Triangulate, accuracy)
2029 {
2030     // the testcase from http://code.opencv.org/issues/4334
2031     {
2032     double P1data[] = { 250, 0, 200, 0, 0, 250, 150, 0, 0, 0, 1, 0 };
2033     double P2data[] = { 250, 0, 200, -250, 0, 250, 150, 0, 0, 0, 1, 0 };
2034     Mat P1(3, 4, CV_64F, P1data), P2(3, 4, CV_64F, P2data);
2035
2036     float x1data[] = { 200.f, 0.f };
2037     float x2data[] = { 170.f, 1.f };
2038     float Xdata[] = { 0.f, -5.f, 25/3.f };
2039     Mat x1(2, 1, CV_32F, x1data);
2040     Mat x2(2, 1, CV_32F, x2data);
2041     Mat res0(1, 3, CV_32F, Xdata);
2042     Mat res_, res;
2043
2044     triangulatePoints(P1, P2, x1, x2, res_);
2045     cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace)
2046     convertPointsFromHomogeneous(res_, res);
2047     res = res.reshape(1, 1);
2048
2049     cout << "[1]:" << endl;
2050     cout << "\tres0: " << res0 << endl;
2051     cout << "\tres: " << res << endl;
2052
2053     ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 1e-1);
2054     }
2055
2056     // another testcase http://code.opencv.org/issues/3461
2057     {
2058     Matx33d K1(6137.147949, 0.000000, 644.974609,
2059                0.000000, 6137.147949, 573.442749,
2060                0.000000, 0.000000,  1.000000);
2061     Matx33d K2(6137.147949,  0.000000, 644.674438,
2062                0.000000, 6137.147949, 573.079834,
2063                0.000000,  0.000000, 1.000000);
2064
2065     Matx34d RT1(1, 0, 0, 0,
2066                 0, 1, 0, 0,
2067                 0, 0, 1, 0);
2068
2069     Matx34d RT2(0.998297, 0.0064108, -0.0579766,     143.614334,
2070                -0.0065818, 0.999975, -0.00275888,   -5.160085,
2071                0.0579574, 0.00313577, 0.998314,     96.066109);
2072
2073     Matx34d P1 = K1*RT1;
2074     Matx34d P2 = K2*RT2;
2075
2076     float x1data[] = { 438.f, 19.f };
2077     float x2data[] = { 452.363600f, 16.452225f };
2078     float Xdata[] = { -81.049530f, -215.702804f, 2401.645449f };
2079     Mat x1(2, 1, CV_32F, x1data);
2080     Mat x2(2, 1, CV_32F, x2data);
2081     Mat res0(1, 3, CV_32F, Xdata);
2082     Mat res_, res;
2083
2084     triangulatePoints(P1, P2, x1, x2, res_);
2085     cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace)
2086     convertPointsFromHomogeneous(res_, res);
2087     res = res.reshape(1, 1);
2088
2089     cout << "[2]:" << endl;
2090     cout << "\tres0: " << res0 << endl;
2091     cout << "\tres: " << res << endl;
2092
2093     ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 2);
2094     }
2095 }
2096
2097 ///////////////////////////////////////////////////////////////////////////////////////////////////
2098
2099 TEST(CV_RecoverPoseTest, regression_15341)
2100 {
2101     // initialize test data
2102     const int invalid_point_count = 2;
2103     const float _points1_[] = {
2104         1537.7f, 166.8f,
2105         1599.1f, 179.6f,
2106         1288.0f, 207.5f,
2107         1507.1f, 193.2f,
2108         1742.7f, 210.0f,
2109         1041.6f, 271.7f,
2110         1591.8f, 247.2f,
2111         1524.0f, 261.3f,
2112         1330.3f, 285.0f,
2113         1403.1f, 284.0f,
2114         1506.6f, 342.9f,
2115         1502.8f, 347.3f,
2116         1344.9f, 364.9f,
2117         0.0f, 0.0f  // last point is initial invalid
2118     };
2119
2120     const float _points2_[] = {
2121         1533.4f, 532.9f,
2122         1596.6f, 552.4f,
2123         1277.0f, 556.4f,
2124         1502.1f, 557.6f,
2125         1744.4f, 601.3f,
2126         1023.0f, 612.6f,
2127         1589.2f, 621.6f,
2128         1519.4f, 629.0f,
2129         1320.3f, 637.3f,
2130         1395.2f, 642.2f,
2131         1501.5f, 710.3f,
2132         1497.6f, 714.2f,
2133         1335.1f, 719.61f,
2134         1000.0f, 1000.0f  // last point is initial invalid
2135     };
2136
2137     vector<Point2f> _points1; Mat(14, 1, CV_32FC2, (void*)_points1_).copyTo(_points1);
2138     vector<Point2f> _points2; Mat(14, 1, CV_32FC2, (void*)_points2_).copyTo(_points2);
2139
2140     const int point_count = (int) _points1.size();
2141     CV_Assert(point_count == (int) _points2.size());
2142
2143     // camera matrix with both focal lengths = 1, and principal point = (0, 0)
2144     const Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
2145     const Mat zeroDistCoeffs = Mat::zeros(1, 5, CV_64F);
2146
2147     int Inliers = 0;
2148
2149     const int ntests = 3;
2150     for (int testcase = 1; testcase <= ntests; ++testcase)
2151     {
2152         if (testcase == 1) // testcase with vector input data
2153         {
2154             // init temporary test data
2155             vector<unsigned char> mask(point_count);
2156             vector<Point2f> points1(_points1);
2157             vector<Point2f> points2(_points2);
2158
2159             // Estimation of fundamental matrix using the RANSAC algorithm
2160             Mat E, E2, R, t;
2161             E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, mask);
2162             E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask);
2163             EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) <<
2164                 "Two big difference between the same essential matrices computed using different functions, testcase " << testcase;
2165             EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function findEssentialMat failed, testcase " << testcase;
2166             points2[12] = Point2f(0.0f, 0.0f); // provoke another outlier detection for recover Pose
2167             Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask);
2168             EXPECT_EQ(0, (int)mask[12]) << "Detecting outliers in function failed, testcase " << testcase;
2169         }
2170         else // testcase with mat input data
2171         {
2172             Mat points1(_points1, true);
2173             Mat points2(_points2, true);
2174             Mat mask;
2175
2176             if (testcase == 2)
2177             {
2178                 // init temporary testdata
2179                 mask = Mat::zeros(point_count, 1, CV_8UC1);
2180             }
2181             else // testcase == 3 - with transposed mask
2182             {
2183                 mask = Mat::zeros(1, point_count, CV_8UC1);
2184             }
2185
2186             // Estimation of fundamental matrix using the RANSAC algorithm
2187             Mat E, E2, R, t;
2188             E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, mask);
2189             E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask);
2190             EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) <<
2191                 "Two big difference between the same essential matrices computed using different functions, testcase " << testcase;
2192             EXPECT_EQ(0, (int)mask.at<unsigned char>(13)) << "Detecting outliers in function findEssentialMat failed, testcase " << testcase;
2193             points2.at<Point2f>(12) = Point2f(0.0f, 0.0f); // provoke an outlier detection
2194             Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask);
2195             EXPECT_EQ(0, (int)mask.at<unsigned char>(12)) << "Detecting outliers in function failed, testcase " << testcase;
2196         }
2197         EXPECT_EQ(Inliers, point_count - invalid_point_count) <<
2198             "Number of inliers differs from expected number of inliers, testcase " << testcase;
2199     }
2200 }
2201
2202 }} // namespace