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