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
  44 #include <limits>
  45
  46 #if 0
  47 class CV_ProjectPointsTest : public cvtest::ArrayTest
  48 {
  49 public:
  50     CV_ProjectPointsTest();
  51
  52 protected:
  53     int read_params( CvFileStorage* fs );
  54     void fill_array( int test_case_idx, int i, int j, Mat& arr );
  55     int prepare_test_case( int test_case_idx );
  56     void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
  57     double get_success_error_level( int test_case_idx, int i, int j );
  58     void run_func();
  59     void prepare_to_validation( int );
  60
  61     bool calc_jacobians;
  62 };
  63
  64
  65 CV_ProjectPointsTest::CV_ProjectPointsTest()
  66     : cvtest::ArrayTest( "3d-ProjectPoints", "cvProjectPoints2", "" )
  67 {
  68     test_array[INPUT].push_back(NULL);  // rotation vector
  69     test_array[OUTPUT].push_back(NULL); // rotation matrix
  70     test_array[OUTPUT].push_back(NULL); // jacobian (J)
  71     test_array[OUTPUT].push_back(NULL); // rotation vector (backward transform result)
  72     test_array[OUTPUT].push_back(NULL); // inverse transform jacobian (J1)
  73     test_array[OUTPUT].push_back(NULL); // J*J1 (or J1*J) == I(3x3)
  74     test_array[REF_OUTPUT].push_back(NULL);
  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
  80     element_wise_relative_error = false;
  81     calc_jacobians = false;
  82 }
  83
  84
  85 int CV_ProjectPointsTest::read_params( CvFileStorage* fs )
  86 {
  87     int code = cvtest::ArrayTest::read_params( fs );
  88     return code;
  89 }
  90
  91
  92 void CV_ProjectPointsTest::get_test_array_types_and_sizes(
  93     int /*test_case_idx*/, vector<vector<Size> >& sizes, vector<vector<int> >& types )
  94 {
  95     RNG& rng = ts->get_rng();
  96     int depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F;
  97     int i, code;
  98
  99     code = cvtest::randInt(rng) % 3;
 100     types[INPUT][0] = CV_MAKETYPE(depth, 1);
 101
 102     if( code == 0 )
 103     {
 104         sizes[INPUT][0] = cvSize(1,1);
 105         types[INPUT][0] = CV_MAKETYPE(depth, 3);
 106     }
 107     else if( code == 1 )
 108         sizes[INPUT][0] = cvSize(3,1);
 109     else
 110         sizes[INPUT][0] = cvSize(1,3);
 111
 112     sizes[OUTPUT][0] = cvSize(3, 3);
 113     types[OUTPUT][0] = CV_MAKETYPE(depth, 1);
 114
 115     types[OUTPUT][1] = CV_MAKETYPE(depth, 1);
 116
 117     if( cvtest::randInt(rng) % 2 )
 118         sizes[OUTPUT][1] = cvSize(3,9);
 119     else
 120         sizes[OUTPUT][1] = cvSize(9,3);
 121
 122     types[OUTPUT][2] = types[INPUT][0];
 123     sizes[OUTPUT][2] = sizes[INPUT][0];
 124
 125     types[OUTPUT][3] = types[OUTPUT][1];
 126     sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width);
 127
 128     types[OUTPUT][4] = types[OUTPUT][1];
 129     sizes[OUTPUT][4] = cvSize(3,3);
 130
 131     calc_jacobians = 1;//cvtest::randInt(rng) % 3 != 0;
 132     if( !calc_jacobians )
 133         sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0);
 134
 135     for( i = 0; i < 5; i++ )
 136     {
 137         types[REF_OUTPUT][i] = types[OUTPUT][i];
 138         sizes[REF_OUTPUT][i] = sizes[OUTPUT][i];
 139     }
 140 }
 141
 142
 143 double CV_ProjectPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j )
 144 {
 145     return j == 4 ? 1e-2 : 1e-2;
 146 }
 147
 148
 149 void CV_ProjectPointsTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr )
 150 {
 151     double r[3], theta0, theta1, f;
 152     CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r );
 153     RNG& rng = ts->get_rng();
 154
 155     r[0] = cvtest::randReal(rng)*CV_PI*2;
 156     r[1] = cvtest::randReal(rng)*CV_PI*2;
 157     r[2] = cvtest::randReal(rng)*CV_PI*2;
 158
 159     theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
 160     theta1 = fmod(theta0, CV_PI*2);
 161
 162     if( theta1 > CV_PI )
 163         theta1 = -(CV_PI*2 - theta1);
 164
 165     f = theta1/(theta0 ? theta0 : 1);
 166     r[0] *= f;
 167     r[1] *= f;
 168     r[2] *= f;
 169
 170     cvTsConvert( &_r, arr );
 171 }
 172
 173
 174 int CV_ProjectPointsTest::prepare_test_case( int test_case_idx )
 175 {
 176     int code = cvtest::ArrayTest::prepare_test_case( test_case_idx );
 177     return code;
 178 }
 179
 180
 181 void CV_ProjectPointsTest::run_func()
 182 {
 183     CvMat *v2m_jac = 0, *m2v_jac = 0;
 184     if( calc_jacobians )
 185     {
 186         v2m_jac = &test_mat[OUTPUT][1];
 187         m2v_jac = &test_mat[OUTPUT][3];
 188     }
 189
 190     cvProjectPoints2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac );
 191     cvProjectPoints2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac );
 192 }
 193
 194
 195 void CV_ProjectPointsTest::prepare_to_validation( int /*test_case_idx*/ )
 196 {
 197     const CvMat* vec = &test_mat[INPUT][0];
 198     CvMat* m = &test_mat[REF_OUTPUT][0];
 199     CvMat* vec2 = &test_mat[REF_OUTPUT][2];
 200     CvMat* v2m_jac = 0, *m2v_jac = 0;
 201     double theta0, theta1;
 202
 203     if( calc_jacobians )
 204     {
 205         v2m_jac = &test_mat[REF_OUTPUT][1];
 206         m2v_jac = &test_mat[REF_OUTPUT][3];
 207     }
 208
 209
 210     cvTsProjectPoints( vec, m, v2m_jac );
 211     cvTsProjectPoints( m, vec2, m2v_jac );
 212     cvTsCopy( vec, vec2 );
 213
 214     theta0 = cvNorm( vec2, 0, CV_L2 );
 215     theta1 = fmod( theta0, CV_PI*2 );
 216
 217     if( theta1 > CV_PI )
 218         theta1 = -(CV_PI*2 - theta1);
 219     cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) );
 220
 221     if( calc_jacobians )
 222     {
 223         //cvInvert( v2m_jac, m2v_jac, CV_SVD );
 224         if( cvNorm(&test_mat[OUTPUT][3],0,CV_C) < 1000 )
 225         {
 226             cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3],
 227                       1, 0, 0, &test_mat[OUTPUT][4],
 228                       v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T );
 229         }
 230         else
 231         {
 232             cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) );
 233             cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] );
 234         }
 235         cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) );
 236     }
 237 }
 238
 239
 240 CV_ProjectPointsTest ProjectPoints_test;
 241
 242 #endif
 243
 244 using namespace cv;
 245
 246 // --------------------------------- CV_CameraCalibrationTest --------------------------------------------
 247
 248 class CV_CameraCalibrationTest : public cvtest::BaseTest
 249 {
 250 public:
 251     CV_CameraCalibrationTest();
 252     ~CV_CameraCalibrationTest();
 253     void clear();
 254 protected:
 255     int compare(double* val, double* refVal, int len,
 256                 double eps, const char* paramName);
 257     virtual void calibrate( int imageCount, int* pointCounts,
 258         CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 259         double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 260         double* rotationMatrices, int flags ) = 0;
 261     virtual void project( int pointCount, CvPoint3D64f* objectPoints,
 262         double* rotationMatrix, double*  translationVector,
 263         double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints ) = 0;
 264
 265     void run(int);
 266 };
 267
 268 CV_CameraCalibrationTest::CV_CameraCalibrationTest()
 269 {
 270 }
 271
 272 CV_CameraCalibrationTest::~CV_CameraCalibrationTest()
 273 {
 274     clear();
 275 }
 276
 277 void CV_CameraCalibrationTest::clear()
 278 {
 279     cvtest::BaseTest::clear();
 280 }
 281
 282 int CV_CameraCalibrationTest::compare(double* val, double* ref_val, int len,
 283                                       double eps, const char* param_name )
 284 {
 285     return cvtest::cmpEps2_64f( ts, val, ref_val, len, eps, param_name );
 286 }
 287
 288 void CV_CameraCalibrationTest::run( int start_from )
 289 {
 290     int code = cvtest::TS::OK;
 291     char            filepath[200];
 292     char            filename[200];
 293
 294     CvSize          imageSize;
 295     CvSize          etalonSize;
 296     int             numImages;
 297
 298     CvPoint2D64f*   imagePoints;
 299     CvPoint3D64f*   objectPoints;
 300     CvPoint2D64f*   reprojectPoints;
 301
 302     double*       transVects;
 303     double*       rotMatrs;
 304
 305     double*       goodTransVects;
 306     double*       goodRotMatrs;
 307
 308     double          cameraMatrix[3*3];
 309     double          distortion[5]={0,0,0,0,0};
 310
 311     double          goodDistortion[4];
 312
 313     int*            numbers;
 314     FILE*           file = 0;
 315     FILE*           datafile = 0;
 316     int             i,j;
 317     int             currImage;
 318     int             currPoint;
 319
 320     int             calibFlags;
 321     char            i_dat_file[100];
 322     int             numPoints;
 323     int numTests;
 324     int currTest;
 325
 326     imagePoints     = 0;
 327     objectPoints    = 0;
 328     reprojectPoints = 0;
 329     numbers         = 0;
 330
 331     transVects      = 0;
 332     rotMatrs        = 0;
 333     goodTransVects  = 0;
 334     goodRotMatrs    = 0;
 335     int progress = 0;
 336     int values_read = -1;
 337
 338     sprintf( filepath, "%scameracalibration/", ts->get_data_path().c_str() );
 339     sprintf( filename, "%sdatafiles.txt", filepath );
 340     datafile = fopen( filename, "r" );
 341     if( datafile == 0 )
 342     {
 343         ts->printf( cvtest::TS::LOG, "Could not open file with list of test files: %s\n", filename );
 344         code = cvtest::TS::FAIL_MISSING_TEST_DATA;
 345         goto _exit_;
 346     }
 347
 348     values_read = fscanf(datafile,"%d",&numTests);
 349     CV_Assert(values_read == 1);
 350
 351     for( currTest = start_from; currTest < numTests; currTest++ )
 352     {
 353         values_read = fscanf(datafile,"%s",i_dat_file);
 354         CV_Assert(values_read == 1);
 355         sprintf(filename, "%s%s", filepath, i_dat_file);
 356         file = fopen(filename,"r");
 357
 358         ts->update_context( this, currTest, true );
 359
 360         if( file == 0 )
 361         {
 362             ts->printf( cvtest::TS::LOG,
 363                 "Can't open current test file: %s\n",filename);
 364             if( numTests == 1 )
 365             {
 366                 code = cvtest::TS::FAIL_MISSING_TEST_DATA;
 367                 goto _exit_;
 368             }
 369             continue; // if there is more than one test, just skip the test
 370         }
 371
 372         values_read = fscanf(file,"%d %d\n",&(imageSize.width),&(imageSize.height));
 373         CV_Assert(values_read == 2);
 374         if( imageSize.width <= 0 || imageSize.height <= 0 )
 375         {
 376             ts->printf( cvtest::TS::LOG, "Image size in test file is incorrect\n" );
 377             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 378             goto _exit_;
 379         }
 380
 381         /* Read etalon size */
 382         values_read = fscanf(file,"%d %d\n",&(etalonSize.width),&(etalonSize.height));
 383         CV_Assert(values_read == 2);
 384         if( etalonSize.width <= 0 || etalonSize.height <= 0 )
 385         {
 386             ts->printf( cvtest::TS::LOG, "Pattern size in test file is incorrect\n" );
 387             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 388             goto _exit_;
 389         }
 390
 391         numPoints = etalonSize.width * etalonSize.height;
 392
 393         /* Read number of images */
 394         values_read = fscanf(file,"%d\n",&numImages);
 395         CV_Assert(values_read == 1);
 396         if( numImages <=0 )
 397         {
 398             ts->printf( cvtest::TS::LOG, "Number of images in test file is incorrect\n");
 399             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 400             goto _exit_;
 401         }
 402
 403         /* Need to allocate memory */
 404         imagePoints     = (CvPoint2D64f*)cvAlloc( numPoints *
 405                                                     numImages * sizeof(CvPoint2D64f));
 406
 407         objectPoints    = (CvPoint3D64f*)cvAlloc( numPoints *
 408                                                     numImages * sizeof(CvPoint3D64f));
 409
 410         reprojectPoints = (CvPoint2D64f*)cvAlloc( numPoints *
 411                                                     numImages * sizeof(CvPoint2D64f));
 412
 413         /* Alloc memory for numbers */
 414         numbers = (int*)cvAlloc( numImages * sizeof(int));
 415
 416         /* Fill it by numbers of points of each image*/
 417         for( currImage = 0; currImage < numImages; currImage++ )
 418         {
 419             numbers[currImage] = etalonSize.width * etalonSize.height;
 420         }
 421
 422         /* Allocate memory for translate vectors and rotmatrixs*/
 423         transVects     = (double*)cvAlloc(3 * 1 * numImages * sizeof(double));
 424         rotMatrs       = (double*)cvAlloc(3 * 3 * numImages * sizeof(double));
 425
 426         goodTransVects = (double*)cvAlloc(3 * 1 * numImages * sizeof(double));
 427         goodRotMatrs   = (double*)cvAlloc(3 * 3 * numImages * sizeof(double));
 428
 429         /* Read object points */
 430         i = 0;/* shift for current point */
 431         for( currImage = 0; currImage < numImages; currImage++ )
 432         {
 433             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 434             {
 435                 double x,y,z;
 436                 values_read = fscanf(file,"%lf %lf %lf\n",&x,&y,&z);
 437                 CV_Assert(values_read == 3);
 438
 439                 (objectPoints+i)->x = x;
 440                 (objectPoints+i)->y = y;
 441                 (objectPoints+i)->z = z;
 442                 i++;
 443             }
 444         }
 445
 446         /* Read image points */
 447         i = 0;/* shift for current point */
 448         for( currImage = 0; currImage < numImages; currImage++ )
 449         {
 450             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 451             {
 452                 double x,y;
 453                 values_read = fscanf(file,"%lf %lf\n",&x,&y);
 454                 CV_Assert(values_read == 2);
 455
 456                 (imagePoints+i)->x = x;
 457                 (imagePoints+i)->y = y;
 458                 i++;
 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         values_read = fscanf(file,"%lf",goodDistortion+0); CV_Assert(values_read == 1);
 477         values_read = fscanf(file,"%lf",goodDistortion+1); CV_Assert(values_read == 1);
 478         values_read = fscanf(file,"%lf",goodDistortion+2); CV_Assert(values_read == 1);
 479         values_read = fscanf(file,"%lf",goodDistortion+3); CV_Assert(values_read == 1);
 480
 481         /* Read good Rot matrixes */
 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 * 9 + j * 3 + i);
 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 * 3 + i);
 498                 CV_Assert(values_read == 1);
 499             }
 500         }
 501
 502         calibFlags = 0
 503                      // + CV_CALIB_FIX_PRINCIPAL_POINT
 504                      // + CV_CALIB_ZERO_TANGENT_DIST
 505                      // + CV_CALIB_FIX_ASPECT_RATIO
 506                      // + CV_CALIB_USE_INTRINSIC_GUESS
 507                      + CV_CALIB_FIX_K3
 508                      + CV_CALIB_FIX_K4+CV_CALIB_FIX_K5
 509                      + CV_CALIB_FIX_K6
 510                     ;
 511         memset( cameraMatrix, 0, 9*sizeof(cameraMatrix[0]) );
 512         cameraMatrix[0] = cameraMatrix[4] = 807.;
 513         cameraMatrix[2] = (imageSize.width - 1)*0.5;
 514         cameraMatrix[5] = (imageSize.height - 1)*0.5;
 515         cameraMatrix[8] = 1.;
 516
 517         /* Now we can calibrate camera */
 518         calibrate(  numImages,
 519                     numbers,
 520                     imageSize,
 521                     imagePoints,
 522                     objectPoints,
 523                     distortion,
 524                     cameraMatrix,
 525                     transVects,
 526                     rotMatrs,
 527                     calibFlags );
 528
 529         /* ---- Reproject points to the image ---- */
 530         for( currImage = 0; currImage < numImages; currImage++ )
 531         {
 532             int nPoints = etalonSize.width * etalonSize.height;
 533             project(  nPoints,
 534                       objectPoints + currImage * nPoints,
 535                       rotMatrs + currImage * 9,
 536                       transVects + currImage * 3,
 537                       cameraMatrix,
 538                       distortion,
 539                       reprojectPoints + currImage * nPoints);
 540         }
 541
 542         /* ----- Compute reprojection error ----- */
 543         i = 0;
 544         double dx,dy;
 545         double rx,ry;
 546         double meanDx,meanDy;
 547         double maxDx = 0.0;
 548         double maxDy = 0.0;
 549
 550         meanDx = 0;
 551         meanDy = 0;
 552         for( currImage = 0; currImage < numImages; currImage++ )
 553         {
 554             for( currPoint = 0; currPoint < etalonSize.width * etalonSize.height; currPoint++ )
 555             {
 556                 rx = reprojectPoints[i].x;
 557                 ry = reprojectPoints[i].y;
 558                 dx = rx - imagePoints[i].x;
 559                 dy = ry - imagePoints[i].y;
 560
 561                 meanDx += dx;
 562                 meanDy += dy;
 563
 564                 dx = fabs(dx);
 565                 dy = fabs(dy);
 566
 567                 if( dx > maxDx )
 568                     maxDx = dx;
 569
 570                 if( dy > maxDy )
 571                     maxDy = dy;
 572                 i++;
 573             }
 574         }
 575
 576         meanDx /= numImages * etalonSize.width * etalonSize.height;
 577         meanDy /= numImages * etalonSize.width * etalonSize.height;
 578
 579         /* ========= Compare parameters ========= */
 580
 581         /* ----- Compare focal lengths ----- */
 582         code = compare(cameraMatrix+0,&goodFcx,1,0.1,"fx");
 583         if( code < 0 )
 584             goto _exit_;
 585
 586         code = compare(cameraMatrix+4,&goodFcy,1,0.1,"fy");
 587         if( code < 0 )
 588             goto _exit_;
 589
 590         /* ----- Compare principal points ----- */
 591         code = compare(cameraMatrix+2,&goodCx,1,0.1,"cx");
 592         if( code < 0 )
 593             goto _exit_;
 594
 595         code = compare(cameraMatrix+5,&goodCy,1,0.1,"cy");
 596         if( code < 0 )
 597             goto _exit_;
 598
 599         /* ----- Compare distortion ----- */
 600         code = compare(distortion,goodDistortion,4,0.1,"[k1,k2,p1,p2]");
 601         if( code < 0 )
 602             goto _exit_;
 603
 604         /* ----- Compare rot matrixs ----- */
 605         code = compare(rotMatrs,goodRotMatrs, 9*numImages,0.05,"rotation matrices");
 606         if( code < 0 )
 607             goto _exit_;
 608
 609         /* ----- Compare rot matrixs ----- */
 610         code = compare(transVects,goodTransVects, 3*numImages,0.1,"translation vectors");
 611         if( code < 0 )
 612             goto _exit_;
 613
 614         if( maxDx > 1.0 )
 615         {
 616             ts->printf( cvtest::TS::LOG,
 617                       "Error in reprojection maxDx=%f > 1.0\n",maxDx);
 618             code = cvtest::TS::FAIL_BAD_ACCURACY; goto _exit_;
 619         }
 620
 621         if( maxDy > 1.0 )
 622         {
 623             ts->printf( cvtest::TS::LOG,
 624                       "Error in reprojection maxDy=%f > 1.0\n",maxDy);
 625             code = cvtest::TS::FAIL_BAD_ACCURACY; goto _exit_;
 626         }
 627
 628         progress = update_progress( progress, currTest, numTests, 0 );
 629
 630         cvFree(&imagePoints);
 631         cvFree(&objectPoints);
 632         cvFree(&reprojectPoints);
 633         cvFree(&numbers);
 634
 635         cvFree(&transVects);
 636         cvFree(&rotMatrs);
 637         cvFree(&goodTransVects);
 638         cvFree(&goodRotMatrs);
 639
 640         fclose(file);
 641         file = 0;
 642     }
 643
 644 _exit_:
 645
 646     if( file )
 647         fclose(file);
 648
 649     if( datafile )
 650         fclose(datafile);
 651
 652     /* Free all allocated memory */
 653     cvFree(&imagePoints);
 654     cvFree(&objectPoints);
 655     cvFree(&reprojectPoints);
 656     cvFree(&numbers);
 657
 658     cvFree(&transVects);
 659     cvFree(&rotMatrs);
 660     cvFree(&goodTransVects);
 661     cvFree(&goodRotMatrs);
 662
 663     if( code < 0 )
 664         ts->set_failed_test_info( code );
 665 }
 666
 667 // --------------------------------- CV_CameraCalibrationTest_C --------------------------------------------
 668
 669 class CV_CameraCalibrationTest_C : public CV_CameraCalibrationTest
 670 {
 671 public:
 672     CV_CameraCalibrationTest_C(){}
 673 protected:
 674     virtual void calibrate( int imageCount, int* pointCounts,
 675         CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 676         double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 677         double* rotationMatrices, int flags );
 678     virtual void project( int pointCount, CvPoint3D64f* objectPoints,
 679         double* rotationMatrix, double*  translationVector,
 680         double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints );
 681 };
 682
 683 void CV_CameraCalibrationTest_C::calibrate( int imageCount, int* pointCounts,
 684         CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 685         double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 686         double* rotationMatrices, int flags )
 687 {
 688     int i, total = 0;
 689     for( i = 0; i < imageCount; i++ )
 690         total += pointCounts[i];
 691
 692     CvMat _objectPoints = cvMat(1, total, CV_64FC3, objectPoints);
 693     CvMat _imagePoints = cvMat(1, total, CV_64FC2, imagePoints);
 694     CvMat _pointCounts = cvMat(1, imageCount, CV_32S, pointCounts);
 695     CvMat _cameraMatrix = cvMat(3, 3, CV_64F, cameraMatrix);
 696     CvMat _distCoeffs = cvMat(4, 1, CV_64F, distortionCoeffs);
 697     CvMat _rotationMatrices = cvMat(imageCount, 9, CV_64F, rotationMatrices);
 698     CvMat _translationVectors = cvMat(imageCount, 3, CV_64F, translationVectors);
 699
 700     cvCalibrateCamera2(&_objectPoints, &_imagePoints, &_pointCounts, imageSize,
 701                        &_cameraMatrix, &_distCoeffs, &_rotationMatrices, &_translationVectors,
 702                        flags);
 703 }
 704
 705 void CV_CameraCalibrationTest_C::project( int pointCount, CvPoint3D64f* objectPoints,
 706         double* rotationMatrix, double*  translationVector,
 707         double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints )
 708 {
 709     CvMat _objectPoints = cvMat(1, pointCount, CV_64FC3, objectPoints);
 710     CvMat _imagePoints = cvMat(1, pointCount, CV_64FC2, imagePoints);
 711     CvMat _cameraMatrix = cvMat(3, 3, CV_64F, cameraMatrix);
 712     CvMat _distCoeffs = cvMat(4, 1, CV_64F, distortion);
 713     CvMat _rotationMatrix = cvMat(3, 3, CV_64F, rotationMatrix);
 714     CvMat _translationVector = cvMat(1, 3, CV_64F, translationVector);
 715
 716     cvProjectPoints2(&_objectPoints, &_rotationMatrix, &_translationVector, &_cameraMatrix, &_distCoeffs, &_imagePoints);
 717 }
 718
 719 // --------------------------------- CV_CameraCalibrationTest_CPP --------------------------------------------
 720
 721 class CV_CameraCalibrationTest_CPP : public CV_CameraCalibrationTest
 722 {
 723 public:
 724     CV_CameraCalibrationTest_CPP(){}
 725 protected:
 726     virtual void calibrate( int imageCount, int* pointCounts,
 727         CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 728         double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 729         double* rotationMatrices, int flags );
 730     virtual void project( int pointCount, CvPoint3D64f* objectPoints,
 731         double* rotationMatrix, double*  translationVector,
 732         double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints );
 733 };
 734
 735 void CV_CameraCalibrationTest_CPP::calibrate( int imageCount, int* pointCounts,
 736         CvSize _imageSize, CvPoint2D64f* _imagePoints, CvPoint3D64f* _objectPoints,
 737         double* _distortionCoeffs, double* _cameraMatrix, double* translationVectors,
 738         double* rotationMatrices, int flags )
 739 {
 740     vector<vector<Point3f> > objectPoints( imageCount );
 741     vector<vector<Point2f> > imagePoints( imageCount );
 742     Size imageSize = _imageSize;
 743     Mat cameraMatrix, distCoeffs(1,4,CV_64F,Scalar::all(0));
 744     vector<Mat> rvecs, tvecs;
 745
 746     CvPoint3D64f* op = _objectPoints;
 747     CvPoint2D64f* ip = _imagePoints;
 748     vector<vector<Point3f> >::iterator objectPointsIt = objectPoints.begin();
 749     vector<vector<Point2f> >::iterator imagePointsIt = imagePoints.begin();
 750     for( int i = 0; i < imageCount; ++objectPointsIt, ++imagePointsIt, i++ )
 751     {
 752         int num = pointCounts[i];
 753         objectPointsIt->resize( num );
 754         imagePointsIt->resize( num );
 755         vector<Point3f>::iterator oIt = objectPointsIt->begin();
 756         vector<Point2f>::iterator iIt = imagePointsIt->begin();
 757         for( int j = 0; j < num; ++oIt, ++iIt, j++, op++, ip++)
 758         {
 759             oIt->x = (float)op->x, oIt->y = (float)op->y, oIt->z = (float)op->z;
 760             iIt->x = (float)ip->x, iIt->y = (float)ip->y;
 761         }
 762     }
 763
 764     calibrateCamera( objectPoints,
 765                      imagePoints,
 766                      imageSize,
 767                      cameraMatrix,
 768                      distCoeffs,
 769                      rvecs,
 770                      tvecs,
 771                      flags );
 772
 773     assert( cameraMatrix.type() == CV_64FC1 );
 774     memcpy( _cameraMatrix, cameraMatrix.data, 9*sizeof(double) );
 775
 776     assert( cameraMatrix.type() == CV_64FC1 );
 777     memcpy( _distortionCoeffs, distCoeffs.data, 4*sizeof(double) );
 778
 779     vector<Mat>::iterator rvecsIt = rvecs.begin();
 780     vector<Mat>::iterator tvecsIt = tvecs.begin();
 781     double *rm = rotationMatrices,
 782            *tm = translationVectors;
 783     assert( rvecsIt->type() == CV_64FC1 );
 784     assert( tvecsIt->type() == CV_64FC1 );
 785     for( int i = 0; i < imageCount; ++rvecsIt, ++tvecsIt, i++, rm+=9, tm+=3 )
 786     {
 787         Mat r9( 3, 3, CV_64FC1 );
 788         Rodrigues( *rvecsIt, r9 );
 789         memcpy( rm, r9.data, 9*sizeof(double) );
 790         memcpy( tm, tvecsIt->data, 3*sizeof(double) );
 791     }
 792 }
 793
 794 void CV_CameraCalibrationTest_CPP::project( int pointCount, CvPoint3D64f* _objectPoints,
 795         double* rotationMatrix, double*  translationVector,
 796         double* _cameraMatrix, double* distortion, CvPoint2D64f* _imagePoints )
 797 {
 798     Mat objectPoints( pointCount, 3, CV_64FC1, _objectPoints );
 799     Mat rmat( 3, 3, CV_64FC1, rotationMatrix ),
 800         rvec( 1, 3, CV_64FC1 ),
 801         tvec( 1, 3, CV_64FC1, translationVector );
 802     Mat cameraMatrix( 3, 3, CV_64FC1, _cameraMatrix );
 803     Mat distCoeffs( 1, 4, CV_64FC1, distortion );
 804     vector<Point2f> imagePoints;
 805     Rodrigues( rmat, rvec );
 806
 807     objectPoints.convertTo( objectPoints, CV_32FC1 );
 808     projectPoints( objectPoints, rvec, tvec,
 809                    cameraMatrix, distCoeffs, imagePoints );
 810     vector<Point2f>::const_iterator it = imagePoints.begin();
 811     for( int i = 0; it != imagePoints.end(); ++it, i++ )
 812     {
 813         _imagePoints[i] = cvPoint2D64f( it->x, it->y );
 814     }
 815 }
 816
 817
 818 //----------------------------------------- CV_CalibrationMatrixValuesTest --------------------------------
 819
 820 class CV_CalibrationMatrixValuesTest : public cvtest::BaseTest
 821 {
 822 public:
 823     CV_CalibrationMatrixValuesTest() {}
 824 protected:
 825     void run(int);
 826     virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 827         double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 828         Point2d& principalPoint, double& aspectRatio ) = 0;
 829 };
 830
 831 void CV_CalibrationMatrixValuesTest::run(int)
 832 {
 833     int code = cvtest::TS::OK;
 834     const double fcMinVal = 1e-5;
 835     const double fcMaxVal = 1000;
 836     const double apertureMaxVal = 0.01;
 837
 838     RNG rng = ts->get_rng();
 839
 840     double fx, fy, cx, cy, nx, ny;
 841     Mat cameraMatrix( 3, 3, CV_64FC1 );
 842     cameraMatrix.setTo( Scalar(0) );
 843     fx = cameraMatrix.at<double>(0,0) = rng.uniform( fcMinVal, fcMaxVal );
 844     fy = cameraMatrix.at<double>(1,1) = rng.uniform( fcMinVal, fcMaxVal );
 845     cx = cameraMatrix.at<double>(0,2) = rng.uniform( fcMinVal, fcMaxVal );
 846     cy = cameraMatrix.at<double>(1,2) = rng.uniform( fcMinVal, fcMaxVal );
 847     cameraMatrix.at<double>(2,2) = 1;
 848
 849     Size imageSize( 600, 400 );
 850
 851     double apertureWidth = (double)rng * apertureMaxVal,
 852            apertureHeight = (double)rng * apertureMaxVal;
 853
 854     double fovx, fovy, focalLength, aspectRatio,
 855            goodFovx, goodFovy, goodFocalLength, goodAspectRatio;
 856     Point2d principalPoint, goodPrincipalPoint;
 857
 858
 859     calibMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
 860         fovx, fovy, focalLength, principalPoint, aspectRatio );
 861
 862     // calculate calibration matrix values
 863     goodAspectRatio = fy / fx;
 864
 865     if( apertureWidth != 0.0 && apertureHeight != 0.0 )
 866     {
 867         nx = imageSize.width / apertureWidth;
 868         ny = imageSize.height / apertureHeight;
 869     }
 870     else
 871     {
 872         nx = 1.0;
 873         ny = goodAspectRatio;
 874     }
 875
 876     goodFovx = 2 * atan( imageSize.width / (2 * fx)) * 180.0 / CV_PI;
 877     goodFovy = 2 * atan( imageSize.height / (2 * fy)) * 180.0 / CV_PI;
 878
 879     goodFocalLength = fx / nx;
 880
 881     goodPrincipalPoint.x = cx / nx;
 882     goodPrincipalPoint.y = cy / ny;
 883
 884     // check results
 885     if( fabs(fovx - goodFovx) > FLT_EPSILON )
 886     {
 887         ts->printf( cvtest::TS::LOG, "bad fovx (real=%f, good = %f\n", fovx, goodFovx );
 888         code = cvtest::TS::FAIL_BAD_ACCURACY;
 889         goto _exit_;
 890     }
 891     if( fabs(fovy - goodFovy) > FLT_EPSILON )
 892     {
 893         ts->printf( cvtest::TS::LOG, "bad fovy (real=%f, good = %f\n", fovy, goodFovy );
 894         code = cvtest::TS::FAIL_BAD_ACCURACY;
 895         goto _exit_;
 896     }
 897     if( fabs(focalLength - goodFocalLength) > FLT_EPSILON )
 898     {
 899         ts->printf( cvtest::TS::LOG, "bad focalLength (real=%f, good = %f\n", focalLength, goodFocalLength );
 900         code = cvtest::TS::FAIL_BAD_ACCURACY;
 901         goto _exit_;
 902     }
 903     if( fabs(aspectRatio - goodAspectRatio) > FLT_EPSILON )
 904     {
 905         ts->printf( cvtest::TS::LOG, "bad aspectRatio (real=%f, good = %f\n", aspectRatio, goodAspectRatio );
 906         code = cvtest::TS::FAIL_BAD_ACCURACY;
 907         goto _exit_;
 908     }
 909     if( norm( principalPoint - goodPrincipalPoint ) > FLT_EPSILON )
 910     {
 911         ts->printf( cvtest::TS::LOG, "bad principalPoint\n" );
 912         code = cvtest::TS::FAIL_BAD_ACCURACY;
 913         goto _exit_;
 914     }
 915
 916 _exit_:
 917     RNG& _rng = ts->get_rng();
 918     _rng = rng;
 919     ts->set_failed_test_info( code );
 920 }
 921
 922 //----------------------------------------- CV_CalibrationMatrixValuesTest_C --------------------------------
 923
 924 class CV_CalibrationMatrixValuesTest_C : public CV_CalibrationMatrixValuesTest
 925 {
 926 public:
 927     CV_CalibrationMatrixValuesTest_C(){}
 928 protected:
 929     virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 930         double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 931         Point2d& principalPoint, double& aspectRatio );
 932 };
 933
 934 void CV_CalibrationMatrixValuesTest_C::calibMatrixValues( const Mat& _cameraMatrix, Size imageSize,
 935                                                double apertureWidth, double apertureHeight,
 936                                                double& fovx, double& fovy, double& focalLength,
 937                                                Point2d& principalPoint, double& aspectRatio )
 938 {
 939     CvMat cameraMatrix = _cameraMatrix;
 940     CvPoint2D64f pp;
 941     cvCalibrationMatrixValues( &cameraMatrix, imageSize, apertureWidth, apertureHeight,
 942         &fovx, &fovy, &focalLength, &pp, &aspectRatio );
 943     principalPoint.x = pp.x;
 944     principalPoint.y = pp.y;
 945 }
 946
 947
 948 //----------------------------------------- CV_CalibrationMatrixValuesTest_CPP --------------------------------
 949
 950 class CV_CalibrationMatrixValuesTest_CPP : public CV_CalibrationMatrixValuesTest
 951 {
 952 public:
 953     CV_CalibrationMatrixValuesTest_CPP() {}
 954 protected:
 955     virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 956         double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 957         Point2d& principalPoint, double& aspectRatio );
 958 };
 959
 960 void CV_CalibrationMatrixValuesTest_CPP::calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 961                                                          double apertureWidth, double apertureHeight,
 962                                                          double& fovx, double& fovy, double& focalLength,
 963                                                          Point2d& principalPoint, double& aspectRatio )
 964 {
 965     calibrationMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
 966         fovx, fovy, focalLength, principalPoint, aspectRatio );
 967 }
 968
 969
 970 //----------------------------------------- CV_ProjectPointsTest --------------------------------
 971 void calcdfdx( const vector<vector<Point2f> >& leftF, const vector<vector<Point2f> >& rightF, double eps, Mat& dfdx )
 972 {
 973     const int fdim = 2;
 974     CV_Assert( !leftF.empty() && !rightF.empty() && !leftF[0].empty() && !rightF[0].empty() );
 975     CV_Assert( leftF[0].size() ==  rightF[0].size() );
 976     CV_Assert( fabs(eps) > std::numeric_limits<double>::epsilon() );
 977     int fcount = (int)leftF[0].size(), xdim = (int)leftF.size();
 978
 979     dfdx.create( fcount*fdim, xdim, CV_64FC1 );
 980
 981     vector<vector<Point2f> >::const_iterator arrLeftIt = leftF.begin();
 982     vector<vector<Point2f> >::const_iterator arrRightIt = rightF.begin();
 983     for( int xi = 0; xi < xdim; xi++, ++arrLeftIt, ++arrRightIt )
 984     {
 985         CV_Assert( (int)arrLeftIt->size() ==  fcount );
 986         CV_Assert( (int)arrRightIt->size() ==  fcount );
 987         vector<Point2f>::const_iterator lIt = arrLeftIt->begin();
 988         vector<Point2f>::const_iterator rIt = arrRightIt->begin();
 989         for( int fi = 0; fi < dfdx.rows; fi+=fdim, ++lIt, ++rIt )
 990         {
 991             dfdx.at<double>(fi, xi )   = 0.5 * ((double)(rIt->x - lIt->x)) / eps;
 992             dfdx.at<double>(fi+1, xi ) = 0.5 * ((double)(rIt->y - lIt->y)) / eps;
 993         }
 994     }
 995 }
 996
 997 class CV_ProjectPointsTest : public cvtest::BaseTest
 998 {
 999 public:
1000     CV_ProjectPointsTest() {}
1001 protected:
1002     void run(int);
1003     virtual void project( const Mat& objectPoints,
1004         const Mat& rvec, const Mat& tvec,
1005         const Mat& cameraMatrix,
1006         const Mat& distCoeffs,
1007         vector<Point2f>& imagePoints,
1008         Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1009         Mat& dpdc, Mat& dpddist,
1010         double aspectRatio=0 ) = 0;
1011 };
1012
1013 void CV_ProjectPointsTest::run(int)
1014 {
1015     //typedef float matType;
1016
1017     int code = cvtest::TS::OK;
1018     const int pointCount = 100;
1019
1020     const float zMinVal = 10.0f, zMaxVal = 100.0f,
1021                 rMinVal = -0.3f, rMaxVal = 0.3f,
1022                 tMinVal = -2.0f, tMaxVal = 2.0f;
1023
1024     const float imgPointErr = 1e-3f,
1025                 dEps = 1e-3f;
1026
1027     double err;
1028
1029     Size imgSize( 600, 800 );
1030     Mat_<float> objPoints( pointCount, 3), rvec( 1, 3), rmat, tvec( 1, 3 ), cameraMatrix( 3, 3 ), distCoeffs( 1, 4 ),
1031       leftRvec, rightRvec, leftTvec, rightTvec, leftCameraMatrix, rightCameraMatrix, leftDistCoeffs, rightDistCoeffs;
1032
1033     RNG rng = ts->get_rng();
1034
1035     // generate data
1036     cameraMatrix << 300.f,  0.f,    imgSize.width/2.f,
1037                     0.f,    300.f,  imgSize.height/2.f,
1038                     0.f,    0.f,    1.f;
1039     distCoeffs << 0.1, 0.01, 0.001, 0.001;
1040
1041     rvec(0,0) = rng.uniform( rMinVal, rMaxVal );
1042     rvec(0,1) = rng.uniform( rMinVal, rMaxVal );
1043     rvec(0,2) = rng.uniform( rMinVal, rMaxVal );
1044     Rodrigues( rvec, rmat );
1045
1046     tvec(0,0) = rng.uniform( tMinVal, tMaxVal );
1047     tvec(0,1) = rng.uniform( tMinVal, tMaxVal );
1048     tvec(0,2) = rng.uniform( tMinVal, tMaxVal );
1049
1050     for( int y = 0; y < objPoints.rows; y++ )
1051     {
1052         Mat point(1, 3, CV_32FC1, objPoints.ptr(y) );
1053         float z = rng.uniform( zMinVal, zMaxVal );
1054         point.at<float>(0,2) = z;
1055         point.at<float>(0,0) = (rng.uniform(2.f,(float)(imgSize.width-2)) - cameraMatrix(0,2)) / cameraMatrix(0,0) * z;
1056         point.at<float>(0,1) = (rng.uniform(2.f,(float)(imgSize.height-2)) - cameraMatrix(1,2)) / cameraMatrix(1,1) * z;
1057         point = (point - tvec) * rmat;
1058     }
1059
1060     vector<Point2f> imgPoints;
1061     vector<vector<Point2f> > leftImgPoints;
1062     vector<vector<Point2f> > rightImgPoints;
1063     Mat dpdrot, dpdt, dpdf, dpdc, dpddist,
1064         valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist;
1065
1066     project( objPoints, rvec, tvec, cameraMatrix, distCoeffs,
1067         imgPoints, dpdrot, dpdt, dpdf, dpdc, dpddist, 0 );
1068
1069     // calculate and check image points
1070     assert( (int)imgPoints.size() == pointCount );
1071     vector<Point2f>::const_iterator it = imgPoints.begin();
1072     for( int i = 0; i < pointCount; i++, ++it )
1073     {
1074         Point3d p( objPoints(i,0), objPoints(i,1), objPoints(i,2) );
1075         double z = p.x*rmat(2,0) + p.y*rmat(2,1) + p.z*rmat(2,2) + tvec(0,2),
1076                x = (p.x*rmat(0,0) + p.y*rmat(0,1) + p.z*rmat(0,2) + tvec(0,0)) / z,
1077                y = (p.x*rmat(1,0) + p.y*rmat(1,1) + p.z*rmat(1,2) + tvec(0,1)) / z,
1078                r2 = x*x + y*y,
1079                r4 = r2*r2;
1080         Point2f validImgPoint;
1081         double a1 = 2*x*y,
1082                a2 = r2 + 2*x*x,
1083                a3 = r2 + 2*y*y,
1084                cdist = 1+distCoeffs(0,0)*r2+distCoeffs(0,1)*r4;
1085         validImgPoint.x = static_cast<float>((double)cameraMatrix(0,0)*(x*cdist + (double)distCoeffs(0,2)*a1 + (double)distCoeffs(0,3)*a2)
1086             + (double)cameraMatrix(0,2));
1087         validImgPoint.y = static_cast<float>((double)cameraMatrix(1,1)*(y*cdist + (double)distCoeffs(0,2)*a3 + distCoeffs(0,3)*a1)
1088             + (double)cameraMatrix(1,2));
1089
1090         if( fabs(it->x - validImgPoint.x) > imgPointErr ||
1091             fabs(it->y - validImgPoint.y) > imgPointErr )
1092         {
1093             ts->printf( cvtest::TS::LOG, "bad image point\n" );
1094             code = cvtest::TS::FAIL_BAD_ACCURACY;
1095             goto _exit_;
1096         }
1097     }
1098
1099     // check derivatives
1100     // 1. rotation
1101     leftImgPoints.resize(3);
1102     rightImgPoints.resize(3);
1103     for( int i = 0; i < 3; i++ )
1104     {
1105         rvec.copyTo( leftRvec ); leftRvec(0,i) -= dEps;
1106         project( objPoints, leftRvec, tvec, cameraMatrix, distCoeffs,
1107             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1108         rvec.copyTo( rightRvec ); rightRvec(0,i) += dEps;
1109         project( objPoints, rightRvec, tvec, cameraMatrix, distCoeffs,
1110             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1111     }
1112     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdrot );
1113     err = norm( dpdrot, valDpdrot, NORM_INF );
1114     if( err > 3 )
1115     {
1116         ts->printf( cvtest::TS::LOG, "bad dpdrot: too big difference = %g\n", err );
1117         code = cvtest::TS::FAIL_BAD_ACCURACY;
1118     }
1119
1120     // 2. translation
1121     for( int i = 0; i < 3; i++ )
1122     {
1123         tvec.copyTo( leftTvec ); leftTvec(0,i) -= dEps;
1124         project( objPoints, rvec, leftTvec, cameraMatrix, distCoeffs,
1125             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1126         tvec.copyTo( rightTvec ); rightTvec(0,i) += dEps;
1127         project( objPoints, rvec, rightTvec, cameraMatrix, distCoeffs,
1128             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1129     }
1130     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdt );
1131     if( norm( dpdt, valDpdt, NORM_INF ) > 0.2 )
1132     {
1133         ts->printf( cvtest::TS::LOG, "bad dpdtvec\n" );
1134         code = cvtest::TS::FAIL_BAD_ACCURACY;
1135     }
1136
1137     // 3. camera matrix
1138     // 3.1. focus
1139     leftImgPoints.resize(2);
1140     rightImgPoints.resize(2);
1141     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,0) -= dEps;
1142     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1143         leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1144     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,1) -= dEps;
1145     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1146         leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1147     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,0) += dEps;
1148     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1149         rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1150     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,1) += dEps;
1151     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1152         rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1153     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdf );
1154     if ( norm( dpdf, valDpdf ) > 0.2 )
1155     {
1156         ts->printf( cvtest::TS::LOG, "bad dpdf\n" );
1157         code = cvtest::TS::FAIL_BAD_ACCURACY;
1158     }
1159     // 3.2. principal point
1160     leftImgPoints.resize(2);
1161     rightImgPoints.resize(2);
1162     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,2) -= dEps;
1163     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1164         leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1165     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,2) -= dEps;
1166     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1167         leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1168     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,2) += dEps;
1169     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1170         rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1171     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,2) += dEps;
1172     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1173         rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1174     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdc );
1175     if ( norm( dpdc, valDpdc ) > 0.2 )
1176     {
1177         ts->printf( cvtest::TS::LOG, "bad dpdc\n" );
1178         code = cvtest::TS::FAIL_BAD_ACCURACY;
1179     }
1180
1181     // 4. distortion
1182     leftImgPoints.resize(distCoeffs.cols);
1183     rightImgPoints.resize(distCoeffs.cols);
1184     for( int i = 0; i < distCoeffs.cols; i++ )
1185     {
1186         distCoeffs.copyTo( leftDistCoeffs ); leftDistCoeffs(0,i) -= dEps;
1187         project( objPoints, rvec, tvec, cameraMatrix, leftDistCoeffs,
1188             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1189         distCoeffs.copyTo( rightDistCoeffs ); rightDistCoeffs(0,i) += dEps;
1190         project( objPoints, rvec, tvec, cameraMatrix, rightDistCoeffs,
1191             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1192     }
1193     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpddist );
1194     if( norm( dpddist, valDpddist ) > 0.3 )
1195     {
1196         ts->printf( cvtest::TS::LOG, "bad dpddist\n" );
1197         code = cvtest::TS::FAIL_BAD_ACCURACY;
1198     }
1199
1200 _exit_:
1201     RNG& _rng = ts->get_rng();
1202     _rng = rng;
1203     ts->set_failed_test_info( code );
1204 }
1205
1206 //----------------------------------------- CV_ProjectPointsTest_C --------------------------------
1207 class CV_ProjectPointsTest_C : public CV_ProjectPointsTest
1208 {
1209 public:
1210     CV_ProjectPointsTest_C() {}
1211 protected:
1212     virtual void project( const Mat& objectPoints,
1213         const Mat& rvec, const Mat& tvec,
1214         const Mat& cameraMatrix,
1215         const Mat& distCoeffs,
1216         vector<Point2f>& imagePoints,
1217         Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1218         Mat& dpdc, Mat& dpddist,
1219         double aspectRatio=0 );
1220 };
1221
1222 void CV_ProjectPointsTest_C::project( const Mat& opoints, const Mat& rvec, const Mat& tvec,
1223                                        const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& ipoints,
1224                                        Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio)
1225 {
1226     int npoints = opoints.cols*opoints.rows*opoints.channels()/3;
1227     ipoints.resize(npoints);
1228     dpdrot.create(npoints*2, 3, CV_64F);
1229     dpdt.create(npoints*2, 3, CV_64F);
1230     dpdf.create(npoints*2, 2, CV_64F);
1231     dpdc.create(npoints*2, 2, CV_64F);
1232     dpddist.create(npoints*2, distCoeffs.rows + distCoeffs.cols - 1, CV_64F);
1233     CvMat _objectPoints = opoints, _imagePoints = Mat(ipoints);
1234     CvMat _rvec = rvec, _tvec = tvec, _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
1235     CvMat _dpdrot = dpdrot, _dpdt = dpdt, _dpdf = dpdf, _dpdc = dpdc, _dpddist = dpddist;
1236
1237     cvProjectPoints2( &_objectPoints, &_rvec, &_tvec, &_cameraMatrix, &_distCoeffs,
1238                       &_imagePoints, &_dpdrot, &_dpdt, &_dpdf, &_dpdc, &_dpddist, aspectRatio );
1239 }
1240
1241
1242 //----------------------------------------- CV_ProjectPointsTest_CPP --------------------------------
1243 class CV_ProjectPointsTest_CPP : public CV_ProjectPointsTest
1244 {
1245 public:
1246     CV_ProjectPointsTest_CPP() {}
1247 protected:
1248     virtual void project( const Mat& objectPoints,
1249         const Mat& rvec, const Mat& tvec,
1250         const Mat& cameraMatrix,
1251         const Mat& distCoeffs,
1252         vector<Point2f>& imagePoints,
1253         Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1254         Mat& dpdc, Mat& dpddist,
1255         double aspectRatio=0 );
1256 };
1257
1258 void CV_ProjectPointsTest_CPP::project( const Mat& objectPoints, const Mat& rvec, const Mat& tvec,
1259                                        const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& imagePoints,
1260                                        Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio)
1261 {
1262     Mat J;
1263     projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, J, aspectRatio);
1264     J.colRange(0, 3).copyTo(dpdrot);
1265     J.colRange(3, 6).copyTo(dpdt);
1266     J.colRange(6, 8).copyTo(dpdf);
1267     J.colRange(8, 10).copyTo(dpdc);
1268     J.colRange(10, J.cols).copyTo(dpddist);
1269 }
1270
1271 ///////////////////////////////// Stereo Calibration /////////////////////////////////////
1272
1273 class CV_StereoCalibrationTest : public cvtest::BaseTest
1274 {
1275 public:
1276     CV_StereoCalibrationTest();
1277     ~CV_StereoCalibrationTest();
1278     void clear();
1279 protected:
1280     bool checkPandROI( int test_case_idx,
1281         const Mat& M, const Mat& D, const Mat& R,
1282         const Mat& P, Size imgsize, Rect roi );
1283
1284     // covers of tested functions
1285     virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1286         const vector<vector<Point2f> >& imagePoints1,
1287         const vector<vector<Point2f> >& imagePoints2,
1288         Mat& cameraMatrix1, Mat& distCoeffs1,
1289         Mat& cameraMatrix2, Mat& distCoeffs2,
1290         Size imageSize, Mat& R, Mat& T,
1291         Mat& E, Mat& F, TermCriteria criteria, int flags ) = 0;
1292     virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1293         const Mat& cameraMatrix2, const Mat& distCoeffs2,
1294         Size imageSize, const Mat& R, const Mat& T,
1295         Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1296         double alpha, Size newImageSize,
1297         Rect* validPixROI1, Rect* validPixROI2, int flags ) = 0;
1298     virtual bool rectifyUncalibrated( const Mat& points1,
1299         const Mat& points2, const Mat& F, Size imgSize,
1300         Mat& H1, Mat& H2, double threshold=5 ) = 0;
1301     virtual void triangulate( const Mat& P1, const Mat& P2,
1302         const Mat &points1, const Mat &points2,
1303         Mat &points4D ) = 0;
1304     virtual void correct( const Mat& F,
1305         const Mat &points1, const Mat &points2,
1306         Mat &newPoints1, Mat &newPoints2 ) = 0;
1307
1308     void run(int);
1309 };
1310
1311
1312 CV_StereoCalibrationTest::CV_StereoCalibrationTest()
1313 {
1314 }
1315
1316
1317 CV_StereoCalibrationTest::~CV_StereoCalibrationTest()
1318 {
1319     clear();
1320 }
1321
1322 void CV_StereoCalibrationTest::clear()
1323 {
1324     cvtest::BaseTest::clear();
1325 }
1326
1327 bool CV_StereoCalibrationTest::checkPandROI( int test_case_idx, const Mat& M, const Mat& D, const Mat& R,
1328                                             const Mat& P, Size imgsize, Rect roi )
1329 {
1330     const double eps = 0.05;
1331     const int N = 21;
1332     int x, y, k;
1333     vector<Point2f> pts, upts;
1334
1335     // step 1. check that all the original points belong to the destination image
1336     for( y = 0; y < N; y++ )
1337         for( x = 0; x < N; x++ )
1338             pts.push_back(Point2f((float)x*imgsize.width/(N-1), (float)y*imgsize.height/(N-1)));
1339
1340     undistortPoints(Mat(pts), upts, M, D, R, P );
1341     for( k = 0; k < N*N; k++ )
1342         if( upts[k].x < -imgsize.width*eps || upts[k].x > imgsize.width*(1+eps) ||
1343             upts[k].y < -imgsize.height*eps || upts[k].y > imgsize.height*(1+eps) )
1344         {
1345             ts->printf(cvtest::TS::LOG, "Test #%d. The point (%g, %g) was mapped to (%g, %g) which is out of image\n",
1346                 test_case_idx, pts[k].x, pts[k].y, upts[k].x, upts[k].y);
1347             return false;
1348         }
1349
1350         // step 2. check that all the points inside ROI belong to the original source image
1351         Mat temp(imgsize, CV_8U), utemp, map1, map2;
1352         temp = Scalar::all(1);
1353         initUndistortRectifyMap(M, D, R, P, imgsize, CV_16SC2, map1, map2);
1354         remap(temp, utemp, map1, map2, INTER_LINEAR);
1355
1356         if(roi.x < 0 || roi.y < 0 || roi.x + roi.width > imgsize.width || roi.y + roi.height > imgsize.height)
1357         {
1358             ts->printf(cvtest::TS::LOG, "Test #%d. The ROI=(%d, %d, %d, %d) is outside of the imge rectangle\n",
1359                 test_case_idx, roi.x, roi.y, roi.width, roi.height);
1360             return false;
1361         }
1362         double s = sum(utemp(roi))[0];
1363         if( s > roi.area() || roi.area() - s > roi.area()*(1-eps) )
1364         {
1365             ts->printf(cvtest::TS::LOG, "Test #%d. The ratio of black pixels inside the valid ROI (~%g%%) is too large\n",
1366                 test_case_idx, s*100./roi.area());
1367             return false;
1368         }
1369
1370         return true;
1371 }
1372
1373 void CV_StereoCalibrationTest::run( int )
1374 {
1375     const int ntests = 1;
1376     const double maxReprojErr = 2;
1377     const double maxScanlineDistErr_c = 3;
1378     const double maxScanlineDistErr_uc = 4;
1379     FILE* f = 0;
1380
1381     for(int testcase = 1; testcase <= ntests; testcase++)
1382     {
1383         char filepath[1000];
1384         char buf[1000];
1385         sprintf( filepath, "%sstereo/case%d/stereo_calib.txt", ts->get_data_path().c_str(), testcase );
1386         f = fopen(filepath, "rt");
1387         Size patternSize;
1388         vector<string> imglist;
1389
1390         if( !f || !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &patternSize.width, &patternSize.height) != 2 )
1391         {
1392             ts->printf( cvtest::TS::LOG, "The file %s can not be opened or has invalid content\n", filepath );
1393             ts->set_failed_test_info( f ? cvtest::TS::FAIL_INVALID_TEST_DATA : cvtest::TS::FAIL_MISSING_TEST_DATA );
1394             return;
1395         }
1396
1397         for(;;)
1398         {
1399             if( !fgets( buf, sizeof(buf)-3, f ))
1400                 break;
1401             size_t len = strlen(buf);
1402             while( len > 0 && isspace(buf[len-1]))
1403                 buf[--len] = '\0';
1404             if( buf[0] == '#')
1405                 continue;
1406             sprintf(filepath, "%sstereo/case%d/%s", ts->get_data_path().c_str(), testcase, buf );
1407             imglist.push_back(string(filepath));
1408         }
1409         fclose(f);
1410
1411         if( imglist.size() == 0 || imglist.size() % 2 != 0 )
1412         {
1413             ts->printf( cvtest::TS::LOG, "The number of images is 0 or an odd number in the case #%d\n", testcase );
1414             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
1415             return;
1416         }
1417
1418         int nframes = (int)(imglist.size()/2);
1419         int npoints = patternSize.width*patternSize.height;
1420         vector<vector<Point3f> > objpt(nframes);
1421         vector<vector<Point2f> > imgpt1(nframes);
1422         vector<vector<Point2f> > imgpt2(nframes);
1423         Size imgsize;
1424         int total = 0;
1425
1426         for( int i = 0; i < nframes; i++ )
1427         {
1428             Mat left = imread(imglist[i*2]);
1429             Mat right = imread(imglist[i*2+1]);
1430             if(!left.data || !right.data)
1431             {
1432                 ts->printf( cvtest::TS::LOG, "Can not load images %s and %s, testcase %d\n",
1433                     imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
1434                 ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
1435                 return;
1436             }
1437             imgsize = left.size();
1438             bool found1 = findChessboardCorners(left, patternSize, imgpt1[i]);
1439             bool found2 = findChessboardCorners(right, patternSize, imgpt2[i]);
1440             if(!found1 || !found2)
1441             {
1442                 ts->printf( cvtest::TS::LOG, "The function could not detect boards on the images %s and %s, testcase %d\n",
1443                     imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
1444                 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1445                 return;
1446             }
1447             total += (int)imgpt1[i].size();
1448             for( int j = 0; j < npoints; j++ )
1449                 objpt[i].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f));
1450         }
1451
1452         // rectify (calibrated)
1453         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;
1454         M1.at<double>(0,2) = M2.at<double>(0,2)=(imgsize.width-1)*0.5;
1455         M1.at<double>(1,2) = M2.at<double>(1,2)=(imgsize.height-1)*0.5;
1456         D1 = Scalar::all(0);
1457         D2 = Scalar::all(0);
1458         double err = calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R, T, E, F,
1459             TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6),
1460             CV_CALIB_SAME_FOCAL_LENGTH
1461             //+ CV_CALIB_FIX_ASPECT_RATIO
1462             + CV_CALIB_FIX_PRINCIPAL_POINT
1463             + CV_CALIB_ZERO_TANGENT_DIST
1464             + CV_CALIB_FIX_K3
1465             + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5 //+ CV_CALIB_FIX_K6
1466             );
1467         err /= nframes*npoints;
1468         if( err > maxReprojErr )
1469         {
1470             ts->printf( cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", err, testcase);
1471             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1472             return;
1473         }
1474
1475         Mat R1, R2, P1, P2, Q;
1476         Rect roi1, roi2;
1477         rectify(M1, D1, M2, D2, imgsize, R, T, R1, R2, P1, P2, Q, 1, imgsize, &roi1, &roi2, 0);
1478         Mat eye33 = Mat::eye(3,3,CV_64F);
1479         Mat R1t = R1.t(), R2t = R2.t();
1480
1481         if( norm(R1t*R1 - eye33) > 0.01 ||
1482             norm(R2t*R2 - eye33) > 0.01 ||
1483             abs(determinant(F)) > 0.01)
1484         {
1485             ts->printf( cvtest::TS::LOG, "The computed (by rectify) R1 and R2 are not orthogonal,"
1486                 "or the computed (by calibrate) F is not singular, testcase %d\n", testcase);
1487             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1488             return;
1489         }
1490
1491         if(!checkPandROI(testcase, M1, D1, R1, P1, imgsize, roi1))
1492         {
1493             ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1494             return;
1495         }
1496
1497         if(!checkPandROI(testcase, M2, D2, R2, P2, imgsize, roi2))
1498         {
1499             ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1500             return;
1501         }
1502
1503         //check that Tx after rectification is equal to distance between cameras
1504         double tx = fabs(P2.at<double>(0, 3) / P2.at<double>(0, 0));
1505         if (fabs(tx - norm(T)) > 1e-5)
1506         {
1507             ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1508             return;
1509         }
1510
1511         //check that Q reprojects points before the camera
1512         double testPoint[4] = {0.0, 0.0, 100.0, 1.0};
1513         Mat reprojectedTestPoint = Q * Mat_<double>(4, 1, testPoint);
1514         CV_Assert(reprojectedTestPoint.type() == CV_64FC1);
1515         if( reprojectedTestPoint.at<double>(2) / reprojectedTestPoint.at<double>(3) < 0 )
1516         {
1517             ts->printf( cvtest::TS::LOG, "A point after rectification is reprojected behind the camera, testcase %d\n", testcase);
1518             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1519         }
1520
1521         //check that Q reprojects the same points as reconstructed by triangulation
1522         const float minCoord = -300.0f;
1523         const float maxCoord = 300.0f;
1524         const float minDisparity = 0.1f;
1525         const float maxDisparity = 600.0f;
1526         const int pointsCount = 500;
1527         const float requiredAccuracy = 1e-3f;
1528         RNG& rng = ts->get_rng();
1529
1530         Mat projectedPoints_1(2, pointsCount, CV_32FC1);
1531         Mat projectedPoints_2(2, pointsCount, CV_32FC1);
1532         Mat disparities(1, pointsCount, CV_32FC1);
1533
1534         rng.fill(projectedPoints_1, RNG::UNIFORM, minCoord, maxCoord);
1535         rng.fill(disparities, RNG::UNIFORM, minDisparity, maxDisparity);
1536         projectedPoints_2.row(0) = projectedPoints_1.row(0) - disparities;
1537         Mat ys_2 = projectedPoints_2.row(1);
1538         projectedPoints_1.row(1).copyTo(ys_2);
1539
1540         Mat points4d;
1541         triangulate(P1, P2, projectedPoints_1, projectedPoints_2, points4d);
1542         Mat homogeneousPoints4d = points4d.t();
1543         const int dimension = 4;
1544         homogeneousPoints4d = homogeneousPoints4d.reshape(dimension);
1545         Mat triangulatedPoints;
1546         convertPointsFromHomogeneous(homogeneousPoints4d, triangulatedPoints);
1547
1548         Mat sparsePoints;
1549         sparsePoints.push_back(projectedPoints_1);
1550         sparsePoints.push_back(disparities);
1551         sparsePoints = sparsePoints.t();
1552         sparsePoints = sparsePoints.reshape(3);
1553         Mat reprojectedPoints;
1554         perspectiveTransform(sparsePoints, reprojectedPoints, Q);
1555
1556         if (norm(triangulatedPoints - reprojectedPoints) / sqrt((double)pointsCount) > requiredAccuracy)
1557         {
1558             ts->printf( cvtest::TS::LOG, "Points reprojected with a matrix Q and points reconstructed by triangulation are different, testcase %d\n", testcase);
1559             ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1560         }
1561
1562         //check correctMatches
1563         const float constraintAccuracy = 1e-5f;
1564         Mat newPoints1, newPoints2;
1565         Mat points1 = projectedPoints_1.t();
1566         points1 = points1.reshape(2, 1);
1567         Mat points2 = projectedPoints_2.t();
1568         points2 = points2.reshape(2, 1);
1569         correctMatches(F, points1, points2, newPoints1, newPoints2);
1570         Mat newHomogeneousPoints1, newHomogeneousPoints2;
1571         convertPointsToHomogeneous(newPoints1, newHomogeneousPoints1);
1572         convertPointsToHomogeneous(newPoints2, newHomogeneousPoints2);
1573         newHomogeneousPoints1 = newHomogeneousPoints1.reshape(1);
1574         newHomogeneousPoints2 = newHomogeneousPoints2.reshape(1);
1575         Mat typedF;
1576         F.convertTo(typedF, newHomogeneousPoints1.type());
1577         for (int i = 0; i < newHomogeneousPoints1.rows; ++i)
1578         {
1579             Mat error = newHomogeneousPoints2.row(i) * typedF * newHomogeneousPoints1.row(i).t();
1580             CV_Assert(error.rows == 1 && error.cols == 1);
1581             if (norm(error) > constraintAccuracy)
1582             {
1583                 ts->printf( cvtest::TS::LOG, "Epipolar constraint is violated after correctMatches, testcase %d\n", testcase);
1584                 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1585             }
1586         }
1587
1588         // rectifyUncalibrated
1589         CV_Assert( imgpt1.size() == imgpt2.size() );
1590         Mat _imgpt1( total, 1, CV_32FC2 ), _imgpt2( total, 1, CV_32FC2 );
1591         vector<vector<Point2f> >::const_iterator iit1 = imgpt1.begin();
1592         vector<vector<Point2f> >::const_iterator iit2 = imgpt2.begin();
1593         for( int pi = 0; iit1 != imgpt1.end(); ++iit1, ++iit2 )
1594         {
1595             vector<Point2f>::const_iterator pit1 = iit1->begin();
1596             vector<Point2f>::const_iterator pit2 = iit2->begin();
1597             CV_Assert( iit1->size() == iit2->size() );
1598             for( ; pit1 != iit1->end(); ++pit1, ++pit2, pi++ )
1599             {
1600                 _imgpt1.at<Point2f>(pi,0) = Point2f( pit1->x, pit1->y );
1601                 _imgpt2.at<Point2f>(pi,0) = Point2f( pit2->x, pit2->y );
1602             }
1603         }
1604
1605         Mat _M1, _M2, _D1, _D2;
1606         vector<Mat> _R1, _R2, _T1, _T2;
1607         calibrateCamera( objpt, imgpt1, imgsize, _M1, _D1, _R1, _T1, 0 );
1608         calibrateCamera( objpt, imgpt2, imgsize, _M2, _D2, _R2, _T1, 0 );
1609         undistortPoints( _imgpt1, _imgpt1, _M1, _D1, Mat(), _M1 );
1610         undistortPoints( _imgpt2, _imgpt2, _M2, _D2, Mat(), _M2 );
1611
1612         Mat matF, _H1, _H2;
1613         matF = findFundamentalMat( _imgpt1, _imgpt2 );
1614         rectifyUncalibrated( _imgpt1, _imgpt2, matF, imgsize, _H1, _H2 );
1615
1616         Mat rectifPoints1, rectifPoints2;
1617         perspectiveTransform( _imgpt1, rectifPoints1, _H1 );
1618         perspectiveTransform( _imgpt2, rectifPoints2, _H2 );
1619
1620         bool verticalStereo = abs(P2.at<double>(0,3)) < abs(P2.at<double>(1,3));
1621         double maxDiff_c = 0, maxDiff_uc = 0;
1622         for( int i = 0, k = 0; i < nframes; i++ )
1623         {
1624             vector<Point2f> temp[2];
1625             undistortPoints(Mat(imgpt1[i]), temp[0], M1, D1, R1, P1);
1626             undistortPoints(Mat(imgpt2[i]), temp[1], M2, D2, R2, P2);
1627
1628             for( int j = 0; j < npoints; j++, k++ )
1629             {
1630                 double diff_c = verticalStereo ? abs(temp[0][j].x - temp[1][j].x) : abs(temp[0][j].y - temp[1][j].y);
1631                 Point2f d = rectifPoints1.at<Point2f>(k,0) - rectifPoints2.at<Point2f>(k,0);
1632                 double diff_uc = verticalStereo ? abs(d.x) : abs(d.y);
1633                 maxDiff_c = max(maxDiff_c, diff_c);
1634                 maxDiff_uc = max(maxDiff_uc, diff_uc);
1635                 if( maxDiff_c > maxScanlineDistErr_c )
1636                 {
1637                     ts->printf( cvtest::TS::LOG, "The distance between %s coordinates is too big(=%g) (used calibrated stereo), testcase %d\n",
1638                         verticalStereo ? "x" : "y", diff_c, testcase);
1639                     ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1640                     return;
1641                 }
1642                 if( maxDiff_uc > maxScanlineDistErr_uc )
1643                 {
1644                     ts->printf( cvtest::TS::LOG, "The distance between %s coordinates is too big(=%g) (used uncalibrated stereo), testcase %d\n",
1645                         verticalStereo ? "x" : "y", diff_uc, testcase);
1646                     ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1647                     return;
1648                 }
1649             }
1650         }
1651
1652         ts->printf( cvtest::TS::LOG, "Testcase %d. Max distance (calibrated) =%g\n"
1653             "Max distance (uncalibrated) =%g\n", testcase, maxDiff_c, maxDiff_uc );
1654     }
1655 }
1656
1657 //-------------------------------- CV_StereoCalibrationTest_C ------------------------------
1658
1659 class CV_StereoCalibrationTest_C : public CV_StereoCalibrationTest
1660 {
1661 public:
1662     CV_StereoCalibrationTest_C() {}
1663 protected:
1664     virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1665         const vector<vector<Point2f> >& imagePoints1,
1666         const vector<vector<Point2f> >& imagePoints2,
1667         Mat& cameraMatrix1, Mat& distCoeffs1,
1668         Mat& cameraMatrix2, Mat& distCoeffs2,
1669         Size imageSize, Mat& R, Mat& T,
1670         Mat& E, Mat& F, TermCriteria criteria, int flags );
1671     virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1672         const Mat& cameraMatrix2, const Mat& distCoeffs2,
1673         Size imageSize, const Mat& R, const Mat& T,
1674         Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1675         double alpha, Size newImageSize,
1676         Rect* validPixROI1, Rect* validPixROI2, int flags );
1677     virtual bool rectifyUncalibrated( const Mat& points1,
1678         const Mat& points2, const Mat& F, Size imgSize,
1679         Mat& H1, Mat& H2, double threshold=5 );
1680     virtual void triangulate( const Mat& P1, const Mat& P2,
1681         const Mat &points1, const Mat &points2,
1682         Mat &points4D );
1683     virtual void correct( const Mat& F,
1684         const Mat &points1, const Mat &points2,
1685         Mat &newPoints1, Mat &newPoints2 );
1686 };
1687
1688 double CV_StereoCalibrationTest_C::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1689                  const vector<vector<Point2f> >& imagePoints1,
1690                  const vector<vector<Point2f> >& imagePoints2,
1691                  Mat& cameraMatrix1, Mat& distCoeffs1,
1692                  Mat& cameraMatrix2, Mat& distCoeffs2,
1693                  Size imageSize, Mat& R, Mat& T,
1694                  Mat& E, Mat& F, TermCriteria criteria, int flags )
1695 {
1696     cameraMatrix1.create( 3, 3, CV_64F );
1697     cameraMatrix2.create( 3, 3, CV_64F);
1698     distCoeffs1.create( 1, 5, CV_64F);
1699     distCoeffs2.create( 1, 5, CV_64F);
1700     R.create(3, 3, CV_64F);
1701     T.create(3, 1, CV_64F);
1702     E.create(3, 3, CV_64F);
1703     F.create(3, 3, CV_64F);
1704
1705     int  nimages = (int)objectPoints.size(), total = 0;
1706     for( int i = 0; i < nimages; i++ )
1707     {
1708         total += (int)objectPoints[i].size();
1709     }
1710
1711     Mat npoints( 1, nimages, CV_32S ),
1712         objPt( 1, total, DataType<Point3f>::type ),
1713         imgPt( 1, total, DataType<Point2f>::type ),
1714         imgPt2( 1, total, DataType<Point2f>::type );
1715
1716     Point2f* imgPtData2 = imgPt2.ptr<Point2f>();
1717     Point3f* objPtData = objPt.ptr<Point3f>();
1718     Point2f* imgPtData = imgPt.ptr<Point2f>();
1719     for( int i = 0, ni = 0, j = 0; i < nimages; i++, j += ni )
1720     {
1721         ni = (int)objectPoints[i].size();
1722         ((int*)npoints.data)[i] = ni;
1723         std::copy(objectPoints[i].begin(), objectPoints[i].end(), objPtData + j);
1724         std::copy(imagePoints1[i].begin(), imagePoints1[i].end(), imgPtData + j);
1725         std::copy(imagePoints2[i].begin(), imagePoints2[i].end(), imgPtData2 + j);
1726     }
1727     CvMat _objPt = objPt, _imgPt = imgPt, _imgPt2 = imgPt2, _npoints = npoints;
1728     CvMat _cameraMatrix1 = cameraMatrix1, _distCoeffs1 = distCoeffs1;
1729     CvMat _cameraMatrix2 = cameraMatrix2, _distCoeffs2 = distCoeffs2;
1730     CvMat matR = R, matT = T, matE = E, matF = F;
1731
1732     return cvStereoCalibrate(&_objPt, &_imgPt, &_imgPt2, &_npoints, &_cameraMatrix1,
1733         &_distCoeffs1, &_cameraMatrix2, &_distCoeffs2, imageSize,
1734         &matR, &matT, &matE, &matF, criteria, flags );
1735 }
1736
1737 void CV_StereoCalibrationTest_C::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1738              const Mat& cameraMatrix2, const Mat& distCoeffs2,
1739              Size imageSize, const Mat& R, const Mat& T,
1740              Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1741              double alpha, Size newImageSize,
1742              Rect* validPixROI1, Rect* validPixROI2, int flags )
1743 {
1744     int rtype = CV_64F;
1745     R1.create(3, 3, rtype);
1746     R2.create(3, 3, rtype);
1747     P1.create(3, 4, rtype);
1748     P2.create(3, 4, rtype);
1749     Q.create(4, 4, rtype);
1750     CvMat _cameraMatrix1 = cameraMatrix1, _distCoeffs1 = distCoeffs1;
1751     CvMat _cameraMatrix2 = cameraMatrix2, _distCoeffs2 = distCoeffs2;
1752     CvMat matR = R, matT = T, _R1 = R1, _R2 = R2, _P1 = P1, _P2 = P2, matQ = Q;
1753     cvStereoRectify( &_cameraMatrix1, &_cameraMatrix2, &_distCoeffs1, &_distCoeffs2,
1754         imageSize, &matR, &matT, &_R1, &_R2, &_P1, &_P2, &matQ, flags,
1755         alpha, newImageSize, (CvRect*)validPixROI1, (CvRect*)validPixROI2);
1756 }
1757
1758 bool CV_StereoCalibrationTest_C::rectifyUncalibrated( const Mat& points1,
1759            const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold )
1760 {
1761     H1.create(3, 3, CV_64F);
1762     H2.create(3, 3, CV_64F);
1763     CvMat _pt1 = points1, _pt2 = points2, matF, *pF=0, _H1 = H1, _H2 = H2;
1764     if( F.size() == Size(3, 3) )
1765         pF = &(matF = F);
1766     return cvStereoRectifyUncalibrated(&_pt1, &_pt2, pF, imgSize, &_H1, &_H2, threshold) > 0;
1767 }
1768
1769 void CV_StereoCalibrationTest_C::triangulate( const Mat& P1, const Mat& P2,
1770         const Mat &points1, const Mat &points2,
1771         Mat &points4D )
1772 {
1773     CvMat _P1 = P1, _P2 = P2, _points1 = points1, _points2 = points2;
1774     points4D.create(4, points1.cols, points1.type());
1775     CvMat _points4D = points4D;
1776     cvTriangulatePoints(&_P1, &_P2, &_points1, &_points2, &_points4D);
1777 }
1778
1779 void CV_StereoCalibrationTest_C::correct( const Mat& F,
1780         const Mat &points1, const Mat &points2,
1781         Mat &newPoints1, Mat &newPoints2 )
1782 {
1783     CvMat _F = F, _points1 = points1, _points2 = points2;
1784     newPoints1.create(1, points1.cols, points1.type());
1785     newPoints2.create(1, points2.cols, points2.type());
1786     CvMat _newPoints1 = newPoints1, _newPoints2 = newPoints2;
1787     cvCorrectMatches(&_F, &_points1, &_points2, &_newPoints1, &_newPoints2);
1788 }
1789
1790 //-------------------------------- CV_StereoCalibrationTest_CPP ------------------------------
1791
1792 class CV_StereoCalibrationTest_CPP : public CV_StereoCalibrationTest
1793 {
1794 public:
1795     CV_StereoCalibrationTest_CPP() {}
1796 protected:
1797     virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1798         const vector<vector<Point2f> >& imagePoints1,
1799         const vector<vector<Point2f> >& imagePoints2,
1800         Mat& cameraMatrix1, Mat& distCoeffs1,
1801         Mat& cameraMatrix2, Mat& distCoeffs2,
1802         Size imageSize, Mat& R, Mat& T,
1803         Mat& E, Mat& F, TermCriteria criteria, int flags );
1804     virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1805         const Mat& cameraMatrix2, const Mat& distCoeffs2,
1806         Size imageSize, const Mat& R, const Mat& T,
1807         Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1808         double alpha, Size newImageSize,
1809         Rect* validPixROI1, Rect* validPixROI2, int flags );
1810     virtual bool rectifyUncalibrated( const Mat& points1,
1811         const Mat& points2, const Mat& F, Size imgSize,
1812         Mat& H1, Mat& H2, double threshold=5 );
1813     virtual void triangulate( const Mat& P1, const Mat& P2,
1814         const Mat &points1, const Mat &points2,
1815         Mat &points4D );
1816     virtual void correct( const Mat& F,
1817         const Mat &points1, const Mat &points2,
1818         Mat &newPoints1, Mat &newPoints2 );
1819 };
1820
1821 double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1822                                              const vector<vector<Point2f> >& imagePoints1,
1823                                              const vector<vector<Point2f> >& imagePoints2,
1824                                              Mat& cameraMatrix1, Mat& distCoeffs1,
1825                                              Mat& cameraMatrix2, Mat& distCoeffs2,
1826                                              Size imageSize, Mat& R, Mat& T,
1827                                              Mat& E, Mat& F, TermCriteria criteria, int flags )
1828 {
1829     return stereoCalibrate( objectPoints, imagePoints1, imagePoints2,
1830                     cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
1831                     imageSize, R, T, E, F, criteria, flags );
1832 }
1833
1834 void CV_StereoCalibrationTest_CPP::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1835                                          const Mat& cameraMatrix2, const Mat& distCoeffs2,
1836                                          Size imageSize, const Mat& R, const Mat& T,
1837                                          Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1838                                          double alpha, Size newImageSize,
1839                                          Rect* validPixROI1, Rect* validPixROI2, int flags )
1840 {
1841     stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
1842                 imageSize, R, T, R1, R2, P1, P2, Q, flags, alpha, newImageSize,validPixROI1, validPixROI2 );
1843 }
1844
1845 bool CV_StereoCalibrationTest_CPP::rectifyUncalibrated( const Mat& points1,
1846                        const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold )
1847 {
1848     return stereoRectifyUncalibrated( points1, points2, F, imgSize, H1, H2, threshold );
1849 }
1850
1851 void CV_StereoCalibrationTest_CPP::triangulate( const Mat& P1, const Mat& P2,
1852         const Mat &points1, const Mat &points2,
1853         Mat &points4D )
1854 {
1855     triangulatePoints(P1, P2, points1, points2, points4D);
1856 }
1857
1858 void CV_StereoCalibrationTest_CPP::correct( const Mat& F,
1859         const Mat &points1, const Mat &points2,
1860         Mat &newPoints1, Mat &newPoints2 )
1861 {
1862     correctMatches(F, points1, points2, newPoints1, newPoints2);
1863 }
1864
1865 ///////////////////////////////////////////////////////////////////////////////////////////////////
1866
1867 TEST(Calib3d_CalibrateCamera_C, regression) { CV_CameraCalibrationTest_C test; test.safe_run(); }
1868 TEST(Calib3d_CalibrateCamera_CPP, regression) { CV_CameraCalibrationTest_CPP test; test.safe_run(); }
1869 TEST(Calib3d_CalibrationMatrixValues_C, accuracy) { CV_CalibrationMatrixValuesTest_C test; test.safe_run(); }
1870 TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); }
1871 TEST(Calib3d_ProjectPoints_C, accuracy) { CV_ProjectPointsTest_C  test; test.safe_run(); }
1872 TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); }
1873 TEST(Calib3d_StereoCalibrate_C, regression) { CV_StereoCalibrationTest_C test; test.safe_run(); }
1874 TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); }