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
 337     sprintf( filepath, "%scameracalibration/", ts->get_data_path().c_str() );
 338     sprintf( filename, "%sdatafiles.txt", filepath );
 339     datafile = fopen( filename, "r" );
 340     if( datafile == 0 )
 341     {
 342         ts->printf( cvtest::TS::LOG, "Could not open file with list of test files: %s\n", filename );
 343         code = cvtest::TS::FAIL_MISSING_TEST_DATA;
 344         goto _exit_;
 345     }
 346
 347     fscanf(datafile,"%d",&numTests);
 348
 349     for( currTest = start_from; currTest < numTests; currTest++ )
 350     {
 351         fscanf(datafile,"%s",i_dat_file);
 352         sprintf(filename, "%s%s", filepath, i_dat_file);
 353         file = fopen(filename,"r");
 354
 355         ts->update_context( this, currTest, true );
 356
 357         if( file == 0 )
 358         {
 359             ts->printf( cvtest::TS::LOG,
 360                 "Can't open current test file: %s\n",filename);
 361             if( numTests == 1 )
 362             {
 363                 code = cvtest::TS::FAIL_MISSING_TEST_DATA;
 364                 goto _exit_;
 365             }
 366             continue; // if there is more than one test, just skip the test
 367         }
 368
 369         fscanf(file,"%d %d\n",&(imageSize.width),&(imageSize.height));
 370         if( imageSize.width <= 0 || imageSize.height <= 0 )
 371         {
 372             ts->printf( cvtest::TS::LOG, "Image size in test file is incorrect\n" );
 373             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 374             goto _exit_;
 375         }
 376
 377         /* Read etalon size */
 378         fscanf(file,"%d %d\n",&(etalonSize.width),&(etalonSize.height));
 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             goto _exit_;
 384         }
 385
 386         numPoints = etalonSize.width * etalonSize.height;
 387
 388         /* Read number of images */
 389         fscanf(file,"%d\n",&numImages);
 390         if( numImages <=0 )
 391         {
 392             ts->printf( cvtest::TS::LOG, "Number of images in test file is incorrect\n");
 393             code = cvtest::TS::FAIL_INVALID_TEST_DATA;
 394             goto _exit_;
 395         }
 396
 397         /* Need to allocate memory */
 398         imagePoints     = (CvPoint2D64f*)cvAlloc( numPoints *
 399                                                     numImages * sizeof(CvPoint2D64f));
 400
 401         objectPoints    = (CvPoint3D64f*)cvAlloc( numPoints *
 402                                                     numImages * sizeof(CvPoint3D64f));
 403
 404         reprojectPoints = (CvPoint2D64f*)cvAlloc( numPoints *
 405                                                     numImages * sizeof(CvPoint2D64f));
 406
 407         /* Alloc memory for numbers */
 408         numbers = (int*)cvAlloc( numImages * sizeof(int));
 409
 410         /* Fill it by numbers of points of each image*/
 411         for( currImage = 0; currImage < numImages; currImage++ )
 412         {
 413             numbers[currImage] = etalonSize.width * etalonSize.height;
 414         }
 415
 416         /* Allocate memory for translate vectors and rotmatrixs*/
 417         transVects     = (double*)cvAlloc(3 * 1 * numImages * sizeof(double));
 418         rotMatrs       = (double*)cvAlloc(3 * 3 * numImages * sizeof(double));
 419
 420         goodTransVects = (double*)cvAlloc(3 * 1 * numImages * sizeof(double));
 421         goodRotMatrs   = (double*)cvAlloc(3 * 3 * numImages * sizeof(double));
 422
 423         /* Read object points */
 424         i = 0;/* shift for current point */
 425         for( currImage = 0; currImage < numImages; currImage++ )
 426         {
 427             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 428             {
 429                 double x,y,z;
 430                 fscanf(file,"%lf %lf %lf\n",&x,&y,&z);
 431
 432                 (objectPoints+i)->x = x;
 433                 (objectPoints+i)->y = y;
 434                 (objectPoints+i)->z = z;
 435                 i++;
 436             }
 437         }
 438
 439         /* Read image points */
 440         i = 0;/* shift for current point */
 441         for( currImage = 0; currImage < numImages; currImage++ )
 442         {
 443             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 444             {
 445                 double x,y;
 446                 fscanf(file,"%lf %lf\n",&x,&y);
 447
 448                 (imagePoints+i)->x = x;
 449                 (imagePoints+i)->y = y;
 450                 i++;
 451             }
 452         }
 453
 454         /* Read good data computed before */
 455
 456         /* Focal lengths */
 457         double goodFcx,goodFcy;
 458         fscanf(file,"%lf %lf",&goodFcx,&goodFcy);
 459
 460         /* Principal points */
 461         double goodCx,goodCy;
 462         fscanf(file,"%lf %lf",&goodCx,&goodCy);
 463
 464         /* Read distortion */
 465
 466         fscanf(file,"%lf",goodDistortion+0);
 467         fscanf(file,"%lf",goodDistortion+1);
 468         fscanf(file,"%lf",goodDistortion+2);
 469         fscanf(file,"%lf",goodDistortion+3);
 470
 471         /* Read good Rot matrixes */
 472         for( currImage = 0; currImage < numImages; currImage++ )
 473         {
 474             for( i = 0; i < 3; i++ )
 475                 for( j = 0; j < 3; j++ )
 476                     fscanf(file, "%lf", goodRotMatrs + currImage * 9 + j * 3 + i);
 477         }
 478
 479         /* Read good Trans vectors */
 480         for( currImage = 0; currImage < numImages; currImage++ )
 481         {
 482             for( i = 0; i < 3; i++ )
 483                 fscanf(file, "%lf", goodTransVects + currImage * 3 + i);
 484         }
 485
 486         calibFlags = 0
 487                      // + CV_CALIB_FIX_PRINCIPAL_POINT
 488                      // + CV_CALIB_ZERO_TANGENT_DIST
 489                      // + CV_CALIB_FIX_ASPECT_RATIO
 490                      // + CV_CALIB_USE_INTRINSIC_GUESS
 491                      + CV_CALIB_FIX_K3
 492                      + CV_CALIB_FIX_K4+CV_CALIB_FIX_K5
 493                      + CV_CALIB_FIX_K6
 494                     ;
 495         memset( cameraMatrix, 0, 9*sizeof(cameraMatrix[0]) );
 496         cameraMatrix[0] = cameraMatrix[4] = 807.;
 497         cameraMatrix[2] = (imageSize.width - 1)*0.5;
 498         cameraMatrix[5] = (imageSize.height - 1)*0.5;
 499         cameraMatrix[8] = 1.;
 500
 501         /* Now we can calibrate camera */
 502         calibrate(  numImages,
 503                     numbers,
 504                     imageSize,
 505                     imagePoints,
 506                     objectPoints,
 507                     distortion,
 508                     cameraMatrix,
 509                     transVects,
 510                     rotMatrs,
 511                     calibFlags );
 512
 513         /* ---- Reproject points to the image ---- */
 514         for( currImage = 0; currImage < numImages; currImage++ )
 515         {
 516             int numPoints = etalonSize.width * etalonSize.height;
 517             project(  numPoints,
 518                       objectPoints + currImage * numPoints,
 519                       rotMatrs + currImage * 9,
 520                       transVects + currImage * 3,
 521                       cameraMatrix,
 522                       distortion,
 523                       reprojectPoints + currImage * numPoints);
 524         }
 525
 526         /* ----- Compute reprojection error ----- */
 527         i = 0;
 528         double dx,dy;
 529         double rx,ry;
 530         double meanDx,meanDy;
 531         double maxDx = 0.0;
 532         double maxDy = 0.0;
 533
 534         meanDx = 0;
 535         meanDy = 0;
 536         for( currImage = 0; currImage < numImages; currImage++ )
 537         {
 538             for( currPoint = 0; currPoint < etalonSize.width * etalonSize.height; currPoint++ )
 539             {
 540                 rx = reprojectPoints[i].x;
 541                 ry = reprojectPoints[i].y;
 542                 dx = rx - imagePoints[i].x;
 543                 dy = ry - imagePoints[i].y;
 544
 545                 meanDx += dx;
 546                 meanDy += dy;
 547
 548                 dx = fabs(dx);
 549                 dy = fabs(dy);
 550
 551                 if( dx > maxDx )
 552                     maxDx = dx;
 553
 554                 if( dy > maxDy )
 555                     maxDy = dy;
 556                 i++;
 557             }
 558         }
 559
 560         meanDx /= numImages * etalonSize.width * etalonSize.height;
 561         meanDy /= numImages * etalonSize.width * etalonSize.height;
 562
 563         /* ========= Compare parameters ========= */
 564
 565         /* ----- Compare focal lengths ----- */
 566         code = compare(cameraMatrix+0,&goodFcx,1,0.1,"fx");
 567         if( code < 0 )
 568             goto _exit_;
 569
 570         code = compare(cameraMatrix+4,&goodFcy,1,0.1,"fy");
 571         if( code < 0 )
 572             goto _exit_;
 573
 574         /* ----- Compare principal points ----- */
 575         code = compare(cameraMatrix+2,&goodCx,1,0.1,"cx");
 576         if( code < 0 )
 577             goto _exit_;
 578
 579         code = compare(cameraMatrix+5,&goodCy,1,0.1,"cy");
 580         if( code < 0 )
 581             goto _exit_;
 582
 583         /* ----- Compare distortion ----- */
 584         code = compare(distortion,goodDistortion,4,0.1,"[k1,k2,p1,p2]");
 585         if( code < 0 )
 586             goto _exit_;
 587
 588         /* ----- Compare rot matrixs ----- */
 589         code = compare(rotMatrs,goodRotMatrs, 9*numImages,0.05,"rotation matrices");
 590         if( code < 0 )
 591             goto _exit_;
 592
 593         /* ----- Compare rot matrixs ----- */
 594         code = compare(transVects,goodTransVects, 3*numImages,0.1,"translation vectors");
 595         if( code < 0 )
 596             goto _exit_;
 597
 598         if( maxDx > 1.0 )
 599         {
 600             ts->printf( cvtest::TS::LOG,
 601                       "Error in reprojection maxDx=%f > 1.0\n",maxDx);
 602             code = cvtest::TS::FAIL_BAD_ACCURACY; goto _exit_;
 603         }
 604
 605         if( maxDy > 1.0 )
 606         {
 607             ts->printf( cvtest::TS::LOG,
 608                       "Error in reprojection maxDy=%f > 1.0\n",maxDy);
 609             code = cvtest::TS::FAIL_BAD_ACCURACY; goto _exit_;
 610         }
 611
 612         progress = update_progress( progress, currTest, numTests, 0 );
 613
 614         cvFree(&imagePoints);
 615         cvFree(&objectPoints);
 616         cvFree(&reprojectPoints);
 617         cvFree(&numbers);
 618
 619         cvFree(&transVects);
 620         cvFree(&rotMatrs);
 621         cvFree(&goodTransVects);
 622         cvFree(&goodRotMatrs);
 623
 624         fclose(file);
 625         file = 0;
 626     }
 627
 628 _exit_:
 629
 630     if( file )
 631         fclose(file);
 632
 633     if( datafile )
 634         fclose(datafile);
 635
 636     /* Free all allocated memory */
 637     cvFree(&imagePoints);
 638     cvFree(&objectPoints);
 639     cvFree(&reprojectPoints);
 640     cvFree(&numbers);
 641
 642     cvFree(&transVects);
 643     cvFree(&rotMatrs);
 644     cvFree(&goodTransVects);
 645     cvFree(&goodRotMatrs);
 646
 647     if( code < 0 )
 648         ts->set_failed_test_info( code );
 649 }
 650
 651 // --------------------------------- CV_CameraCalibrationTest_C --------------------------------------------
 652
 653 class CV_CameraCalibrationTest_C : public CV_CameraCalibrationTest
 654 {
 655 public:
 656         CV_CameraCalibrationTest_C(){}
 657 protected:
 658         virtual void calibrate( int imageCount, int* pointCounts,
 659                 CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 660                 double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 661                 double* rotationMatrices, int flags );
 662         virtual void project( int pointCount, CvPoint3D64f* objectPoints,
 663                 double* rotationMatrix, double*  translationVector,
 664                 double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints );
 665 };
 666
 667 void CV_CameraCalibrationTest_C::calibrate( int imageCount, int* pointCounts,
 668                 CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 669                 double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 670                 double* rotationMatrices, int flags )
 671 {
 672     int i, total = 0;
 673     for( i = 0; i < imageCount; i++ )
 674         total += pointCounts[i];
 675
 676     CvMat _objectPoints = cvMat(1, total, CV_64FC3, objectPoints);
 677     CvMat _imagePoints = cvMat(1, total, CV_64FC2, imagePoints);
 678     CvMat _pointCounts = cvMat(1, imageCount, CV_32S, pointCounts);
 679     CvMat _cameraMatrix = cvMat(3, 3, CV_64F, cameraMatrix);
 680     CvMat _distCoeffs = cvMat(4, 1, CV_64F, distortionCoeffs);
 681     CvMat _rotationMatrices = cvMat(imageCount, 9, CV_64F, rotationMatrices);
 682     CvMat _translationVectors = cvMat(imageCount, 3, CV_64F, translationVectors);
 683
 684     cvCalibrateCamera2(&_objectPoints, &_imagePoints, &_pointCounts, imageSize,
 685                        &_cameraMatrix, &_distCoeffs, &_rotationMatrices, &_translationVectors,
 686                        flags);
 687 }
 688
 689 void CV_CameraCalibrationTest_C::project( int pointCount, CvPoint3D64f* objectPoints,
 690                 double* rotationMatrix, double*  translationVector,
 691                 double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints )
 692 {
 693         CvMat _objectPoints = cvMat(1, pointCount, CV_64FC3, objectPoints);
 694     CvMat _imagePoints = cvMat(1, pointCount, CV_64FC2, imagePoints);
 695     CvMat _cameraMatrix = cvMat(3, 3, CV_64F, cameraMatrix);
 696     CvMat _distCoeffs = cvMat(4, 1, CV_64F, distortion);
 697     CvMat _rotationMatrix = cvMat(3, 3, CV_64F, rotationMatrix);
 698     CvMat _translationVector = cvMat(1, 3, CV_64F, translationVector);
 699
 700     cvProjectPoints2(&_objectPoints, &_rotationMatrix, &_translationVector, &_cameraMatrix, &_distCoeffs, &_imagePoints);
 701 }
 702
 703 // --------------------------------- CV_CameraCalibrationTest_CPP --------------------------------------------
 704
 705 class CV_CameraCalibrationTest_CPP : public CV_CameraCalibrationTest
 706 {
 707 public:
 708         CV_CameraCalibrationTest_CPP(){}
 709 protected:
 710         virtual void calibrate( int imageCount, int* pointCounts,
 711                 CvSize imageSize, CvPoint2D64f* imagePoints, CvPoint3D64f* objectPoints,
 712                 double* distortionCoeffs, double* cameraMatrix, double* translationVectors,
 713                 double* rotationMatrices, int flags );
 714         virtual void project( int pointCount, CvPoint3D64f* objectPoints,
 715                 double* rotationMatrix, double*  translationVector,
 716                 double* cameraMatrix, double* distortion, CvPoint2D64f* imagePoints );
 717 };
 718
 719 void CV_CameraCalibrationTest_CPP::calibrate( int imageCount, int* pointCounts,
 720                 CvSize _imageSize, CvPoint2D64f* _imagePoints, CvPoint3D64f* _objectPoints,
 721                 double* _distortionCoeffs, double* _cameraMatrix, double* translationVectors,
 722                 double* rotationMatrices, int flags )
 723 {
 724         vector<vector<Point3f> > objectPoints( imageCount );
 725         vector<vector<Point2f> > imagePoints( imageCount );
 726         Size imageSize = _imageSize;
 727         Mat cameraMatrix, distCoeffs(1,4,CV_64F,Scalar::all(0));
 728         vector<Mat> rvecs, tvecs;
 729
 730         CvPoint3D64f* op = _objectPoints;
 731         CvPoint2D64f* ip = _imagePoints;
 732         vector<vector<Point3f> >::iterator objectPointsIt = objectPoints.begin();
 733         vector<vector<Point2f> >::iterator imagePointsIt = imagePoints.begin();
 734         for( int i = 0; i < imageCount; ++objectPointsIt, ++imagePointsIt, i++ )
 735         {
 736                 int num = pointCounts[i];
 737                 objectPointsIt->resize( num );
 738                 imagePointsIt->resize( num );
 739                 vector<Point3f>::iterator oIt = objectPointsIt->begin();
 740                 vector<Point2f>::iterator iIt = imagePointsIt->begin();
 741                 for( int j = 0; j < num; ++oIt, ++iIt, j++, op++, ip++)
 742                 {
 743                         oIt->x = (float)op->x, oIt->y = (float)op->y, oIt->z = (float)op->z;
 744                         iIt->x = (float)ip->x, iIt->y = (float)ip->y;
 745                 }
 746         }
 747
 748         calibrateCamera( objectPoints,
 749                          imagePoints,
 750                                          imageSize,
 751                                          cameraMatrix,
 752                                          distCoeffs,
 753                                          rvecs,
 754                                          tvecs,
 755                                          flags );
 756
 757         assert( cameraMatrix.type() == CV_64FC1 );
 758         memcpy( _cameraMatrix, cameraMatrix.data, 9*sizeof(double) );
 759
 760         assert( cameraMatrix.type() == CV_64FC1 );
 761         memcpy( _distortionCoeffs, distCoeffs.data, 4*sizeof(double) );
 762
 763         vector<Mat>::iterator rvecsIt = rvecs.begin();
 764         vector<Mat>::iterator tvecsIt = tvecs.begin();
 765         double *rm = rotationMatrices,
 766                    *tm = translationVectors;
 767         assert( rvecsIt->type() == CV_64FC1 );
 768         assert( tvecsIt->type() == CV_64FC1 );
 769         for( int i = 0; i < imageCount; ++rvecsIt, ++tvecsIt, i++, rm+=9, tm+=3 )
 770         {
 771                 Mat r9( 3, 3, CV_64FC1 );
 772                 Rodrigues( *rvecsIt, r9 );
 773                 memcpy( rm, r9.data, 9*sizeof(double) );
 774                 memcpy( tm, tvecsIt->data, 3*sizeof(double) );
 775         }
 776 }
 777
 778 void CV_CameraCalibrationTest_CPP::project( int pointCount, CvPoint3D64f* _objectPoints,
 779                 double* rotationMatrix, double*  translationVector,
 780                 double* _cameraMatrix, double* distortion, CvPoint2D64f* _imagePoints )
 781 {
 782         Mat objectPoints( pointCount, 3, CV_64FC1, _objectPoints );
 783         Mat rmat( 3, 3, CV_64FC1, rotationMatrix ),
 784                 rvec( 1, 3, CV_64FC1 ),
 785                 tvec( 1, 3, CV_64FC1, translationVector );
 786         Mat cameraMatrix( 3, 3, CV_64FC1, _cameraMatrix );
 787         Mat distCoeffs( 1, 4, CV_64FC1, distortion );
 788         vector<Point2f> imagePoints;
 789         Rodrigues( rmat, rvec );
 790
 791         objectPoints.convertTo( objectPoints, CV_32FC1 );
 792         projectPoints( objectPoints, rvec, tvec,
 793                                    cameraMatrix, distCoeffs, imagePoints );
 794         vector<Point2f>::const_iterator it = imagePoints.begin();
 795         for( int i = 0; it != imagePoints.end(); ++it, i++ )
 796         {
 797                 _imagePoints[i] = cvPoint2D64f( it->x, it->y );
 798         }
 799 }
 800
 801
 802 //----------------------------------------- CV_CalibrationMatrixValuesTest --------------------------------
 803
 804 class CV_CalibrationMatrixValuesTest : public cvtest::BaseTest
 805 {
 806 public:
 807         CV_CalibrationMatrixValuesTest() {}
 808 protected:
 809         void run(int);
 810         virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 811                 double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 812                 Point2d& principalPoint, double& aspectRatio ) = 0;
 813 };
 814
 815 void CV_CalibrationMatrixValuesTest::run(int)
 816 {
 817         int code = cvtest::TS::OK;
 818         const double fcMinVal = 1e-5;
 819         const double fcMaxVal = 1000;
 820         const double apertureMaxVal = 0.01;
 821
 822         RNG rng = ts->get_rng();
 823
 824         double fx, fy, cx, cy, nx, ny;
 825         Mat cameraMatrix( 3, 3, CV_64FC1 );
 826         cameraMatrix.setTo( Scalar(0) );
 827         fx = cameraMatrix.at<double>(0,0) = rng.uniform( fcMinVal, fcMaxVal );
 828         fy = cameraMatrix.at<double>(1,1) = rng.uniform( fcMinVal, fcMaxVal );
 829         cx = cameraMatrix.at<double>(0,2) = rng.uniform( fcMinVal, fcMaxVal );
 830         cy = cameraMatrix.at<double>(1,2) = rng.uniform( fcMinVal, fcMaxVal );
 831         cameraMatrix.at<double>(2,2) = 1;
 832
 833         Size imageSize( 600, 400 );
 834
 835         double apertureWidth = (double)rng * apertureMaxVal,
 836                    apertureHeight = (double)rng * apertureMaxVal;
 837
 838         double fovx, fovy, focalLength, aspectRatio,
 839                    goodFovx, goodFovy, goodFocalLength, goodAspectRatio;
 840         Point2d principalPoint, goodPrincipalPoint;
 841
 842
 843         calibMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
 844                 fovx, fovy, focalLength, principalPoint, aspectRatio );
 845
 846         // calculate calibration matrix values
 847         goodAspectRatio = fy / fx;
 848
 849         if( apertureWidth != 0.0 && apertureHeight != 0.0 )
 850         {
 851                 nx = imageSize.width / apertureWidth;
 852                 ny = imageSize.height / apertureHeight;
 853         }
 854         else
 855         {
 856                 nx = 1.0;
 857                 ny = goodAspectRatio;
 858         }
 859
 860         goodFovx = 2 * atan( imageSize.width / (2 * fx)) * 180.0 / CV_PI;
 861         goodFovy = 2 * atan( imageSize.height / (2 * fy)) * 180.0 / CV_PI;
 862
 863         goodFocalLength = fx / nx;
 864
 865         goodPrincipalPoint.x = cx / nx;
 866         goodPrincipalPoint.y = cy / ny;
 867
 868         // check results
 869         if( fabs(fovx - goodFovx) > FLT_EPSILON )
 870         {
 871                 ts->printf( cvtest::TS::LOG, "bad fovx (real=%f, good = %f\n", fovx, goodFovx );
 872                 code = cvtest::TS::FAIL_BAD_ACCURACY;
 873                 goto _exit_;
 874         }
 875         if( fabs(fovy - goodFovy) > FLT_EPSILON )
 876         {
 877                 ts->printf( cvtest::TS::LOG, "bad fovy (real=%f, good = %f\n", fovy, goodFovy );
 878                 code = cvtest::TS::FAIL_BAD_ACCURACY;
 879                 goto _exit_;
 880         }
 881         if( fabs(focalLength - goodFocalLength) > FLT_EPSILON )
 882         {
 883                 ts->printf( cvtest::TS::LOG, "bad focalLength (real=%f, good = %f\n", focalLength, goodFocalLength );
 884                 code = cvtest::TS::FAIL_BAD_ACCURACY;
 885                 goto _exit_;
 886         }
 887         if( fabs(aspectRatio - goodAspectRatio) > FLT_EPSILON )
 888         {
 889                 ts->printf( cvtest::TS::LOG, "bad aspectRatio (real=%f, good = %f\n", aspectRatio, goodAspectRatio );
 890                 code = cvtest::TS::FAIL_BAD_ACCURACY;
 891                 goto _exit_;
 892         }
 893         if( norm( principalPoint - goodPrincipalPoint ) > FLT_EPSILON )
 894         {
 895                 ts->printf( cvtest::TS::LOG, "bad principalPoint\n" );
 896                 code = cvtest::TS::FAIL_BAD_ACCURACY;
 897                 goto _exit_;
 898         }
 899
 900 _exit_:
 901         RNG& _rng = ts->get_rng();
 902         _rng = rng;
 903         ts->set_failed_test_info( code );
 904 }
 905
 906 //----------------------------------------- CV_CalibrationMatrixValuesTest_C --------------------------------
 907
 908 class CV_CalibrationMatrixValuesTest_C : public CV_CalibrationMatrixValuesTest
 909 {
 910 public:
 911         CV_CalibrationMatrixValuesTest_C(){}
 912 protected:
 913         virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 914                 double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 915                 Point2d& principalPoint, double& aspectRatio );
 916 };
 917
 918 void CV_CalibrationMatrixValuesTest_C::calibMatrixValues( const Mat& _cameraMatrix, Size imageSize,
 919                                                                                            double apertureWidth, double apertureHeight,
 920                                                                                            double& fovx, double& fovy, double& focalLength,
 921                                                                                            Point2d& principalPoint, double& aspectRatio )
 922 {
 923         CvMat cameraMatrix = _cameraMatrix;
 924         CvPoint2D64f pp;
 925         cvCalibrationMatrixValues( &cameraMatrix, imageSize, apertureWidth, apertureHeight,
 926                 &fovx, &fovy, &focalLength, &pp, &aspectRatio );
 927         principalPoint.x = pp.x;
 928         principalPoint.y = pp.y;
 929 }
 930
 931
 932 //----------------------------------------- CV_CalibrationMatrixValuesTest_CPP --------------------------------
 933
 934 class CV_CalibrationMatrixValuesTest_CPP : public CV_CalibrationMatrixValuesTest
 935 {
 936 public:
 937         CV_CalibrationMatrixValuesTest_CPP() {}
 938 protected:
 939         virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 940                 double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
 941                 Point2d& principalPoint, double& aspectRatio );
 942 };
 943
 944 void CV_CalibrationMatrixValuesTest_CPP::calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
 945                                                                                                                  double apertureWidth, double apertureHeight,
 946                                                                                                                  double& fovx, double& fovy, double& focalLength,
 947                                                                                                                  Point2d& principalPoint, double& aspectRatio )
 948 {
 949         calibrationMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
 950                 fovx, fovy, focalLength, principalPoint, aspectRatio );
 951 }
 952
 953
 954 //----------------------------------------- CV_ProjectPointsTest --------------------------------
 955 void calcdfdx( const vector<vector<Point2f> >& leftF, const vector<vector<Point2f> >& rightF, double eps, Mat& dfdx )
 956 {
 957     const int fdim = 2;
 958         CV_Assert( !leftF.empty() && !rightF.empty() && !leftF[0].empty() && !rightF[0].empty() );
 959         CV_Assert( leftF[0].size() ==  rightF[0].size() );
 960         CV_Assert( fabs(eps) > std::numeric_limits<double>::epsilon() );
 961         int fcount = (int)leftF[0].size(), xdim = (int)leftF.size();
 962
 963         dfdx.create( fcount*fdim, xdim, CV_64FC1 );
 964
 965         vector<vector<Point2f> >::const_iterator arrLeftIt = leftF.begin();
 966         vector<vector<Point2f> >::const_iterator arrRightIt = rightF.begin();
 967         for( int xi = 0; xi < xdim; xi++, ++arrLeftIt, ++arrRightIt )
 968         {
 969         CV_Assert( (int)arrLeftIt->size() ==  fcount );
 970         CV_Assert( (int)arrRightIt->size() ==  fcount );
 971         vector<Point2f>::const_iterator lIt = arrLeftIt->begin();
 972         vector<Point2f>::const_iterator rIt = arrRightIt->begin();
 973         for( int fi = 0; fi < dfdx.rows; fi+=fdim, ++lIt, ++rIt )
 974         {
 975             dfdx.at<double>(fi, xi )   = 0.5 * ((double)(rIt->x - lIt->x)) / eps;
 976                         dfdx.at<double>(fi+1, xi ) = 0.5 * ((double)(rIt->y - lIt->y)) / eps;
 977                 }
 978         }
 979 }
 980
 981 class CV_ProjectPointsTest : public cvtest::BaseTest
 982 {
 983 public:
 984         CV_ProjectPointsTest() {}
 985 protected:
 986         void run(int);
 987         virtual void project( const Mat& objectPoints,
 988                 const Mat& rvec, const Mat& tvec,
 989                 const Mat& cameraMatrix,
 990                 const Mat& distCoeffs,
 991                 vector<Point2f>& imagePoints,
 992                 Mat& dpdrot, Mat& dpdt, Mat& dpdf,
 993                 Mat& dpdc, Mat& dpddist,
 994                 double aspectRatio=0 ) = 0;
 995 };
 996
 997 void CV_ProjectPointsTest::run(int)
 998 {
 999     //typedef float matType;
1000
1001         int code = cvtest::TS::OK;
1002         const int pointCount = 100;
1003
1004         const float zMinVal = 10.0f, zMaxVal = 100.0f,
1005                 rMinVal = -0.3f, rMaxVal = 0.3f,
1006                                 tMinVal = -2.0f, tMaxVal = 2.0f;
1007
1008     const float imgPointErr = 1e-3f,
1009                 dEps = 1e-3f;
1010
1011     double err;
1012
1013     Size imgSize( 600, 800 );
1014     Mat_<float> objPoints( pointCount, 3), rvec( 1, 3), rmat, tvec( 1, 3 ), cameraMatrix( 3, 3 ), distCoeffs( 1, 4 ),
1015       leftRvec, rightRvec, leftTvec, rightTvec, leftCameraMatrix, rightCameraMatrix, leftDistCoeffs, rightDistCoeffs;
1016
1017         RNG rng = ts->get_rng();
1018
1019         // generate data
1020         cameraMatrix << 300.f,  0.f,    imgSize.width/2.f,
1021                     0.f,    300.f,  imgSize.height/2.f,
1022                     0.f,    0.f,    1.f;
1023         distCoeffs << 0.1, 0.01, 0.001, 0.001;
1024
1025         rvec(0,0) = rng.uniform( rMinVal, rMaxVal );
1026         rvec(0,1) = rng.uniform( rMinVal, rMaxVal );
1027         rvec(0,2) = rng.uniform( rMinVal, rMaxVal );
1028         Rodrigues( rvec, rmat );
1029
1030         tvec(0,0) = rng.uniform( tMinVal, tMaxVal );
1031         tvec(0,1) = rng.uniform( tMinVal, tMaxVal );
1032         tvec(0,2) = rng.uniform( tMinVal, tMaxVal );
1033
1034     for( int y = 0; y < objPoints.rows; y++ )
1035         {
1036             Mat point(1, 3, CV_32FC1, objPoints.ptr(y) );
1037             float z = rng.uniform( zMinVal, zMaxVal );
1038             point.at<float>(0,2) = z;
1039         point.at<float>(0,0) = (rng.uniform(2.f,(float)(imgSize.width-2)) - cameraMatrix(0,2)) / cameraMatrix(0,0) * z;
1040         point.at<float>(0,1) = (rng.uniform(2.f,(float)(imgSize.height-2)) - cameraMatrix(1,2)) / cameraMatrix(1,1) * z;
1041         point = (point - tvec) * rmat;
1042         }
1043
1044         vector<Point2f> imgPoints;
1045         vector<vector<Point2f> > leftImgPoints;
1046         vector<vector<Point2f> > rightImgPoints;
1047         Mat dpdrot, dpdt, dpdf, dpdc, dpddist,
1048                 valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist;
1049
1050         project( objPoints, rvec, tvec, cameraMatrix, distCoeffs,
1051                 imgPoints, dpdrot, dpdt, dpdf, dpdc, dpddist, 0 );
1052
1053     // calculate and check image points
1054     assert( (int)imgPoints.size() == pointCount );
1055         vector<Point2f>::const_iterator it = imgPoints.begin();
1056         for( int i = 0; i < pointCount; i++, ++it )
1057         {
1058             Point3d p( objPoints(i,0), objPoints(i,1), objPoints(i,2) );
1059             double z = p.x*rmat(2,0) + p.y*rmat(2,1) + p.z*rmat(2,2) + tvec(0,2),
1060                x = (p.x*rmat(0,0) + p.y*rmat(0,1) + p.z*rmat(0,2) + tvec(0,0)) / z,
1061                y = (p.x*rmat(1,0) + p.y*rmat(1,1) + p.z*rmat(1,2) + tvec(0,1)) / z,
1062                r2 = x*x + y*y,
1063                            r4 = r2*r2;
1064                 Point2f validImgPoint;
1065                 double a1 = 2*x*y,
1066                a2 = r2 + 2*x*x,
1067                a3 = r2 + 2*y*y,
1068                cdist = 1+distCoeffs(0,0)*r2+distCoeffs(0,1)*r4;
1069                 validImgPoint.x = static_cast<float>((double)cameraMatrix(0,0)*(x*cdist + (double)distCoeffs(0,2)*a1 + (double)distCoeffs(0,3)*a2)
1070             + (double)cameraMatrix(0,2));
1071                 validImgPoint.y = static_cast<float>((double)cameraMatrix(1,1)*(y*cdist + (double)distCoeffs(0,2)*a3 + distCoeffs(0,3)*a1)
1072             + (double)cameraMatrix(1,2));
1073
1074         if( fabs(it->x - validImgPoint.x) > imgPointErr ||
1075             fabs(it->y - validImgPoint.y) > imgPointErr )
1076                 {
1077                         ts->printf( cvtest::TS::LOG, "bad image point\n" );
1078                         code = cvtest::TS::FAIL_BAD_ACCURACY;
1079                         goto _exit_;
1080                 }
1081         }
1082
1083         // check derivatives
1084         // 1. rotation
1085         leftImgPoints.resize(3);
1086     rightImgPoints.resize(3);
1087         for( int i = 0; i < 3; i++ )
1088         {
1089         rvec.copyTo( leftRvec ); leftRvec(0,i) -= dEps;
1090         project( objPoints, leftRvec, tvec, cameraMatrix, distCoeffs,
1091             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1092         rvec.copyTo( rightRvec ); rightRvec(0,i) += dEps;
1093         project( objPoints, rightRvec, tvec, cameraMatrix, distCoeffs,
1094             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1095         }
1096     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdrot );
1097     err = norm( dpdrot, valDpdrot, NORM_INF );
1098     if( err > 3 )
1099         {
1100                 ts->printf( cvtest::TS::LOG, "bad dpdrot: too big difference = %g\n", err );
1101                 code = cvtest::TS::FAIL_BAD_ACCURACY;
1102         }
1103
1104     // 2. translation
1105     for( int i = 0; i < 3; i++ )
1106         {
1107         tvec.copyTo( leftTvec ); leftTvec(0,i) -= dEps;
1108         project( objPoints, rvec, leftTvec, cameraMatrix, distCoeffs,
1109             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1110         tvec.copyTo( rightTvec ); rightTvec(0,i) += dEps;
1111         project( objPoints, rvec, rightTvec, cameraMatrix, distCoeffs,
1112             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1113         }
1114     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdt );
1115     if( norm( dpdt, valDpdt, NORM_INF ) > 0.2 )
1116         {
1117                 ts->printf( cvtest::TS::LOG, "bad dpdtvec\n" );
1118                 code = cvtest::TS::FAIL_BAD_ACCURACY;
1119         }
1120
1121     // 3. camera matrix
1122     // 3.1. focus
1123     leftImgPoints.resize(2);
1124     rightImgPoints.resize(2);
1125     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,0) -= dEps;
1126     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1127         leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1128     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,1) -= dEps;
1129     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1130         leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1131     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,0) += dEps;
1132     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1133         rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1134     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,1) += dEps;
1135     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1136         rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1137     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdf );
1138     if ( norm( dpdf, valDpdf ) > 0.2 )
1139     {
1140         ts->printf( cvtest::TS::LOG, "bad dpdf\n" );
1141         code = cvtest::TS::FAIL_BAD_ACCURACY;
1142     }
1143     // 3.2. principal point
1144     leftImgPoints.resize(2);
1145     rightImgPoints.resize(2);
1146     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,2) -= dEps;
1147     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1148         leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1149     cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,2) -= dEps;
1150     project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
1151         leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1152     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,2) += dEps;
1153     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1154         rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1155     cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,2) += dEps;
1156     project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
1157         rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1158     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdc );
1159     if ( norm( dpdc, valDpdc ) > 0.2 )
1160     {
1161         ts->printf( cvtest::TS::LOG, "bad dpdc\n" );
1162         code = cvtest::TS::FAIL_BAD_ACCURACY;
1163     }
1164
1165     // 4. distortion
1166     leftImgPoints.resize(distCoeffs.cols);
1167     rightImgPoints.resize(distCoeffs.cols);
1168         for( int i = 0; i < distCoeffs.cols; i++ )
1169         {
1170         distCoeffs.copyTo( leftDistCoeffs ); leftDistCoeffs(0,i) -= dEps;
1171         project( objPoints, rvec, tvec, cameraMatrix, leftDistCoeffs,
1172             leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1173         distCoeffs.copyTo( rightDistCoeffs ); rightDistCoeffs(0,i) += dEps;
1174         project( objPoints, rvec, tvec, cameraMatrix, rightDistCoeffs,
1175             rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
1176         }
1177     calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpddist );
1178     if( norm( dpddist, valDpddist ) > 0.3 )
1179         {
1180                 ts->printf( cvtest::TS::LOG, "bad dpddist\n" );
1181                 code = cvtest::TS::FAIL_BAD_ACCURACY;
1182         }
1183
1184 _exit_:
1185         RNG& _rng = ts->get_rng();
1186         _rng = rng;
1187         ts->set_failed_test_info( code );
1188 }
1189
1190 //----------------------------------------- CV_ProjectPointsTest_C --------------------------------
1191 class CV_ProjectPointsTest_C : public CV_ProjectPointsTest
1192 {
1193 public:
1194         CV_ProjectPointsTest_C() {}
1195 protected:
1196         virtual void project( const Mat& objectPoints,
1197                 const Mat& rvec, const Mat& tvec,
1198                 const Mat& cameraMatrix,
1199                 const Mat& distCoeffs,
1200                 vector<Point2f>& imagePoints,
1201                 Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1202                 Mat& dpdc, Mat& dpddist,
1203                 double aspectRatio=0 );
1204 };
1205
1206 void CV_ProjectPointsTest_C::project( const Mat& opoints, const Mat& rvec, const Mat& tvec,
1207                                                                            const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& ipoints,
1208                                                                            Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio)
1209 {
1210     int npoints = opoints.cols*opoints.rows*opoints.channels()/3;
1211     ipoints.resize(npoints);
1212     dpdrot.create(npoints*2, 3, CV_64F);
1213     dpdt.create(npoints*2, 3, CV_64F);
1214     dpdf.create(npoints*2, 2, CV_64F);
1215     dpdc.create(npoints*2, 2, CV_64F);
1216     dpddist.create(npoints*2, distCoeffs.rows + distCoeffs.cols - 1, CV_64F);
1217     CvMat _objectPoints = opoints, _imagePoints = Mat(ipoints);
1218     CvMat _rvec = rvec, _tvec = tvec, _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
1219     CvMat _dpdrot = dpdrot, _dpdt = dpdt, _dpdf = dpdf, _dpdc = dpdc, _dpddist = dpddist;
1220
1221         cvProjectPoints2( &_objectPoints, &_rvec, &_tvec, &_cameraMatrix, &_distCoeffs,
1222                       &_imagePoints, &_dpdrot, &_dpdt, &_dpdf, &_dpdc, &_dpddist, aspectRatio );
1223 }
1224
1225
1226 //----------------------------------------- CV_ProjectPointsTest_CPP --------------------------------
1227 class CV_ProjectPointsTest_CPP : public CV_ProjectPointsTest
1228 {
1229 public:
1230         CV_ProjectPointsTest_CPP() {}
1231 protected:
1232         virtual void project( const Mat& objectPoints,
1233                 const Mat& rvec, const Mat& tvec,
1234                 const Mat& cameraMatrix,
1235                 const Mat& distCoeffs,
1236                 vector<Point2f>& imagePoints,
1237                 Mat& dpdrot, Mat& dpdt, Mat& dpdf,
1238                 Mat& dpdc, Mat& dpddist,
1239                 double aspectRatio=0 );
1240 };
1241
1242 void CV_ProjectPointsTest_CPP::project( const Mat& objectPoints, const Mat& rvec, const Mat& tvec,
1243                                                                            const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& imagePoints,
1244                                                                            Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio)
1245 {
1246     Mat J;
1247         projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, J, aspectRatio);
1248     J.colRange(0, 3).copyTo(dpdrot);
1249     J.colRange(3, 6).copyTo(dpdt);
1250     J.colRange(6, 8).copyTo(dpdf);
1251     J.colRange(8, 10).copyTo(dpdc);
1252     J.colRange(10, J.cols).copyTo(dpddist);
1253 }
1254
1255 ///////////////////////////////// Stereo Calibration /////////////////////////////////////
1256
1257 class CV_StereoCalibrationTest : public cvtest::BaseTest
1258 {
1259 public:
1260         CV_StereoCalibrationTest();
1261         ~CV_StereoCalibrationTest();
1262         void clear();
1263 protected:
1264         bool checkPandROI( int test_case_idx,
1265                 const Mat& M, const Mat& D, const Mat& R,
1266                 const Mat& P, Size imgsize, Rect roi );
1267
1268         // covers of tested functions
1269         virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1270                 const vector<vector<Point2f> >& imagePoints1,
1271                 const vector<vector<Point2f> >& imagePoints2,
1272                 Mat& cameraMatrix1, Mat& distCoeffs1,
1273                 Mat& cameraMatrix2, Mat& distCoeffs2,
1274                 Size imageSize, Mat& R, Mat& T,
1275                 Mat& E, Mat& F, TermCriteria criteria, int flags ) = 0;
1276         virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1277                 const Mat& cameraMatrix2, const Mat& distCoeffs2,
1278                 Size imageSize, const Mat& R, const Mat& T,
1279                 Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1280                 double alpha, Size newImageSize,
1281                 Rect* validPixROI1, Rect* validPixROI2, int flags ) = 0;
1282         virtual bool rectifyUncalibrated( const Mat& points1,
1283                 const Mat& points2, const Mat& F, Size imgSize,
1284                 Mat& H1, Mat& H2, double threshold=5 ) = 0;
1285
1286         void run(int);
1287 };
1288
1289
1290 CV_StereoCalibrationTest::CV_StereoCalibrationTest()
1291 {
1292 }
1293
1294
1295 CV_StereoCalibrationTest::~CV_StereoCalibrationTest()
1296 {
1297         clear();
1298 }
1299
1300 void CV_StereoCalibrationTest::clear()
1301 {
1302         cvtest::BaseTest::clear();
1303 }
1304
1305 bool CV_StereoCalibrationTest::checkPandROI( int test_case_idx, const Mat& M, const Mat& D, const Mat& R,
1306                                                                                         const Mat& P, Size imgsize, Rect roi )
1307 {
1308         const double eps = 0.05;
1309         const int N = 21;
1310         int x, y, k;
1311         vector<Point2f> pts, upts;
1312
1313         // step 1. check that all the original points belong to the destination image
1314         for( y = 0; y < N; y++ )
1315                 for( x = 0; x < N; x++ )
1316                         pts.push_back(Point2f((float)x*imgsize.width/(N-1), (float)y*imgsize.height/(N-1)));
1317
1318         undistortPoints(Mat(pts), upts, M, D, R, P );
1319         for( k = 0; k < N*N; k++ )
1320                 if( upts[k].x < -imgsize.width*eps || upts[k].x > imgsize.width*(1+eps) ||
1321                         upts[k].y < -imgsize.height*eps || upts[k].y > imgsize.height*(1+eps) )
1322                 {
1323                         ts->printf(cvtest::TS::LOG, "Test #%d. The point (%g, %g) was mapped to (%g, %g) which is out of image\n",
1324                                 test_case_idx, pts[k].x, pts[k].y, upts[k].x, upts[k].y);
1325                         return false;
1326                 }
1327
1328                 // step 2. check that all the points inside ROI belong to the original source image
1329                 Mat temp(imgsize, CV_8U), utemp, map1, map2;
1330                 temp = Scalar::all(1);
1331                 initUndistortRectifyMap(M, D, R, P, imgsize, CV_16SC2, map1, map2);
1332                 remap(temp, utemp, map1, map2, INTER_LINEAR);
1333
1334                 if(roi.x < 0 || roi.y < 0 || roi.x + roi.width > imgsize.width || roi.y + roi.height > imgsize.height)
1335                 {
1336                         ts->printf(cvtest::TS::LOG, "Test #%d. The ROI=(%d, %d, %d, %d) is outside of the imge rectangle\n",
1337                                 test_case_idx, roi.x, roi.y, roi.width, roi.height);
1338                         return false;
1339                 }
1340                 double s = sum(utemp(roi))[0];
1341                 if( s > roi.area() || roi.area() - s > roi.area()*(1-eps) )
1342                 {
1343                         ts->printf(cvtest::TS::LOG, "Test #%d. The ratio of black pixels inside the valid ROI (~%g%%) is too large\n",
1344                                 test_case_idx, s*100./roi.area());
1345                         return false;
1346                 }
1347
1348                 return true;
1349 }
1350
1351 void CV_StereoCalibrationTest::run( int )
1352 {
1353         const int ntests = 1;
1354         const double maxReprojErr = 2;
1355         const double maxScanlineDistErr_c = 3;
1356         const double maxScanlineDistErr_uc = 4;
1357         FILE* f = 0;
1358
1359         for(int testcase = 1; testcase <= ntests; testcase++)
1360         {
1361                 char filepath[1000];
1362                 char buf[1000];
1363                 sprintf( filepath, "%sstereo/case%d/stereo_calib.txt", ts->get_data_path().c_str(), testcase );
1364                 f = fopen(filepath, "rt");
1365                 Size patternSize;
1366                 vector<string> imglist;
1367
1368                 if( !f || !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &patternSize.width, &patternSize.height) != 2 )
1369                 {
1370                         ts->printf( cvtest::TS::LOG, "The file %s can not be opened or has invalid content\n", filepath );
1371                         ts->set_failed_test_info( f ? cvtest::TS::FAIL_INVALID_TEST_DATA : cvtest::TS::FAIL_MISSING_TEST_DATA );
1372                         return;
1373                 }
1374
1375                 for(;;)
1376                 {
1377                         if( !fgets( buf, sizeof(buf)-3, f ))
1378                                 break;
1379                         size_t len = strlen(buf);
1380                         while( len > 0 && isspace(buf[len-1]))
1381                                 buf[--len] = '\0';
1382                         if( buf[0] == '#')
1383                                 continue;
1384                         sprintf(filepath, "%sstereo/case%d/%s", ts->get_data_path().c_str(), testcase, buf );
1385                         imglist.push_back(string(filepath));
1386                 }
1387                 fclose(f);
1388
1389                 if( imglist.size() == 0 || imglist.size() % 2 != 0 )
1390                 {
1391                         ts->printf( cvtest::TS::LOG, "The number of images is 0 or an odd number in the case #%d\n", testcase );
1392                         ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
1393                         return;
1394                 }
1395
1396                 int nframes = (int)(imglist.size()/2);
1397                 int npoints = patternSize.width*patternSize.height;
1398                 vector<vector<Point3f> > objpt(nframes);
1399                 vector<vector<Point2f> > imgpt1(nframes);
1400                 vector<vector<Point2f> > imgpt2(nframes);
1401                 Size imgsize;
1402                 int total = 0;
1403
1404                 for( int i = 0; i < nframes; i++ )
1405                 {
1406                         Mat left = imread(imglist[i*2]);
1407                         Mat right = imread(imglist[i*2+1]);
1408                         if(!left.data || !right.data)
1409                         {
1410                                 ts->printf( cvtest::TS::LOG, "Can not load images %s and %s, testcase %d\n",
1411                                         imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
1412                                 ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
1413                                 return;
1414                         }
1415                         imgsize = left.size();
1416                         bool found1 = findChessboardCorners(left, patternSize, imgpt1[i]);
1417                         bool found2 = findChessboardCorners(right, patternSize, imgpt2[i]);
1418                         if(!found1 || !found2)
1419                         {
1420                                 ts->printf( cvtest::TS::LOG, "The function could not detect boards on the images %s and %s, testcase %d\n",
1421                                         imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
1422                                 ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1423                                 return;
1424                         }
1425                         total += (int)imgpt1[i].size();
1426                         for( int j = 0; j < npoints; j++ )
1427                                 objpt[i].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f));
1428                 }
1429
1430                 // rectify (calibrated)
1431                 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;
1432                 M1.at<double>(0,2) = M2.at<double>(0,2)=(imgsize.width-1)*0.5;
1433                 M1.at<double>(1,2) = M2.at<double>(1,2)=(imgsize.height-1)*0.5;
1434                 D1 = Scalar::all(0);
1435                 D2 = Scalar::all(0);
1436                 double err = calibrateStereoCamera(objpt, imgpt1, imgpt2, M1, D1, M2, D2, imgsize, R, T, E, F,
1437                         TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 1e-6),
1438                         CV_CALIB_SAME_FOCAL_LENGTH
1439                         //+ CV_CALIB_FIX_ASPECT_RATIO
1440                         + CV_CALIB_FIX_PRINCIPAL_POINT
1441                         + CV_CALIB_ZERO_TANGENT_DIST
1442             + CV_CALIB_FIX_K3
1443             + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5 //+ CV_CALIB_FIX_K6
1444                         );
1445                 err /= nframes*npoints;
1446                 if( err > maxReprojErr )
1447                 {
1448                         ts->printf( cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n", err, testcase);
1449                         ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1450                         return;
1451                 }
1452
1453                 Mat R1, R2, P1, P2, Q;
1454                 Rect roi1, roi2;
1455                 rectify(M1, D1, M2, D2, imgsize, R, T, R1, R2, P1, P2, Q, 1, imgsize, &roi1, &roi2, 0);
1456                 Mat eye33 = Mat::eye(3,3,CV_64F);
1457                 Mat R1t = R1.t(), R2t = R2.t();
1458
1459                 if( norm(R1t*R1 - eye33) > 0.01 ||
1460                         norm(R2t*R2 - eye33) > 0.01 ||
1461                         abs(determinant(F)) > 0.01)
1462                 {
1463                         ts->printf( cvtest::TS::LOG, "The computed (by rectify) R1 and R2 are not orthogonal,"
1464                                 "or the computed (by calibrate) F is not singular, testcase %d\n", testcase);
1465                         ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
1466                         return;
1467                 }
1468
1469                 if(!checkPandROI(testcase, M1, D1, R1, P1, imgsize, roi1))
1470                 {
1471                         ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1472                         return;
1473                 }
1474
1475                 if(!checkPandROI(testcase, M2, D2, R2, P2, imgsize, roi2))
1476                 {
1477                         ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1478                         return;
1479                 }
1480
1481                 // rectifyUncalibrated
1482                 CV_Assert( imgpt1.size() == imgpt2.size() );
1483                 Mat _imgpt1( total, 1, CV_32FC2 ), _imgpt2( total, 1, CV_32FC2 );
1484                 vector<vector<Point2f> >::const_iterator iit1 = imgpt1.begin();
1485                 vector<vector<Point2f> >::const_iterator iit2 = imgpt2.begin();
1486                 for( int pi = 0; iit1 != imgpt1.end(); ++iit1, ++iit2 )
1487                 {
1488                         vector<Point2f>::const_iterator pit1 = iit1->begin();
1489                         vector<Point2f>::const_iterator pit2 = iit2->begin();
1490                         CV_Assert( iit1->size() == iit2->size() );
1491                         for( ; pit1 != iit1->end(); ++pit1, ++pit2, pi++ )
1492                         {
1493                                 _imgpt1.at<Point2f>(pi,0) = Point2f( pit1->x, pit1->y );
1494                                 _imgpt2.at<Point2f>(pi,0) = Point2f( pit2->x, pit2->y );
1495                         }
1496                 }
1497
1498                 Mat _M1, _M2, _D1, _D2;
1499                 vector<Mat> _R1, _R2, _T1, _T2;
1500                 calibrateCamera( objpt, imgpt1, imgsize, _M1, _D1, _R1, _T1, 0 );
1501                 calibrateCamera( objpt, imgpt2, imgsize, _M2, _D2, _R2, _T1, 0 );
1502                 undistortPoints( _imgpt1, _imgpt1, _M1, _D1, Mat(), _M1 );
1503                 undistortPoints( _imgpt2, _imgpt2, _M2, _D2, Mat(), _M2 );
1504
1505                 Mat matF, _H1, _H2;
1506                 matF = findFundamentalMat( _imgpt1, _imgpt2 );
1507                 rectifyUncalibrated( _imgpt1, _imgpt2, matF, imgsize, _H1, _H2 );
1508
1509                 Mat rectifPoints1, rectifPoints2;
1510                 perspectiveTransform( _imgpt1, rectifPoints1, _H1 );
1511                 perspectiveTransform( _imgpt2, rectifPoints2, _H2 );
1512
1513                 bool verticalStereo = abs(P2.at<double>(0,3)) < abs(P2.at<double>(1,3));
1514                 double maxDiff_c = 0, maxDiff_uc = 0;
1515                 for( int i = 0, k = 0; i < nframes; i++ )
1516                 {
1517                         vector<Point2f> temp[2];
1518                         undistortPoints(Mat(imgpt1[i]), temp[0], M1, D1, R1, P1);
1519                         undistortPoints(Mat(imgpt2[i]), temp[1], M2, D2, R2, P2);
1520
1521                         for( int j = 0; j < npoints; j++, k++ )
1522                         {
1523                                 double diff_c = verticalStereo ? abs(temp[0][j].x - temp[1][j].x) : abs(temp[0][j].y - temp[1][j].y);
1524                                 Point2f d = rectifPoints1.at<Point2f>(k,0) - rectifPoints2.at<Point2f>(k,0);
1525                                 double diff_uc = verticalStereo ? abs(d.x) : abs(d.y);
1526                                 maxDiff_c = max(maxDiff_c, diff_c);
1527                                 maxDiff_uc = max(maxDiff_uc, diff_uc);
1528                                 if( maxDiff_c > maxScanlineDistErr_c )
1529                                 {
1530                                         ts->printf( cvtest::TS::LOG, "The distance between %s coordinates is too big(=%g) (used calibrated stereo), testcase %d\n",
1531                                                 verticalStereo ? "x" : "y", diff_c, testcase);
1532                                         ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1533                                         return;
1534                                 }
1535                                 if( maxDiff_uc > maxScanlineDistErr_uc )
1536                                 {
1537                                         ts->printf( cvtest::TS::LOG, "The distance between %s coordinates is too big(=%g) (used uncalibrated stereo), testcase %d\n",
1538                                                 verticalStereo ? "x" : "y", diff_uc, testcase);
1539                                         ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
1540                                         return;
1541                                 }
1542                         }
1543                 }
1544
1545                 ts->printf( cvtest::TS::LOG, "Testcase %d. Max distance (calibrated) =%g\n"
1546                         "Max distance (uncalibrated) =%g\n", testcase, maxDiff_c, maxDiff_uc );
1547         }
1548 }
1549
1550 //-------------------------------- CV_StereoCalibrationTest_C ------------------------------
1551
1552 class CV_StereoCalibrationTest_C : public CV_StereoCalibrationTest
1553 {
1554 public:
1555         CV_StereoCalibrationTest_C() {}
1556 protected:
1557         virtual double calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1558                 const vector<vector<Point2f> >& imagePoints1,
1559                 const vector<vector<Point2f> >& imagePoints2,
1560                 Mat& cameraMatrix1, Mat& distCoeffs1,
1561                 Mat& cameraMatrix2, Mat& distCoeffs2,
1562                 Size imageSize, Mat& R, Mat& T,
1563                 Mat& E, Mat& F, TermCriteria criteria, int flags );
1564         virtual void rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1565                 const Mat& cameraMatrix2, const Mat& distCoeffs2,
1566                 Size imageSize, const Mat& R, const Mat& T,
1567                 Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1568                 double alpha, Size newImageSize,
1569                 Rect* validPixROI1, Rect* validPixROI2, int flags );
1570         virtual bool rectifyUncalibrated( const Mat& points1,
1571                 const Mat& points2, const Mat& F, Size imgSize,
1572                 Mat& H1, Mat& H2, double threshold=5 );
1573 };
1574
1575 double CV_StereoCalibrationTest_C::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1576                                  const vector<vector<Point2f> >& imagePoints1,
1577                                  const vector<vector<Point2f> >& imagePoints2,
1578                                  Mat& cameraMatrix1, Mat& distCoeffs1,
1579                                  Mat& cameraMatrix2, Mat& distCoeffs2,
1580                                  Size imageSize, Mat& R, Mat& T,
1581                                  Mat& E, Mat& F, TermCriteria criteria, int flags )
1582 {
1583         cameraMatrix1.create( 3, 3, CV_64F );
1584         cameraMatrix2.create( 3, 3, CV_64F);
1585         distCoeffs1.create( 1, 5, CV_64F);
1586         distCoeffs2.create( 1, 5, CV_64F);
1587         R.create(3, 3, CV_64F);
1588         T.create(3, 1, CV_64F);
1589         E.create(3, 3, CV_64F);
1590         F.create(3, 3, CV_64F);
1591
1592         int  nimages = (int)objectPoints.size(), total = 0;
1593         for( int i = 0; i < nimages; i++ )
1594         {
1595                 total += (int)objectPoints[i].size();
1596         }
1597
1598         Mat npoints( 1, nimages, CV_32S ),
1599                 objPt( 1, total, DataType<Point3f>::type ),
1600                 imgPt( 1, total, DataType<Point2f>::type ),
1601                 imgPt2( 1, total, DataType<Point2f>::type );
1602
1603         Point2f* imgPtData2 = imgPt2.ptr<Point2f>();
1604         Point3f* objPtData = objPt.ptr<Point3f>();
1605         Point2f* imgPtData = imgPt.ptr<Point2f>();
1606         for( int i = 0, ni = 0, j = 0; i < nimages; i++, j += ni )
1607         {
1608                 ni = (int)objectPoints[i].size();
1609                 ((int*)npoints.data)[i] = ni;
1610                 std::copy(objectPoints[i].begin(), objectPoints[i].end(), objPtData + j);
1611                 std::copy(imagePoints1[i].begin(), imagePoints1[i].end(), imgPtData + j);
1612                 std::copy(imagePoints2[i].begin(), imagePoints2[i].end(), imgPtData2 + j);
1613         }
1614         CvMat _objPt = objPt, _imgPt = imgPt, _imgPt2 = imgPt2, _npoints = npoints;
1615         CvMat _cameraMatrix1 = cameraMatrix1, _distCoeffs1 = distCoeffs1;
1616         CvMat _cameraMatrix2 = cameraMatrix2, _distCoeffs2 = distCoeffs2;
1617         CvMat matR = R, matT = T, matE = E, matF = F;
1618
1619         return cvStereoCalibrate(&_objPt, &_imgPt, &_imgPt2, &_npoints, &_cameraMatrix1,
1620                 &_distCoeffs1, &_cameraMatrix2, &_distCoeffs2, imageSize,
1621                 &matR, &matT, &matE, &matF, criteria, flags );
1622 }
1623
1624 void CV_StereoCalibrationTest_C::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1625                          const Mat& cameraMatrix2, const Mat& distCoeffs2,
1626                          Size imageSize, const Mat& R, const Mat& T,
1627                          Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1628                          double alpha, Size newImageSize,
1629                          Rect* validPixROI1, Rect* validPixROI2, int flags )
1630 {
1631         int rtype = CV_64F;
1632         R1.create(3, 3, rtype);
1633         R2.create(3, 3, rtype);
1634         P1.create(3, 4, rtype);
1635         P2.create(3, 4, rtype);
1636         Q.create(4, 4, rtype);
1637         CvMat _cameraMatrix1 = cameraMatrix1, _distCoeffs1 = distCoeffs1;
1638         CvMat _cameraMatrix2 = cameraMatrix2, _distCoeffs2 = distCoeffs2;
1639         CvMat matR = R, matT = T, _R1 = R1, _R2 = R2, _P1 = P1, _P2 = P2, matQ = Q;
1640         cvStereoRectify( &_cameraMatrix1, &_cameraMatrix2, &_distCoeffs1, &_distCoeffs2,
1641                 imageSize, &matR, &matT, &_R1, &_R2, &_P1, &_P2, &matQ, flags,
1642                 alpha, newImageSize, (CvRect*)validPixROI1, (CvRect*)validPixROI2);
1643 }
1644
1645 bool CV_StereoCalibrationTest_C::rectifyUncalibrated( const Mat& points1,
1646                    const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold )
1647 {
1648         H1.create(3, 3, CV_64F);
1649         H2.create(3, 3, CV_64F);
1650         CvMat _pt1 = points1, _pt2 = points2, matF, *pF=0, _H1 = H1, _H2 = H2;
1651         if( F.size() == Size(3, 3) )
1652                 pF = &(matF = F);
1653         return cvStereoRectifyUncalibrated(&_pt1, &_pt2, pF, imgSize, &_H1, &_H2, threshold) > 0;
1654 }
1655
1656
1657 //-------------------------------- CV_StereoCalibrationTest_CPP ------------------------------
1658
1659 class CV_StereoCalibrationTest_CPP : public CV_StereoCalibrationTest
1660 {
1661 public:
1662         CV_StereoCalibrationTest_CPP() {}
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 };
1681
1682 double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
1683                                                                                          const vector<vector<Point2f> >& imagePoints1,
1684                                                                                          const vector<vector<Point2f> >& imagePoints2,
1685                                                                                          Mat& cameraMatrix1, Mat& distCoeffs1,
1686                                                                                          Mat& cameraMatrix2, Mat& distCoeffs2,
1687                                                                                          Size imageSize, Mat& R, Mat& T,
1688                                                                                          Mat& E, Mat& F, TermCriteria criteria, int flags )
1689 {
1690         return stereoCalibrate( objectPoints, imagePoints1, imagePoints2,
1691                                         cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
1692                                         imageSize, R, T, E, F, criteria, flags );
1693 }
1694
1695 void CV_StereoCalibrationTest_CPP::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
1696                                                                                  const Mat& cameraMatrix2, const Mat& distCoeffs2,
1697                                                                                  Size imageSize, const Mat& R, const Mat& T,
1698                                                                                  Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
1699                                                                                  double alpha, Size newImageSize,
1700                                                                                  Rect* validPixROI1, Rect* validPixROI2, int flags )
1701 {
1702         stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
1703                                 imageSize, R, T, R1, R2, P1, P2, Q, flags, alpha, newImageSize,validPixROI1, validPixROI2 );
1704 }
1705
1706 bool CV_StereoCalibrationTest_CPP::rectifyUncalibrated( const Mat& points1,
1707                                            const Mat& points2, const Mat& F, Size imgSize, Mat& H1, Mat& H2, double threshold )
1708 {
1709         return stereoRectifyUncalibrated( points1, points2, F, imgSize, H1, H2, threshold );
1710 }
1711
1712 ///////////////////////////////////////////////////////////////////////////////////////////////////
1713
1714 TEST(Calib3d_CalibrateCamera_C, regression) { CV_CameraCalibrationTest_C test; test.safe_run(); }
1715 TEST(Calib3d_CalibrateCamera_CPP, regression) { CV_CameraCalibrationTest_CPP test; test.safe_run(); }
1716 TEST(Calib3d_CalibrationMatrixValues_C, accuracy) { CV_CalibrationMatrixValuesTest_C test; test.safe_run(); }
1717 TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); }
1718 TEST(Calib3d_ProjectPoints_C, accuracy) { CV_ProjectPointsTest_C  test; test.safe_run(); }
1719 TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); }
1720 TEST(Calib3d_StereoCalibrate_C, regression) { CV_StereoCalibrationTest_C test; test.safe_run(); }
1721 TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); }