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 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #include "test_precomp.hpp" 44 #include "opencv2/imgproc/imgproc_c.h" 45 46 using namespace cv; 47 using namespace std; 48 49 class CV_DefaultNewCameraMatrixTest : public cvtest::ArrayTest 50 { 51 public: 52 CV_DefaultNewCameraMatrixTest(); 53 protected: 54 int prepare_test_case (int test_case_idx); 55 void prepare_to_validation( int test_case_idx ); 56 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 57 void run_func(); 58 59 private: 60 cv::Size img_size; 61 cv::Mat camera_mat; 62 cv::Mat new_camera_mat; 63 64 int matrix_type; 65 66 bool center_principal_point; 67 68 static const int MAX_X = 2048; 69 static const int MAX_Y = 2048; 70 static const int MAX_VAL = 10000; 71 }; 72 73 CV_DefaultNewCameraMatrixTest::CV_DefaultNewCameraMatrixTest() 74 { 75 test_array[INPUT].push_back(NULL); 76 test_array[OUTPUT].push_back(NULL); 77 test_array[REF_OUTPUT].push_back(NULL); 78 79 matrix_type = 0; 80 center_principal_point = false; 81 } 82 83 void CV_DefaultNewCameraMatrixTest::get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ) 84 { 85 cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types); 86 RNG& rng = ts->get_rng(); 87 matrix_type = types[INPUT][0] = types[OUTPUT][0]= types[REF_OUTPUT][0] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 88 sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,3); 89 } 90 91 int CV_DefaultNewCameraMatrixTest::prepare_test_case(int test_case_idx) 92 { 93 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); 94 95 if (code <= 0) 96 return code; 97 98 RNG& rng = ts->get_rng(); 99 100 img_size.width = cvtest::randInt(rng) % MAX_X + 1; 101 img_size.height = cvtest::randInt(rng) % MAX_Y + 1; 102 103 center_principal_point = ((cvtest::randInt(rng) % 2)!=0); 104 105 // Generating camera_mat matrix 106 double sz = MAX(img_size.width, img_size.height); 107 double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7; 108 double a[9] = {0,0,0,0,0,0,0,0,1}; 109 Mat _a(3,3,CV_64F,a); 110 a[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 111 a[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 112 a[0] = sz/(0.9 - cvtest::randReal(rng)*0.6); 113 a[4] = aspect_ratio*a[0]; 114 115 Mat& _a0 = test_mat[INPUT][0]; 116 cvtest::convert(_a, _a0, _a0.type()); 117 camera_mat = _a0; 118 119 return code; 120 121 } 122 123 void CV_DefaultNewCameraMatrixTest::run_func() 124 { 125 new_camera_mat = cv::getDefaultNewCameraMatrix(camera_mat,img_size,center_principal_point); 126 } 127 128 void CV_DefaultNewCameraMatrixTest::prepare_to_validation( int /*test_case_idx*/ ) 129 { 130 const Mat& src = test_mat[INPUT][0]; 131 Mat& dst = test_mat[REF_OUTPUT][0]; 132 Mat& test_output = test_mat[OUTPUT][0]; 133 Mat& output = new_camera_mat; 134 cvtest::convert( output, test_output, test_output.type() ); 135 if (!center_principal_point) 136 { 137 cvtest::copy(src, dst); 138 } 139 else 140 { 141 double a[9] = {0,0,0,0,0,0,0,0,1}; 142 Mat _a(3,3,CV_64F,a); 143 if (matrix_type == CV_64F) 144 { 145 a[0] = src.at<double>(0,0); 146 a[4] = src.at<double>(1,1); 147 } 148 else 149 { 150 a[0] = src.at<float>(0,0); 151 a[4] = src.at<float>(1,1); 152 } 153 a[2] = (img_size.width - 1)*0.5; 154 a[5] = (img_size.height - 1)*0.5; 155 cvtest::convert( _a, dst, dst.type() ); 156 } 157 } 158 159 //--------- 160 161 class CV_UndistortPointsTest : public cvtest::ArrayTest 162 { 163 public: 164 CV_UndistortPointsTest(); 165 protected: 166 int prepare_test_case (int test_case_idx); 167 void prepare_to_validation( int test_case_idx ); 168 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 169 double get_success_error_level( int test_case_idx, int i, int j ); 170 void run_func(); 171 void distortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, 172 const CvMat* _distCoeffs, const CvMat* matR, const CvMat* matP); 173 174 private: 175 bool useCPlus; 176 bool useDstMat; 177 static const int N_POINTS = 10; 178 static const int MAX_X = 2048; 179 static const int MAX_Y = 2048; 180 181 bool zero_new_cam; 182 bool zero_distortion; 183 bool zero_R; 184 185 cv::Size img_size; 186 cv::Mat dst_points_mat; 187 188 cv::Mat camera_mat; 189 cv::Mat R; 190 cv::Mat P; 191 cv::Mat distortion_coeffs; 192 cv::Mat src_points; 193 std::vector<cv::Point2f> dst_points; 194 }; 195 196 CV_UndistortPointsTest::CV_UndistortPointsTest() 197 { 198 test_array[INPUT].push_back(NULL); // points matrix 199 test_array[INPUT].push_back(NULL); // camera matrix 200 test_array[INPUT].push_back(NULL); // distortion coeffs 201 test_array[INPUT].push_back(NULL); // R matrix 202 test_array[INPUT].push_back(NULL); // P matrix 203 test_array[OUTPUT].push_back(NULL); // distorted dst points 204 test_array[TEMP].push_back(NULL); // dst points 205 test_array[REF_OUTPUT].push_back(NULL); 206 207 useCPlus = useDstMat = false; 208 zero_new_cam = zero_distortion = zero_R = false; 209 } 210 211 void CV_UndistortPointsTest::get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ) 212 { 213 cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types); 214 RNG& rng = ts->get_rng(); 215 useCPlus = ((cvtest::randInt(rng) % 2)!=0); 216 //useCPlus = 0; 217 if (useCPlus) 218 { 219 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = types[TEMP][0]= CV_32FC2; 220 } 221 else 222 { 223 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = types[TEMP][0]= cvtest::randInt(rng)%2 ? CV_64FC2 : CV_32FC2; 224 } 225 types[INPUT][1] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 226 types[INPUT][2] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 227 types[INPUT][3] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 228 types[INPUT][4] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 229 230 sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = sizes[TEMP][0]= cvtest::randInt(rng)%2 ? cvSize(1,N_POINTS) : cvSize(N_POINTS,1); 231 sizes[INPUT][1] = sizes[INPUT][3] = cvSize(3,3); 232 sizes[INPUT][4] = cvtest::randInt(rng)%2 ? cvSize(3,3) : cvSize(4,3); 233 234 if (cvtest::randInt(rng)%2) 235 { 236 if (cvtest::randInt(rng)%2) 237 { 238 sizes[INPUT][2] = cvSize(1,4); 239 } 240 else 241 { 242 sizes[INPUT][2] = cvSize(1,5); 243 } 244 } 245 else 246 { 247 if (cvtest::randInt(rng)%2) 248 { 249 sizes[INPUT][2] = cvSize(4,1); 250 } 251 else 252 { 253 sizes[INPUT][2] = cvSize(5,1); 254 } 255 } 256 } 257 258 int CV_UndistortPointsTest::prepare_test_case(int test_case_idx) 259 { 260 RNG& rng = ts->get_rng(); 261 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); 262 263 if (code <= 0) 264 return code; 265 266 useDstMat = (cvtest::randInt(rng) % 2) == 0; 267 268 img_size.width = cvtest::randInt(rng) % MAX_X + 1; 269 img_size.height = cvtest::randInt(rng) % MAX_Y + 1; 270 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows; 271 double cam[9] = {0,0,0,0,0,0,0,0,1}; 272 vector<double> dist(dist_size); 273 vector<double> proj(test_mat[INPUT][4].cols * test_mat[INPUT][4].rows); 274 vector<Point2d> points(N_POINTS); 275 276 Mat _camera(3,3,CV_64F,cam); 277 Mat _distort(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,&dist[0]); 278 Mat _proj(test_mat[INPUT][4].size(), CV_64F, &proj[0]); 279 Mat _points(test_mat[INPUT][0].size(), CV_64FC2, &points[0]); 280 281 _proj = Scalar::all(0); 282 283 //Generating points 284 for( int i = 0; i < N_POINTS; i++ ) 285 { 286 points[i].x = cvtest::randReal(rng)*img_size.width; 287 points[i].y = cvtest::randReal(rng)*img_size.height; 288 } 289 290 //Generating camera matrix 291 double sz = MAX(img_size.width,img_size.height); 292 double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7; 293 cam[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 294 cam[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 295 cam[0] = sz/(0.9 - cvtest::randReal(rng)*0.6); 296 cam[4] = aspect_ratio*cam[0]; 297 298 //Generating distortion coeffs 299 dist[0] = cvtest::randReal(rng)*0.06 - 0.03; 300 dist[1] = cvtest::randReal(rng)*0.06 - 0.03; 301 if( dist[0]*dist[1] > 0 ) 302 dist[1] = -dist[1]; 303 if( cvtest::randInt(rng)%4 != 0 ) 304 { 305 dist[2] = cvtest::randReal(rng)*0.004 - 0.002; 306 dist[3] = cvtest::randReal(rng)*0.004 - 0.002; 307 if (dist_size > 4) 308 dist[4] = cvtest::randReal(rng)*0.004 - 0.002; 309 } 310 else 311 { 312 dist[2] = dist[3] = 0; 313 if (dist_size > 4) 314 dist[4] = 0; 315 } 316 317 //Generating P matrix (projection) 318 if( test_mat[INPUT][4].cols != 4 ) 319 { 320 proj[8] = 1; 321 if (cvtest::randInt(rng)%2 == 0) // use identity new camera matrix 322 { 323 proj[0] = 1; 324 proj[4] = 1; 325 } 326 else 327 { 328 proj[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10% 329 proj[4] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10% 330 proj[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15% 331 proj[5] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15% 332 } 333 } 334 else 335 { 336 proj[10] = 1; 337 proj[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10% 338 proj[5] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10% 339 proj[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15% 340 proj[6] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15% 341 342 proj[3] = (img_size.height + img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 343 proj[7] = (img_size.height + img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 344 proj[11] = (img_size.height + img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 345 } 346 347 //Generating R matrix 348 Mat _rot(3,3,CV_64F); 349 Mat rotation(1,3,CV_64F); 350 rotation.at<double>(0) = CV_PI*(cvtest::randReal(rng) - (double)0.5); // phi 351 rotation.at<double>(1) = CV_PI*(cvtest::randReal(rng) - (double)0.5); // ksi 352 rotation.at<double>(2) = CV_PI*(cvtest::randReal(rng) - (double)0.5); //khi 353 cvtest::Rodrigues(rotation, _rot); 354 355 //copying data 356 //src_points = &_points; 357 _points.convertTo(test_mat[INPUT][0], test_mat[INPUT][0].type()); 358 _camera.convertTo(test_mat[INPUT][1], test_mat[INPUT][1].type()); 359 _distort.convertTo(test_mat[INPUT][2], test_mat[INPUT][2].type()); 360 _rot.convertTo(test_mat[INPUT][3], test_mat[INPUT][3].type()); 361 _proj.convertTo(test_mat[INPUT][4], test_mat[INPUT][4].type()); 362 363 zero_distortion = (cvtest::randInt(rng)%2) == 0 ? false : true; 364 zero_new_cam = (cvtest::randInt(rng)%2) == 0 ? false : true; 365 zero_R = (cvtest::randInt(rng)%2) == 0 ? false : true; 366 367 if (useCPlus) 368 { 369 _points.convertTo(src_points, CV_32F); 370 371 camera_mat = test_mat[INPUT][1]; 372 distortion_coeffs = test_mat[INPUT][2]; 373 R = test_mat[INPUT][3]; 374 P = test_mat[INPUT][4]; 375 } 376 377 return code; 378 } 379 380 void CV_UndistortPointsTest::prepare_to_validation(int /*test_case_idx*/) 381 { 382 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows; 383 double cam[9] = {0,0,0,0,0,0,0,0,1}; 384 double rot[9] = {1,0,0,0,1,0,0,0,1}; 385 386 double* dist = new double[dist_size ]; 387 double* proj = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows]; 388 double* points = new double[N_POINTS*2]; 389 double* r_points = new double[N_POINTS*2]; 390 //Run reference calculations 391 CvMat ref_points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,r_points); 392 CvMat _camera = cvMat(3,3,CV_64F,cam); 393 CvMat _rot = cvMat(3,3,CV_64F,rot); 394 CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist); 395 CvMat _proj = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,proj); 396 CvMat _points= cvMat(test_mat[TEMP][0].rows,test_mat[TEMP][0].cols,CV_64FC2,points); 397 398 Mat __camera = cvarrToMat(&_camera); 399 Mat __distort = cvarrToMat(&_distort); 400 Mat __rot = cvarrToMat(&_rot); 401 Mat __proj = cvarrToMat(&_proj); 402 Mat __points = cvarrToMat(&_points); 403 Mat _ref_points = cvarrToMat(&ref_points); 404 405 cvtest::convert(test_mat[INPUT][1], __camera, __camera.type()); 406 cvtest::convert(test_mat[INPUT][2], __distort, __distort.type()); 407 cvtest::convert(test_mat[INPUT][3], __rot, __rot.type()); 408 cvtest::convert(test_mat[INPUT][4], __proj, __proj.type()); 409 410 if (useCPlus) 411 { 412 if (useDstMat) 413 { 414 CvMat temp = dst_points_mat; 415 for (int i=0;i<N_POINTS*2;i++) 416 { 417 points[i] = temp.data.fl[i]; 418 } 419 } 420 else 421 { 422 for (int i=0;i<N_POINTS;i++) 423 { 424 points[2*i] = dst_points[i].x; 425 points[2*i+1] = dst_points[i].y; 426 } 427 } 428 } 429 else 430 { 431 cvtest::convert(test_mat[TEMP][0],__points, __points.type()); 432 } 433 434 CvMat* input2 = zero_distortion ? 0 : &_distort; 435 CvMat* input3 = zero_R ? 0 : &_rot; 436 CvMat* input4 = zero_new_cam ? 0 : &_proj; 437 distortPoints(&_points,&ref_points,&_camera,input2,input3,input4); 438 439 Mat& dst = test_mat[REF_OUTPUT][0]; 440 cvtest::convert(_ref_points, dst, dst.type()); 441 442 cvtest::copy(test_mat[INPUT][0], test_mat[OUTPUT][0]); 443 444 delete[] dist; 445 delete[] proj; 446 delete[] points; 447 delete[] r_points; 448 } 449 450 void CV_UndistortPointsTest::run_func() 451 { 452 453 if (useCPlus) 454 { 455 cv::Mat input2,input3,input4; 456 input2 = zero_distortion ? cv::Mat() : cv::Mat(test_mat[INPUT][2]); 457 input3 = zero_R ? cv::Mat() : cv::Mat(test_mat[INPUT][3]); 458 input4 = zero_new_cam ? cv::Mat() : cv::Mat(test_mat[INPUT][4]); 459 460 if (useDstMat) 461 { 462 //cv::undistortPoints(src_points,dst_points_mat,camera_mat,distortion_coeffs,R,P); 463 cv::undistortPoints(src_points,dst_points_mat,camera_mat,input2,input3,input4); 464 } 465 else 466 { 467 //cv::undistortPoints(src_points,dst_points,camera_mat,distortion_coeffs,R,P); 468 cv::undistortPoints(src_points,dst_points,camera_mat,input2,input3,input4); 469 } 470 } 471 else 472 { 473 CvMat _input0 = test_mat[INPUT][0], _input1 = test_mat[INPUT][1], _input2, _input3, _input4; 474 CvMat _output = test_mat[TEMP][0]; 475 if(!zero_distortion) 476 _input2 = test_mat[INPUT][2]; 477 if(!zero_R) 478 _input3 = test_mat[INPUT][3]; 479 if(!zero_new_cam) 480 _input4 = test_mat[INPUT][4]; 481 cvUndistortPoints(&_input0, &_output, &_input1, 482 zero_distortion ? 0 : &_input2, 483 zero_R ? 0 : &_input3, 484 zero_new_cam ? 0 : &_input4); 485 } 486 } 487 488 void CV_UndistortPointsTest::distortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, 489 const CvMat* _distCoeffs, 490 const CvMat* matR, const CvMat* matP) 491 { 492 double a[9]; 493 494 CvMat* __P; 495 if ((!matP)||(matP->cols == 3)) 496 __P = cvCreateMat(3,3,CV_64F); 497 else 498 __P = cvCreateMat(3,4,CV_64F); 499 if (matP) 500 { 501 cvtest::convert(cvarrToMat(matP), cvarrToMat(__P), -1); 502 } 503 else 504 { 505 cvZero(__P); 506 __P->data.db[0] = 1; 507 __P->data.db[4] = 1; 508 __P->data.db[8] = 1; 509 } 510 CvMat* __R = cvCreateMat(3,3,CV_64F); 511 if (matR) 512 { 513 cvCopy(matR,__R); 514 } 515 else 516 { 517 cvZero(__R); 518 __R->data.db[0] = 1; 519 __R->data.db[4] = 1; 520 __R->data.db[8] = 1; 521 } 522 for (int i=0;i<N_POINTS;i++) 523 { 524 int movement = __P->cols > 3 ? 1 : 0; 525 double x = (_src->data.db[2*i]-__P->data.db[2])/__P->data.db[0]; 526 double y = (_src->data.db[2*i+1]-__P->data.db[5+movement])/__P->data.db[4+movement]; 527 CvMat inverse = cvMat(3,3,CV_64F,a); 528 cvInvert(__R,&inverse); 529 double w1 = x*inverse.data.db[6]+y*inverse.data.db[7]+inverse.data.db[8]; 530 double _x = (x*inverse.data.db[0]+y*inverse.data.db[1]+inverse.data.db[2])/w1; 531 double _y = (x*inverse.data.db[3]+y*inverse.data.db[4]+inverse.data.db[5])/w1; 532 533 //Distortions 534 535 double __x = _x; 536 double __y = _y; 537 if (_distCoeffs) 538 { 539 double r2 = _x*_x+_y*_y; 540 541 __x = _x*(1+_distCoeffs->data.db[0]*r2+_distCoeffs->data.db[1]*r2*r2)+ 542 2*_distCoeffs->data.db[2]*_x*_y+_distCoeffs->data.db[3]*(r2+2*_x*_x); 543 __y = _y*(1+_distCoeffs->data.db[0]*r2+_distCoeffs->data.db[1]*r2*r2)+ 544 2*_distCoeffs->data.db[3]*_x*_y+_distCoeffs->data.db[2]*(r2+2*_y*_y); 545 if ((_distCoeffs->cols > 4) || (_distCoeffs->rows > 4)) 546 { 547 __x+=_x*_distCoeffs->data.db[4]*r2*r2*r2; 548 __y+=_y*_distCoeffs->data.db[4]*r2*r2*r2; 549 } 550 } 551 552 553 _dst->data.db[2*i] = __x*_cameraMatrix->data.db[0]+_cameraMatrix->data.db[2]; 554 _dst->data.db[2*i+1] = __y*_cameraMatrix->data.db[4]+_cameraMatrix->data.db[5]; 555 556 } 557 558 cvReleaseMat(&__R); 559 cvReleaseMat(&__P); 560 561 } 562 563 564 double CV_UndistortPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) 565 { 566 return 5e-2; 567 } 568 569 //------------------------------------------------------ 570 571 class CV_InitUndistortRectifyMapTest : public cvtest::ArrayTest 572 { 573 public: 574 CV_InitUndistortRectifyMapTest(); 575 protected: 576 int prepare_test_case (int test_case_idx); 577 void prepare_to_validation( int test_case_idx ); 578 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 579 double get_success_error_level( int test_case_idx, int i, int j ); 580 void run_func(); 581 582 private: 583 bool useCPlus; 584 static const int N_POINTS = 100; 585 static const int MAX_X = 2048; 586 static const int MAX_Y = 2048; 587 bool zero_new_cam; 588 bool zero_distortion; 589 bool zero_R; 590 591 592 cv::Size img_size; 593 594 cv::Mat camera_mat; 595 cv::Mat R; 596 cv::Mat new_camera_mat; 597 cv::Mat distortion_coeffs; 598 cv::Mat mapx; 599 cv::Mat mapy; 600 CvMat* _mapx; 601 CvMat* _mapy; 602 int mat_type; 603 }; 604 605 CV_InitUndistortRectifyMapTest::CV_InitUndistortRectifyMapTest() 606 { 607 test_array[INPUT].push_back(NULL); // test points matrix 608 test_array[INPUT].push_back(NULL); // camera matrix 609 test_array[INPUT].push_back(NULL); // distortion coeffs 610 test_array[INPUT].push_back(NULL); // R matrix 611 test_array[INPUT].push_back(NULL); // new camera matrix 612 test_array[OUTPUT].push_back(NULL); // distorted dst points 613 test_array[REF_OUTPUT].push_back(NULL); 614 615 useCPlus = false; 616 zero_distortion = zero_new_cam = zero_R = false; 617 _mapx = _mapy = NULL; 618 mat_type = 0; 619 } 620 621 void CV_InitUndistortRectifyMapTest::get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ) 622 { 623 cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types); 624 RNG& rng = ts->get_rng(); 625 useCPlus = ((cvtest::randInt(rng) % 2)!=0); 626 //useCPlus = 0; 627 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC2; 628 629 types[INPUT][1] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 630 types[INPUT][2] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 631 types[INPUT][3] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 632 types[INPUT][4] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; 633 634 sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(N_POINTS,1); 635 sizes[INPUT][1] = sizes[INPUT][3] = cvSize(3,3); 636 sizes[INPUT][4] = cvSize(3,3); 637 638 if (cvtest::randInt(rng)%2) 639 { 640 if (cvtest::randInt(rng)%2) 641 { 642 sizes[INPUT][2] = cvSize(1,4); 643 } 644 else 645 { 646 sizes[INPUT][2] = cvSize(1,5); 647 } 648 } 649 else 650 { 651 if (cvtest::randInt(rng)%2) 652 { 653 sizes[INPUT][2] = cvSize(4,1); 654 } 655 else 656 { 657 sizes[INPUT][2] = cvSize(5,1); 658 } 659 } 660 } 661 662 663 int CV_InitUndistortRectifyMapTest::prepare_test_case(int test_case_idx) 664 { 665 RNG& rng = ts->get_rng(); 666 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); 667 668 if (code <= 0) 669 return code; 670 671 img_size.width = cvtest::randInt(rng) % MAX_X + 1; 672 img_size.height = cvtest::randInt(rng) % MAX_Y + 1; 673 674 if (useCPlus) 675 { 676 mat_type = (cvtest::randInt(rng) % 2) == 0 ? CV_32FC1 : CV_16SC2; 677 if ((cvtest::randInt(rng) % 4) == 0) 678 mat_type = -1; 679 if ((cvtest::randInt(rng) % 4) == 0) 680 mat_type = CV_32FC2; 681 _mapx = 0; 682 _mapy = 0; 683 } 684 else 685 { 686 int typex = (cvtest::randInt(rng) % 2) == 0 ? CV_32FC1 : CV_16SC2; 687 //typex = CV_32FC1; ///!!!!!!!!!!!!!!!! 688 int typey = (typex == CV_32FC1) ? CV_32FC1 : CV_16UC1; 689 690 _mapx = cvCreateMat(img_size.height,img_size.width,typex); 691 _mapy = cvCreateMat(img_size.height,img_size.width,typey); 692 693 694 } 695 696 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows; 697 double cam[9] = {0,0,0,0,0,0,0,0,1}; 698 vector<double> dist(dist_size); 699 vector<double> new_cam(test_mat[INPUT][4].cols * test_mat[INPUT][4].rows); 700 vector<Point2d> points(N_POINTS); 701 702 Mat _camera(3,3,CV_64F,cam); 703 Mat _distort(test_mat[INPUT][2].size(),CV_64F,&dist[0]); 704 Mat _new_cam(test_mat[INPUT][4].size(),CV_64F,&new_cam[0]); 705 Mat _points(test_mat[INPUT][0].size(),CV_64FC2, &points[0]); 706 707 //Generating points 708 for (int i=0;i<N_POINTS;i++) 709 { 710 points[i].x = cvtest::randReal(rng)*img_size.width; 711 points[i].y = cvtest::randReal(rng)*img_size.height; 712 } 713 714 //Generating camera matrix 715 double sz = MAX(img_size.width,img_size.height); 716 double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7; 717 cam[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 718 cam[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5; 719 cam[0] = sz/(0.9 - cvtest::randReal(rng)*0.6); 720 cam[4] = aspect_ratio*cam[0]; 721 722 //Generating distortion coeffs 723 dist[0] = cvtest::randReal(rng)*0.06 - 0.03; 724 dist[1] = cvtest::randReal(rng)*0.06 - 0.03; 725 if( dist[0]*dist[1] > 0 ) 726 dist[1] = -dist[1]; 727 if( cvtest::randInt(rng)%4 != 0 ) 728 { 729 dist[2] = cvtest::randReal(rng)*0.004 - 0.002; 730 dist[3] = cvtest::randReal(rng)*0.004 - 0.002; 731 if (dist_size > 4) 732 dist[4] = cvtest::randReal(rng)*0.004 - 0.002; 733 } 734 else 735 { 736 dist[2] = dist[3] = 0; 737 if (dist_size > 4) 738 dist[4] = 0; 739 } 740 741 //Generating new camera matrix 742 _new_cam = Scalar::all(0); 743 new_cam[8] = 1; 744 745 //new_cam[0] = cam[0]; 746 //new_cam[4] = cam[4]; 747 //new_cam[2] = cam[2]; 748 //new_cam[5] = cam[5]; 749 750 new_cam[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10% 751 new_cam[4] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10% 752 new_cam[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15% 753 new_cam[5] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15% 754 755 756 //Generating R matrix 757 Mat _rot(3,3,CV_64F); 758 Mat rotation(1,3,CV_64F); 759 rotation.at<double>(0) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // phi 760 rotation.at<double>(1) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // ksi 761 rotation.at<double>(2) = CV_PI/3*(cvtest::randReal(rng) - (double)0.5); //khi 762 cvtest::Rodrigues(rotation, _rot); 763 764 //cvSetIdentity(_rot); 765 //copying data 766 cvtest::convert( _points, test_mat[INPUT][0], test_mat[INPUT][0].type()); 767 cvtest::convert( _camera, test_mat[INPUT][1], test_mat[INPUT][1].type()); 768 cvtest::convert( _distort, test_mat[INPUT][2], test_mat[INPUT][2].type()); 769 cvtest::convert( _rot, test_mat[INPUT][3], test_mat[INPUT][3].type()); 770 cvtest::convert( _new_cam, test_mat[INPUT][4], test_mat[INPUT][4].type()); 771 772 zero_distortion = (cvtest::randInt(rng)%2) == 0 ? false : true; 773 zero_new_cam = (cvtest::randInt(rng)%2) == 0 ? false : true; 774 zero_R = (cvtest::randInt(rng)%2) == 0 ? false : true; 775 776 if (useCPlus) 777 { 778 camera_mat = test_mat[INPUT][1]; 779 distortion_coeffs = test_mat[INPUT][2]; 780 R = test_mat[INPUT][3]; 781 new_camera_mat = test_mat[INPUT][4]; 782 } 783 784 return code; 785 } 786 787 void CV_InitUndistortRectifyMapTest::prepare_to_validation(int/* test_case_idx*/) 788 { 789 #if 0 790 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows; 791 double cam[9] = {0,0,0,0,0,0,0,0,1}; 792 double rot[9] = {1,0,0,0,1,0,0,0,1}; 793 vector<double> dist(dist_size); 794 vector<double> new_cam(test_mat[INPUT][4].cols * test_mat[INPUT][4].rows); 795 vector<Point2d> points(N_POINTS); 796 vector<Point2d> r_points(N_POINTS); 797 //Run reference calculations 798 Mat ref_points(test_mat[INPUT][0].size(),CV_64FC2,&r_points[0]); 799 Mat _camera(3,3,CV_64F,cam); 800 Mat _rot(3,3,CV_64F,rot); 801 Mat _distort(test_mat[INPUT][2].size(),CV_64F,&dist[0]); 802 Mat _new_cam(test_mat[INPUT][4].size(),CV_64F,&new_cam[0]); 803 Mat _points(test_mat[INPUT][0].size(),CV_64FC2,&points[0]); 804 805 cvtest::convert(test_mat[INPUT][1],_camera,_camera.type()); 806 cvtest::convert(test_mat[INPUT][2],_distort,_distort.type()); 807 cvtest::convert(test_mat[INPUT][3],_rot,_rot.type()); 808 cvtest::convert(test_mat[INPUT][4],_new_cam,_new_cam.type()); 809 810 //Applying precalculated undistort rectify map 811 if (!useCPlus) 812 { 813 mapx = cv::Mat(_mapx); 814 mapy = cv::Mat(_mapy); 815 } 816 cv::Mat map1,map2; 817 cv::convertMaps(mapx,mapy,map1,map2,CV_32FC1); 818 CvMat _map1 = map1; 819 CvMat _map2 = map2; 820 const Point2d* sptr = (const Point2d*)test_mat[INPUT][0].data; 821 for( int i = 0;i < N_POINTS; i++ ) 822 { 823 int u = saturate_cast<int>(sptr[i].x); 824 int v = saturate_cast<int>(sptr[i].y); 825 points[i].x = _map1.data.fl[v*_map1.cols + u]; 826 points[i].y = _map2.data.fl[v*_map2.cols + u]; 827 } 828 829 //--- 830 831 cv::undistortPoints(_points, ref_points, _camera, 832 zero_distortion ? Mat() : _distort, 833 zero_R ? Mat::eye(3,3,CV_64F) : _rot, 834 zero_new_cam ? _camera : _new_cam); 835 //cvTsDistortPoints(&_points,&ref_points,&_camera,&_distort,&_rot,&_new_cam); 836 cvtest::convert(ref_points, test_mat[REF_OUTPUT][0], test_mat[REF_OUTPUT][0].type()); 837 cvtest::copy(test_mat[INPUT][0],test_mat[OUTPUT][0]); 838 839 cvReleaseMat(&_mapx); 840 cvReleaseMat(&_mapy); 841 #else 842 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows; 843 double cam[9] = {0,0,0,0,0,0,0,0,1}; 844 double rot[9] = {1,0,0,0,1,0,0,0,1}; 845 double* dist = new double[dist_size ]; 846 double* new_cam = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows]; 847 double* points = new double[N_POINTS*2]; 848 double* r_points = new double[N_POINTS*2]; 849 //Run reference calculations 850 CvMat ref_points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,r_points); 851 CvMat _camera = cvMat(3,3,CV_64F,cam); 852 CvMat _rot = cvMat(3,3,CV_64F,rot); 853 CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist); 854 CvMat _new_cam = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,new_cam); 855 CvMat _points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,points); 856 857 CvMat _input1 = test_mat[INPUT][1]; 858 CvMat _input2 = test_mat[INPUT][2]; 859 CvMat _input3 = test_mat[INPUT][3]; 860 CvMat _input4 = test_mat[INPUT][4]; 861 862 cvtest::convert(cvarrToMat(&_input1), cvarrToMat(&_camera), -1); 863 cvtest::convert(cvarrToMat(&_input2), cvarrToMat(&_distort), -1); 864 cvtest::convert(cvarrToMat(&_input3), cvarrToMat(&_rot), -1); 865 cvtest::convert(cvarrToMat(&_input4), cvarrToMat(&_new_cam), -1); 866 867 //Applying precalculated undistort rectify map 868 if (!useCPlus) 869 { 870 mapx = cv::cvarrToMat(_mapx); 871 mapy = cv::cvarrToMat(_mapy); 872 } 873 cv::Mat map1,map2; 874 cv::convertMaps(mapx,mapy,map1,map2,CV_32FC1); 875 CvMat _map1 = map1; 876 CvMat _map2 = map2; 877 for (int i=0;i<N_POINTS;i++) 878 { 879 double u = test_mat[INPUT][0].ptr<double>()[2*i]; 880 double v = test_mat[INPUT][0].ptr<double>()[2*i+1]; 881 _points.data.db[2*i] = (double)_map1.data.fl[(int)v*_map1.cols+(int)u]; 882 _points.data.db[2*i+1] = (double)_map2.data.fl[(int)v*_map2.cols+(int)u]; 883 } 884 885 //--- 886 887 cvUndistortPoints(&_points,&ref_points,&_camera, 888 zero_distortion ? 0 : &_distort, zero_R ? 0 : &_rot, zero_new_cam ? &_camera : &_new_cam); 889 //cvTsDistortPoints(&_points,&ref_points,&_camera,&_distort,&_rot,&_new_cam); 890 CvMat dst = test_mat[REF_OUTPUT][0]; 891 cvtest::convert(cvarrToMat(&ref_points), cvarrToMat(&dst), -1); 892 893 cvtest::copy(test_mat[INPUT][0],test_mat[OUTPUT][0]); 894 895 delete[] dist; 896 delete[] new_cam; 897 delete[] points; 898 delete[] r_points; 899 cvReleaseMat(&_mapx); 900 cvReleaseMat(&_mapy); 901 #endif 902 } 903 904 void CV_InitUndistortRectifyMapTest::run_func() 905 { 906 if (useCPlus) 907 { 908 cv::Mat input2,input3,input4; 909 input2 = zero_distortion ? cv::Mat() : test_mat[INPUT][2]; 910 input3 = zero_R ? cv::Mat() : test_mat[INPUT][3]; 911 input4 = zero_new_cam ? cv::Mat() : test_mat[INPUT][4]; 912 cv::initUndistortRectifyMap(camera_mat,input2,input3,input4,img_size,mat_type,mapx,mapy); 913 } 914 else 915 { 916 CvMat input1 = test_mat[INPUT][1], input2, input3, input4; 917 if( !zero_distortion ) 918 input2 = test_mat[INPUT][2]; 919 if( !zero_R ) 920 input3 = test_mat[INPUT][3]; 921 if( !zero_new_cam ) 922 input4 = test_mat[INPUT][4]; 923 cvInitUndistortRectifyMap(&input1, 924 zero_distortion ? 0 : &input2, 925 zero_R ? 0 : &input3, 926 zero_new_cam ? 0 : &input4, 927 _mapx,_mapy); 928 } 929 } 930 931 double CV_InitUndistortRectifyMapTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) 932 { 933 return 8; 934 } 935 936 ////////////////////////////////////////////////////////////////////////////////////////////////////// 937 938 TEST(Calib3d_DefaultNewCameraMatrix, accuracy) { CV_DefaultNewCameraMatrixTest test; test.safe_run(); } 939 TEST(Calib3d_UndistortPoints, accuracy) { CV_UndistortPointsTest test; test.safe_run(); } 940 TEST(Calib3d_InitUndistortRectifyMap, accuracy) { CV_InitUndistortRectifyMapTest test; test.safe_run(); } 941
