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
CV_CameraCalibrationTest()270 CV_CameraCalibrationTest::CV_CameraCalibrationTest()
271 {
272 }
273
~CV_CameraCalibrationTest()274 CV_CameraCalibrationTest::~CV_CameraCalibrationTest()
275 {
276 clear();
277 }
278
clear()279 void CV_CameraCalibrationTest::clear()
280 {
281 cvtest::BaseTest::clear();
282 }
283
compare(double * val,double * ref_val,int len,double eps,const char * param_name)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
run(int start_from)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:
CV_CameraCalibrationTest_C()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
calibrate(int imageCount,int * pointCounts,CvSize imageSize,CvPoint2D64f * imagePoints,CvPoint3D64f * objectPoints,double * distortionCoeffs,double * cameraMatrix,double * translationVectors,double * rotationMatrices,int flags)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
project(int pointCount,CvPoint3D64f * objectPoints,double * rotationMatrix,double * translationVector,double * cameraMatrix,double * distortion,CvPoint2D64f * imagePoints)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:
CV_CameraCalibrationTest_CPP()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
calibrate(int imageCount,int * pointCounts,CvSize _imageSize,CvPoint2D64f * _imagePoints,CvPoint3D64f * _objectPoints,double * _distortionCoeffs,double * _cameraMatrix,double * translationVectors,double * rotationMatrices,int flags)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
project(int pointCount,CvPoint3D64f * _objectPoints,double * rotationMatrix,double * translationVector,double * _cameraMatrix,double * distortion,CvPoint2D64f * _imagePoints)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:
CV_CalibrationMatrixValuesTest()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
run(int)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:
CV_CalibrationMatrixValuesTest_C()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
calibMatrixValues(const Mat & _cameraMatrix,Size imageSize,double apertureWidth,double apertureHeight,double & fovx,double & fovy,double & focalLength,Point2d & principalPoint,double & aspectRatio)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:
CV_CalibrationMatrixValuesTest_CPP()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
calibMatrixValues(const Mat & cameraMatrix,Size imageSize,double apertureWidth,double apertureHeight,double & fovx,double & fovy,double & focalLength,Point2d & principalPoint,double & aspectRatio)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 --------------------------------
calcdfdx(const vector<vector<Point2f>> & leftF,const vector<vector<Point2f>> & rightF,double eps,Mat & dfdx)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:
CV_ProjectPointsTest()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
run(int)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:
CV_ProjectPointsTest_C()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
project(const Mat & opoints,const Mat & rvec,const Mat & tvec,const Mat & cameraMatrix,const Mat & distCoeffs,vector<Point2f> & ipoints,Mat & dpdrot,Mat & dpdt,Mat & dpdf,Mat & dpdc,Mat & dpddist,double aspectRatio)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:
CV_ProjectPointsTest_CPP()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
project(const Mat & objectPoints,const Mat & rvec,const Mat & tvec,const Mat & cameraMatrix,const Mat & distCoeffs,vector<Point2f> & imagePoints,Mat & dpdrot,Mat & dpdt,Mat & dpdf,Mat & dpdc,Mat & dpddist,double aspectRatio)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
CV_StereoCalibrationTest()1314 CV_StereoCalibrationTest::CV_StereoCalibrationTest()
1315 {
1316 }
1317
1318
~CV_StereoCalibrationTest()1319 CV_StereoCalibrationTest::~CV_StereoCalibrationTest()
1320 {
1321 clear();
1322 }
1323
clear()1324 void CV_StereoCalibrationTest::clear()
1325 {
1326 cvtest::BaseTest::clear();
1327 }
1328
checkPandROI(int test_case_idx,const Mat & M,const Mat & D,const Mat & R,const Mat & P,Size imgsize,Rect roi)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
run(int)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:
CV_StereoCalibrationTest_C()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
calibrateStereoCamera(const vector<vector<Point3f>> & objectPoints,const vector<vector<Point2f>> & imagePoints1,const vector<vector<Point2f>> & imagePoints2,Mat & cameraMatrix1,Mat & distCoeffs1,Mat & cameraMatrix2,Mat & distCoeffs2,Size imageSize,Mat & R,Mat & T,Mat & E,Mat & F,TermCriteria criteria,int flags)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
rectify(const Mat & cameraMatrix1,const Mat & distCoeffs1,const Mat & cameraMatrix2,const Mat & distCoeffs2,Size imageSize,const Mat & R,const Mat & T,Mat & R1,Mat & R2,Mat & P1,Mat & P2,Mat & Q,double alpha,Size newImageSize,Rect * validPixROI1,Rect * validPixROI2,int flags)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
rectifyUncalibrated(const Mat & points1,const Mat & points2,const Mat & F,Size imgSize,Mat & H1,Mat & H2,double threshold)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
triangulate(const Mat & P1,const Mat & P2,const Mat & points1,const Mat & points2,Mat & points4D)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
correct(const Mat & F,const Mat & points1,const Mat & points2,Mat & newPoints1,Mat & newPoints2)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:
CV_StereoCalibrationTest_CPP()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
calibrateStereoCamera(const vector<vector<Point3f>> & objectPoints,const vector<vector<Point2f>> & imagePoints1,const vector<vector<Point2f>> & imagePoints2,Mat & cameraMatrix1,Mat & distCoeffs1,Mat & cameraMatrix2,Mat & distCoeffs2,Size imageSize,Mat & R,Mat & T,Mat & E,Mat & F,TermCriteria criteria,int flags)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
rectify(const Mat & cameraMatrix1,const Mat & distCoeffs1,const Mat & cameraMatrix2,const Mat & distCoeffs2,Size imageSize,const Mat & R,const Mat & T,Mat & R1,Mat & R2,Mat & P1,Mat & P2,Mat & Q,double alpha,Size newImageSize,Rect * validPixROI1,Rect * validPixROI2,int flags)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
rectifyUncalibrated(const Mat & points1,const Mat & points2,const Mat & F,Size imgSize,Mat & H1,Mat & H2,double threshold)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
triangulate(const Mat & P1,const Mat & P2,const Mat & points1,const Mat & points2,Mat & points4D)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
correct(const Mat & F,const Mat & points1,const Mat & points2,Mat & newPoints1,Mat & newPoints2)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
TEST(Calib3d_CalibrateCamera_C,regression)1870 TEST(Calib3d_CalibrateCamera_C, regression) { CV_CameraCalibrationTest_C test; test.safe_run(); }
TEST(Calib3d_CalibrateCamera_CPP,regression)1871 TEST(Calib3d_CalibrateCamera_CPP, regression) { CV_CameraCalibrationTest_CPP test; test.safe_run(); }
TEST(Calib3d_CalibrationMatrixValues_C,accuracy)1872 TEST(Calib3d_CalibrationMatrixValues_C, accuracy) { CV_CalibrationMatrixValuesTest_C test; test.safe_run(); }
TEST(Calib3d_CalibrationMatrixValues_CPP,accuracy)1873 TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); }
TEST(Calib3d_ProjectPoints_C,accuracy)1874 TEST(Calib3d_ProjectPoints_C, accuracy) { CV_ProjectPointsTest_C test; test.safe_run(); }
TEST(Calib3d_ProjectPoints_CPP,regression)1875 TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); }
TEST(Calib3d_StereoCalibrate_C,regression)1876 TEST(Calib3d_StereoCalibrate_C, regression) { CV_StereoCalibrationTest_C test; test.safe_run(); }
TEST(Calib3d_StereoCalibrate_CPP,regression)1877 TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); }
1878
1879
TEST(Calib3d_Triangulate,accuracy)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