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 using namespace cv; 46 using namespace std; 47 48 int cvTsRodrigues( const CvMat* src, CvMat* dst, CvMat* jacobian ) 49 { 50 int depth; 51 int i; 52 float Jf[27]; 53 double J[27]; 54 CvMat _Jf, matJ = cvMat( 3, 9, CV_64F, J ); 55 56 depth = CV_MAT_DEPTH(src->type); 57 58 if( jacobian ) 59 { 60 assert( (jacobian->rows == 9 && jacobian->cols == 3) || 61 (jacobian->rows == 3 && jacobian->cols == 9) ); 62 } 63 64 if( src->cols == 1 || src->rows == 1 ) 65 { 66 double r[3], theta; 67 CvMat _r = cvMat( src->rows, src->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(src->type)), r); 68 69 assert( dst->rows == 3 && dst->cols == 3 ); 70 71 cvConvert( src, &_r ); 72 73 theta = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); 74 if( theta < DBL_EPSILON ) 75 { 76 cvSetIdentity( dst ); 77 78 if( jacobian ) 79 { 80 memset( J, 0, sizeof(J) ); 81 J[5] = J[15] = J[19] = 1; 82 J[7] = J[11] = J[21] = -1; 83 } 84 } 85 else 86 { 87 // omega = r/theta (~[w1, w2, w3]) 88 double itheta = 1./theta; 89 double w1 = r[0]*itheta, w2 = r[1]*itheta, w3 = r[2]*itheta; 90 double alpha = cos(theta); 91 double beta = sin(theta); 92 double gamma = 1 - alpha; 93 double omegav[] = 94 { 95 0, -w3, w2, 96 w3, 0, -w1, 97 -w2, w1, 0 98 }; 99 double A[] = 100 { 101 w1*w1, w1*w2, w1*w3, 102 w2*w1, w2*w2, w2*w3, 103 w3*w1, w3*w2, w3*w3 104 }; 105 double R[9]; 106 CvMat _omegav = cvMat(3, 3, CV_64F, omegav); 107 CvMat matA = cvMat(3, 3, CV_64F, A); 108 CvMat matR = cvMat(3, 3, CV_64F, R); 109 110 cvSetIdentity( &matR, cvRealScalar(alpha) ); 111 cvScaleAdd( &_omegav, cvRealScalar(beta), &matR, &matR ); 112 cvScaleAdd( &matA, cvRealScalar(gamma), &matR, &matR ); 113 cvConvert( &matR, dst ); 114 115 if( jacobian ) 116 { 117 // m3 = [r, theta] 118 double dm3din[] = 119 { 120 1, 0, 0, 121 0, 1, 0, 122 0, 0, 1, 123 w1, w2, w3 124 }; 125 // m2 = [omega, theta] 126 double dm2dm3[] = 127 { 128 itheta, 0, 0, -w1*itheta, 129 0, itheta, 0, -w2*itheta, 130 0, 0, itheta, -w3*itheta, 131 0, 0, 0, 1 132 }; 133 double t0[9*4]; 134 double dm1dm2[21*4]; 135 double dRdm1[9*21]; 136 CvMat _dm3din = cvMat( 4, 3, CV_64FC1, dm3din ); 137 CvMat _dm2dm3 = cvMat( 4, 4, CV_64FC1, dm2dm3 ); 138 CvMat _dm1dm2 = cvMat( 21, 4, CV_64FC1, dm1dm2 ); 139 CvMat _dRdm1 = cvMat( 9, 21, CV_64FC1, dRdm1 ); 140 CvMat _dRdm1_part; 141 CvMat _t0 = cvMat( 9, 4, CV_64FC1, t0 ); 142 CvMat _t1 = cvMat( 9, 4, CV_64FC1, dRdm1 ); 143 144 // m1 = [alpha, beta, gamma, omegav; A] 145 memset( dm1dm2, 0, sizeof(dm1dm2) ); 146 dm1dm2[3] = -beta; 147 dm1dm2[7] = alpha; 148 dm1dm2[11] = beta; 149 150 // dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; 151 // 0 0 -1 0 0 0 1 0 0; 152 // 0 1 0 -1 0 0 0 0 0]' 153 // ------------------- 154 // 0 0 0 0 0 0 0 0 0 155 dm1dm2[12 + 6] = dm1dm2[12 + 20] = dm1dm2[12 + 25] = 1; 156 dm1dm2[12 + 9] = dm1dm2[12 + 14] = dm1dm2[12 + 28] = -1; 157 158 double dm1dw[] = 159 { 160 2*w1, w2, w3, w2, 0, 0, w3, 0, 0, 161 0, w1, 0, w1, 2*w2, w3, 0, w3, 0, 162 0, 0, w1, 0, 0, w2, w1, w2, 2*w3 163 }; 164 165 CvMat _dm1dw = cvMat( 3, 9, CV_64FC1, dm1dw ); 166 CvMat _dm1dm2_part; 167 168 cvGetSubRect( &_dm1dm2, &_dm1dm2_part, cvRect(0,12,3,9) ); 169 cvTranspose( &_dm1dw, &_dm1dm2_part ); 170 171 memset( dRdm1, 0, sizeof(dRdm1) ); 172 dRdm1[0*21] = dRdm1[4*21] = dRdm1[8*21] = 1; 173 174 cvGetCol( &_dRdm1, &_dRdm1_part, 1 ); 175 cvTranspose( &_omegav, &_omegav ); 176 cvReshape( &_omegav, &_omegav, 1, 1 ); 177 cvTranspose( &_omegav, &_dRdm1_part ); 178 179 cvGetCol( &_dRdm1, &_dRdm1_part, 2 ); 180 cvReshape( &matA, &matA, 1, 1 ); 181 cvTranspose( &matA, &_dRdm1_part ); 182 183 cvGetSubRect( &_dRdm1, &_dRdm1_part, cvRect(3,0,9,9) ); 184 cvSetIdentity( &_dRdm1_part, cvScalarAll(beta) ); 185 186 cvGetSubRect( &_dRdm1, &_dRdm1_part, cvRect(12,0,9,9) ); 187 cvSetIdentity( &_dRdm1_part, cvScalarAll(gamma) ); 188 189 matJ = cvMat( 9, 3, CV_64FC1, J ); 190 191 cvMatMul( &_dRdm1, &_dm1dm2, &_t0 ); 192 cvMatMul( &_t0, &_dm2dm3, &_t1 ); 193 cvMatMul( &_t1, &_dm3din, &matJ ); 194 195 _t0 = cvMat( 3, 9, CV_64FC1, t0 ); 196 cvTranspose( &matJ, &_t0 ); 197 198 for( i = 0; i < 3; i++ ) 199 { 200 _t1 = cvMat( 3, 3, CV_64FC1, t0 + i*9 ); 201 cvTranspose( &_t1, &_t1 ); 202 } 203 204 cvTranspose( &_t0, &matJ ); 205 } 206 } 207 } 208 else if( src->cols == 3 && src->rows == 3 ) 209 { 210 double R[9], A[9], I[9], r[3], W[3], U[9], V[9]; 211 double tr, alpha, beta, theta; 212 CvMat matR = cvMat( 3, 3, CV_64F, R ); 213 CvMat matA = cvMat( 3, 3, CV_64F, A ); 214 CvMat matI = cvMat( 3, 3, CV_64F, I ); 215 CvMat _r = cvMat( dst->rows, dst->cols, CV_MAKETYPE(CV_64F, CV_MAT_CN(dst->type)), r ); 216 CvMat matW = cvMat( 1, 3, CV_64F, W ); 217 CvMat matU = cvMat( 3, 3, CV_64F, U ); 218 CvMat matV = cvMat( 3, 3, CV_64F, V ); 219 220 cvConvert( src, &matR ); 221 cvSVD( &matR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 222 cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T ); 223 224 cvMulTransposed( &matR, &matA, 0 ); 225 cvSetIdentity( &matI ); 226 227 if( cvNorm( &matA, &matI, CV_C ) > 1e-3 || 228 fabs( cvDet(&matR) - 1 ) > 1e-3 ) 229 return 0; 230 231 tr = (cvTrace(&matR).val[0] - 1.)*0.5; 232 tr = tr > 1. ? 1. : tr < -1. ? -1. : tr; 233 theta = acos(tr); 234 alpha = cos(theta); 235 beta = sin(theta); 236 237 if( beta >= 1e-5 ) 238 { 239 double dtheta_dtr = -1./sqrt(1 - tr*tr); 240 double vth = 1/(2*beta); 241 242 // om1 = [R(3,2) - R(2,3), R(1,3) - R(3,1), R(2,1) - R(1,2)]' 243 double om1[] = { R[7] - R[5], R[2] - R[6], R[3] - R[1] }; 244 // om = om1*vth 245 // r = om*theta 246 double d3 = vth*theta; 247 248 r[0] = om1[0]*d3; r[1] = om1[1]*d3; r[2] = om1[2]*d3; 249 cvConvert( &_r, dst ); 250 251 if( jacobian ) 252 { 253 // var1 = [vth;theta] 254 // var = [om1;var1] = [om1;vth;theta] 255 double dvth_dtheta = -vth*alpha/beta; 256 double d1 = 0.5*dvth_dtheta*dtheta_dtr; 257 double d2 = 0.5*dtheta_dtr; 258 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR 259 double dvardR[5*9] = 260 { 261 0, 0, 0, 0, 0, 1, 0, -1, 0, 262 0, 0, -1, 0, 0, 0, 1, 0, 0, 263 0, 1, 0, -1, 0, 0, 0, 0, 0, 264 d1, 0, 0, 0, d1, 0, 0, 0, d1, 265 d2, 0, 0, 0, d2, 0, 0, 0, d2 266 }; 267 // var2 = [om;theta] 268 double dvar2dvar[] = 269 { 270 vth, 0, 0, om1[0], 0, 271 0, vth, 0, om1[1], 0, 272 0, 0, vth, om1[2], 0, 273 0, 0, 0, 0, 1 274 }; 275 double domegadvar2[] = 276 { 277 theta, 0, 0, om1[0]*vth, 278 0, theta, 0, om1[1]*vth, 279 0, 0, theta, om1[2]*vth 280 }; 281 282 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); 283 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar ); 284 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 ); 285 double t0[3*5]; 286 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 ); 287 288 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 ); 289 cvMatMul( &_t0, &_dvardR, &matJ ); 290 } 291 } 292 else if( tr > 0 ) 293 { 294 cvZero( dst ); 295 if( jacobian ) 296 { 297 memset( J, 0, sizeof(J) ); 298 J[5] = J[15] = J[19] = 0.5; 299 J[7] = J[11] = J[21] = -0.5; 300 } 301 } 302 else 303 { 304 r[0] = theta*sqrt((R[0] + 1)*0.5); 305 r[1] = theta*sqrt((R[4] + 1)*0.5)*(R[1] >= 0 ? 1 : -1); 306 r[2] = theta*sqrt((R[8] + 1)*0.5)*(R[2] >= 0 ? 1 : -1); 307 cvConvert( &_r, dst ); 308 309 if( jacobian ) 310 memset( J, 0, sizeof(J) ); 311 } 312 313 if( jacobian ) 314 { 315 for( i = 0; i < 3; i++ ) 316 { 317 CvMat t = cvMat( 3, 3, CV_64F, J + i*9 ); 318 cvTranspose( &t, &t ); 319 } 320 } 321 } 322 else 323 { 324 assert(0); 325 return 0; 326 } 327 328 if( jacobian ) 329 { 330 if( depth == CV_32F ) 331 { 332 if( jacobian->rows == matJ.rows ) 333 cvConvert( &matJ, jacobian ); 334 else 335 { 336 _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf ); 337 cvConvert( &matJ, &_Jf ); 338 cvTranspose( &_Jf, jacobian ); 339 } 340 } 341 else if( jacobian->rows == matJ.rows ) 342 cvCopy( &matJ, jacobian ); 343 else 344 cvTranspose( &matJ, jacobian ); 345 } 346 347 return 1; 348 } 349 350 351 void cvtest::Rodrigues(const Mat& src, Mat& dst, Mat* jac) 352 { 353 CvMat _src = src, _dst = dst, _jac; 354 if( jac ) 355 _jac = *jac; 356 cvTsRodrigues(&_src, &_dst, jac ? &_jac : 0); 357 } 358 359 360 static void test_convertHomogeneous( const Mat& _src, Mat& _dst ) 361 { 362 Mat src = _src, dst = _dst; 363 int i, count, sdims, ddims; 364 int sstep1, sstep2, dstep1, dstep2; 365 366 if( src.depth() != CV_64F ) 367 _src.convertTo(src, CV_64F); 368 369 if( dst.depth() != CV_64F ) 370 dst.create(dst.size(), CV_MAKETYPE(CV_64F, _dst.channels())); 371 372 if( src.rows > src.cols ) 373 { 374 count = src.rows; 375 sdims = src.channels()*src.cols; 376 sstep1 = (int)(src.step/sizeof(double)); 377 sstep2 = 1; 378 } 379 else 380 { 381 count = src.cols; 382 sdims = src.channels()*src.rows; 383 if( src.rows == 1 ) 384 { 385 sstep1 = sdims; 386 sstep2 = 1; 387 } 388 else 389 { 390 sstep1 = 1; 391 sstep2 = (int)(src.step/sizeof(double)); 392 } 393 } 394 395 if( dst.rows > dst.cols ) 396 { 397 CV_Assert( count == dst.rows ); 398 ddims = dst.channels()*dst.cols; 399 dstep1 = (int)(dst.step/sizeof(double)); 400 dstep2 = 1; 401 } 402 else 403 { 404 assert( count == dst.cols ); 405 ddims = dst.channels()*dst.rows; 406 if( dst.rows == 1 ) 407 { 408 dstep1 = ddims; 409 dstep2 = 1; 410 } 411 else 412 { 413 dstep1 = 1; 414 dstep2 = (int)(dst.step/sizeof(double)); 415 } 416 } 417 418 double* s = src.ptr<double>(); 419 double* d = dst.ptr<double>(); 420 421 if( sdims <= ddims ) 422 { 423 int wstep = dstep2*(ddims - 1); 424 425 for( i = 0; i < count; i++, s += sstep1, d += dstep1 ) 426 { 427 double x = s[0]; 428 double y = s[sstep2]; 429 430 d[wstep] = 1; 431 d[0] = x; 432 d[dstep2] = y; 433 434 if( sdims >= 3 ) 435 { 436 d[dstep2*2] = s[sstep2*2]; 437 if( sdims == 4 ) 438 d[dstep2*3] = s[sstep2*3]; 439 } 440 } 441 } 442 else 443 { 444 int wstep = sstep2*(sdims - 1); 445 446 for( i = 0; i < count; i++, s += sstep1, d += dstep1 ) 447 { 448 double w = s[wstep]; 449 double x = s[0]; 450 double y = s[sstep2]; 451 452 w = w ? 1./w : 1; 453 454 d[0] = x*w; 455 d[dstep2] = y*w; 456 457 if( ddims == 3 ) 458 d[dstep2*2] = s[sstep2*2]*w; 459 } 460 } 461 462 if( dst.data != _dst.data ) 463 dst.convertTo(_dst, _dst.depth()); 464 } 465 466 467 void 468 test_projectPoints( const Mat& _3d, const Mat& Rt, const Mat& A, Mat& _2d, RNG* rng, double sigma ) 469 { 470 CV_Assert( _3d.isContinuous() ); 471 472 double p[12]; 473 Mat P( 3, 4, CV_64F, p ); 474 gemm(A, Rt, 1, Mat(), 0, P); 475 476 int i, count = _3d.cols; 477 478 Mat noise; 479 if( rng ) 480 { 481 if( sigma == 0 ) 482 rng = 0; 483 else 484 { 485 noise.create( 1, _3d.cols, CV_64FC2 ); 486 rng->fill(noise, RNG::NORMAL, Scalar::all(0), Scalar::all(sigma) ); 487 } 488 } 489 490 Mat temp( 1, count, CV_64FC3 ); 491 492 for( i = 0; i < count; i++ ) 493 { 494 const double* M = _3d.ptr<double>() + i*3; 495 double* m = temp.ptr<double>() + i*3; 496 double X = M[0], Y = M[1], Z = M[2]; 497 double u = p[0]*X + p[1]*Y + p[2]*Z + p[3]; 498 double v = p[4]*X + p[5]*Y + p[6]*Z + p[7]; 499 double s = p[8]*X + p[9]*Y + p[10]*Z + p[11]; 500 501 if( !noise.empty() ) 502 { 503 u += noise.at<Point2d>(i).x*s; 504 v += noise.at<Point2d>(i).y*s; 505 } 506 507 m[0] = u; 508 m[1] = v; 509 m[2] = s; 510 } 511 512 test_convertHomogeneous( temp, _2d ); 513 } 514 515 516 /********************************** Rodrigues transform ********************************/ 517 518 class CV_RodriguesTest : public cvtest::ArrayTest 519 { 520 public: 521 CV_RodriguesTest(); 522 523 protected: 524 int read_params( CvFileStorage* fs ); 525 void fill_array( int test_case_idx, int i, int j, Mat& arr ); 526 int prepare_test_case( int test_case_idx ); 527 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 528 double get_success_error_level( int test_case_idx, int i, int j ); 529 void run_func(); 530 void prepare_to_validation( int ); 531 532 bool calc_jacobians; 533 bool test_cpp; 534 }; 535 536 537 CV_RodriguesTest::CV_RodriguesTest() 538 { 539 test_array[INPUT].push_back(NULL); // rotation vector 540 test_array[OUTPUT].push_back(NULL); // rotation matrix 541 test_array[OUTPUT].push_back(NULL); // jacobian (J) 542 test_array[OUTPUT].push_back(NULL); // rotation vector (backward transform result) 543 test_array[OUTPUT].push_back(NULL); // inverse transform jacobian (J1) 544 test_array[OUTPUT].push_back(NULL); // J*J1 (or J1*J) == I(3x3) 545 test_array[REF_OUTPUT].push_back(NULL); 546 test_array[REF_OUTPUT].push_back(NULL); 547 test_array[REF_OUTPUT].push_back(NULL); 548 test_array[REF_OUTPUT].push_back(NULL); 549 test_array[REF_OUTPUT].push_back(NULL); 550 551 element_wise_relative_error = false; 552 calc_jacobians = false; 553 554 test_cpp = false; 555 } 556 557 558 int CV_RodriguesTest::read_params( CvFileStorage* fs ) 559 { 560 int code = cvtest::ArrayTest::read_params( fs ); 561 return code; 562 } 563 564 565 void CV_RodriguesTest::get_test_array_types_and_sizes( 566 int /*test_case_idx*/, vector<vector<Size> >& sizes, vector<vector<int> >& types ) 567 { 568 RNG& rng = ts->get_rng(); 569 int depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 570 int i, code; 571 572 code = cvtest::randInt(rng) % 3; 573 types[INPUT][0] = CV_MAKETYPE(depth, 1); 574 575 if( code == 0 ) 576 { 577 sizes[INPUT][0] = cvSize(1,1); 578 types[INPUT][0] = CV_MAKETYPE(depth, 3); 579 } 580 else if( code == 1 ) 581 sizes[INPUT][0] = cvSize(3,1); 582 else 583 sizes[INPUT][0] = cvSize(1,3); 584 585 sizes[OUTPUT][0] = cvSize(3, 3); 586 types[OUTPUT][0] = CV_MAKETYPE(depth, 1); 587 588 types[OUTPUT][1] = CV_MAKETYPE(depth, 1); 589 590 if( cvtest::randInt(rng) % 2 ) 591 sizes[OUTPUT][1] = cvSize(3,9); 592 else 593 sizes[OUTPUT][1] = cvSize(9,3); 594 595 types[OUTPUT][2] = types[INPUT][0]; 596 sizes[OUTPUT][2] = sizes[INPUT][0]; 597 598 types[OUTPUT][3] = types[OUTPUT][1]; 599 sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width); 600 601 types[OUTPUT][4] = types[OUTPUT][1]; 602 sizes[OUTPUT][4] = cvSize(3,3); 603 604 calc_jacobians = cvtest::randInt(rng) % 3 != 0; 605 if( !calc_jacobians ) 606 sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0); 607 608 for( i = 0; i < 5; i++ ) 609 { 610 types[REF_OUTPUT][i] = types[OUTPUT][i]; 611 sizes[REF_OUTPUT][i] = sizes[OUTPUT][i]; 612 } 613 test_cpp = (cvtest::randInt(rng) & 256) == 0; 614 } 615 616 617 double CV_RodriguesTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j ) 618 { 619 return j == 4 ? 1e-2 : 1e-2; 620 } 621 622 623 void CV_RodriguesTest::fill_array( int test_case_idx, int i, int j, Mat& arr ) 624 { 625 if( i == INPUT && j == 0 ) 626 { 627 double r[3], theta0, theta1, f; 628 Mat _r( arr.rows, arr.cols, CV_MAKETYPE(CV_64F,arr.channels()), r ); 629 RNG& rng = ts->get_rng(); 630 631 r[0] = cvtest::randReal(rng)*CV_PI*2; 632 r[1] = cvtest::randReal(rng)*CV_PI*2; 633 r[2] = cvtest::randReal(rng)*CV_PI*2; 634 635 theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); 636 theta1 = fmod(theta0, CV_PI*2); 637 638 if( theta1 > CV_PI ) 639 theta1 = -(CV_PI*2 - theta1); 640 641 f = theta1/(theta0 ? theta0 : 1); 642 r[0] *= f; 643 r[1] *= f; 644 r[2] *= f; 645 646 cvtest::convert( _r, arr, arr.type() ); 647 } 648 else 649 cvtest::ArrayTest::fill_array( test_case_idx, i, j, arr ); 650 } 651 652 653 int CV_RodriguesTest::prepare_test_case( int test_case_idx ) 654 { 655 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); 656 return code; 657 } 658 659 660 void CV_RodriguesTest::run_func() 661 { 662 CvMat v2m_jac, m2v_jac; 663 664 if( calc_jacobians ) 665 { 666 v2m_jac = test_mat[OUTPUT][1]; 667 m2v_jac = test_mat[OUTPUT][3]; 668 } 669 670 if( !test_cpp ) 671 { 672 CvMat _input = test_mat[INPUT][0], _output = test_mat[OUTPUT][0], _output2 = test_mat[OUTPUT][2]; 673 cvRodrigues2( &_input, &_output, calc_jacobians ? &v2m_jac : 0 ); 674 cvRodrigues2( &_output, &_output2, calc_jacobians ? &m2v_jac : 0 ); 675 } 676 else 677 { 678 cv::Mat v = test_mat[INPUT][0], M = test_mat[OUTPUT][0], v2 = test_mat[OUTPUT][2]; 679 cv::Mat M0 = M, v2_0 = v2; 680 if( !calc_jacobians ) 681 { 682 cv::Rodrigues(v, M); 683 cv::Rodrigues(M, v2); 684 } 685 else 686 { 687 cv::Mat J1 = test_mat[OUTPUT][1], J2 = test_mat[OUTPUT][3]; 688 cv::Mat J1_0 = J1, J2_0 = J2; 689 cv::Rodrigues(v, M, J1); 690 cv::Rodrigues(M, v2, J2); 691 if( J1.data != J1_0.data ) 692 { 693 if( J1.size() != J1_0.size() ) 694 J1 = J1.t(); 695 J1.convertTo(J1_0, J1_0.type()); 696 } 697 if( J2.data != J2_0.data ) 698 { 699 if( J2.size() != J2_0.size() ) 700 J2 = J2.t(); 701 J2.convertTo(J2_0, J2_0.type()); 702 } 703 } 704 if( M.data != M0.data ) 705 M.reshape(M0.channels(), M0.rows).convertTo(M0, M0.type()); 706 if( v2.data != v2_0.data ) 707 v2.reshape(v2_0.channels(), v2_0.rows).convertTo(v2_0, v2_0.type()); 708 } 709 } 710 711 712 void CV_RodriguesTest::prepare_to_validation( int /*test_case_idx*/ ) 713 { 714 const Mat& vec = test_mat[INPUT][0]; 715 Mat& m = test_mat[REF_OUTPUT][0]; 716 Mat& vec2 = test_mat[REF_OUTPUT][2]; 717 Mat* v2m_jac = 0, *m2v_jac = 0; 718 double theta0, theta1; 719 720 if( calc_jacobians ) 721 { 722 v2m_jac = &test_mat[REF_OUTPUT][1]; 723 m2v_jac = &test_mat[REF_OUTPUT][3]; 724 } 725 726 727 cvtest::Rodrigues( vec, m, v2m_jac ); 728 cvtest::Rodrigues( m, vec2, m2v_jac ); 729 cvtest::copy( vec, vec2 ); 730 731 theta0 = norm( vec2, CV_L2 ); 732 theta1 = fmod( theta0, CV_PI*2 ); 733 734 if( theta1 > CV_PI ) 735 theta1 = -(CV_PI*2 - theta1); 736 vec2 *= theta1/(theta0 ? theta0 : 1); 737 738 if( calc_jacobians ) 739 { 740 //cvInvert( v2m_jac, m2v_jac, CV_SVD ); 741 double nrm = cvtest::norm(test_mat[REF_OUTPUT][3], CV_C); 742 if( FLT_EPSILON < nrm && nrm < 1000 ) 743 { 744 gemm( test_mat[OUTPUT][1], test_mat[OUTPUT][3], 745 1, Mat(), 0, test_mat[OUTPUT][4], 746 v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T ); 747 } 748 else 749 { 750 setIdentity(test_mat[OUTPUT][4], Scalar::all(1.)); 751 cvtest::copy( test_mat[REF_OUTPUT][2], test_mat[OUTPUT][2] ); 752 } 753 setIdentity(test_mat[REF_OUTPUT][4], Scalar::all(1.)); 754 } 755 } 756 757 758 /********************************** fundamental matrix *********************************/ 759 760 class CV_FundamentalMatTest : public cvtest::ArrayTest 761 { 762 public: 763 CV_FundamentalMatTest(); 764 765 protected: 766 int read_params( CvFileStorage* fs ); 767 void fill_array( int test_case_idx, int i, int j, Mat& arr ); 768 int prepare_test_case( int test_case_idx ); 769 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 770 double get_success_error_level( int test_case_idx, int i, int j ); 771 void run_func(); 772 void prepare_to_validation( int ); 773 774 int method; 775 int img_size; 776 int cube_size; 777 int dims; 778 int f_result; 779 double min_f, max_f; 780 double sigma; 781 bool test_cpp; 782 }; 783 784 785 CV_FundamentalMatTest::CV_FundamentalMatTest() 786 { 787 // input arrays: 788 // 0, 1 - arrays of 2d points that are passed to %func%. 789 // Can have different data type, layout, be stored in homogeneous coordinates or not. 790 // 2 - array of 3d points that are projected to both view planes 791 // 3 - [R|t] matrix for the second view plane (for the first one it is [I|0] 792 // 4, 5 - intrinsic matrices 793 test_array[INPUT].push_back(NULL); 794 test_array[INPUT].push_back(NULL); 795 test_array[INPUT].push_back(NULL); 796 test_array[INPUT].push_back(NULL); 797 test_array[INPUT].push_back(NULL); 798 test_array[INPUT].push_back(NULL); 799 test_array[TEMP].push_back(NULL); 800 test_array[TEMP].push_back(NULL); 801 test_array[OUTPUT].push_back(NULL); 802 test_array[OUTPUT].push_back(NULL); 803 test_array[REF_OUTPUT].push_back(NULL); 804 test_array[REF_OUTPUT].push_back(NULL); 805 806 element_wise_relative_error = false; 807 808 method = 0; 809 img_size = 10; 810 cube_size = 10; 811 dims = 0; 812 min_f = 1; 813 max_f = 3; 814 sigma = 0;//0.1; 815 f_result = 0; 816 817 test_cpp = false; 818 } 819 820 821 int CV_FundamentalMatTest::read_params( CvFileStorage* fs ) 822 { 823 int code = cvtest::ArrayTest::read_params( fs ); 824 return code; 825 } 826 827 828 void CV_FundamentalMatTest::get_test_array_types_and_sizes( int /*test_case_idx*/, 829 vector<vector<Size> >& sizes, vector<vector<int> >& types ) 830 { 831 RNG& rng = ts->get_rng(); 832 int pt_depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 833 double pt_count_exp = cvtest::randReal(rng)*6 + 1; 834 int pt_count = cvRound(exp(pt_count_exp)); 835 836 dims = cvtest::randInt(rng) % 2 + 2; 837 method = 1 << (cvtest::randInt(rng) % 4); 838 839 if( method == CV_FM_7POINT ) 840 pt_count = 7; 841 else 842 { 843 pt_count = MAX( pt_count, 8 + (method == CV_FM_8POINT) ); 844 if( pt_count >= 8 && cvtest::randInt(rng) % 2 ) 845 method |= CV_FM_8POINT; 846 } 847 848 types[INPUT][0] = CV_MAKETYPE(pt_depth, 1); 849 850 if( cvtest::randInt(rng) % 2 ) 851 sizes[INPUT][0] = cvSize(pt_count, dims); 852 else 853 { 854 sizes[INPUT][0] = cvSize(dims, pt_count); 855 if( cvtest::randInt(rng) % 2 ) 856 { 857 types[INPUT][0] = CV_MAKETYPE(pt_depth, dims); 858 if( cvtest::randInt(rng) % 2 ) 859 sizes[INPUT][0] = cvSize(pt_count, 1); 860 else 861 sizes[INPUT][0] = cvSize(1, pt_count); 862 } 863 } 864 865 sizes[INPUT][1] = sizes[INPUT][0]; 866 types[INPUT][1] = types[INPUT][0]; 867 868 sizes[INPUT][2] = cvSize(pt_count, 1 ); 869 types[INPUT][2] = CV_64FC3; 870 871 sizes[INPUT][3] = cvSize(4,3); 872 types[INPUT][3] = CV_64FC1; 873 874 sizes[INPUT][4] = sizes[INPUT][5] = cvSize(3,3); 875 types[INPUT][4] = types[INPUT][5] = CV_MAKETYPE(CV_64F, 1); 876 877 sizes[TEMP][0] = cvSize(3,3); 878 types[TEMP][0] = CV_64FC1; 879 sizes[TEMP][1] = cvSize(pt_count,1); 880 types[TEMP][1] = CV_8UC1; 881 882 sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,1); 883 types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC1; 884 sizes[OUTPUT][1] = sizes[REF_OUTPUT][1] = cvSize(pt_count,1); 885 types[OUTPUT][1] = types[REF_OUTPUT][1] = CV_8UC1; 886 887 test_cpp = (cvtest::randInt(rng) & 256) == 0; 888 } 889 890 891 double CV_FundamentalMatTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) 892 { 893 return 1e-2; 894 } 895 896 897 void CV_FundamentalMatTest::fill_array( int test_case_idx, int i, int j, Mat& arr ) 898 { 899 double t[12]={0}; 900 RNG& rng = ts->get_rng(); 901 902 if( i != INPUT ) 903 { 904 cvtest::ArrayTest::fill_array( test_case_idx, i, j, arr ); 905 return; 906 } 907 908 switch( j ) 909 { 910 case 0: 911 case 1: 912 return; // fill them later in prepare_test_case 913 case 2: 914 { 915 double* p = arr.ptr<double>(); 916 for( i = 0; i < arr.cols*3; i += 3 ) 917 { 918 p[i] = cvtest::randReal(rng)*cube_size; 919 p[i+1] = cvtest::randReal(rng)*cube_size; 920 p[i+2] = cvtest::randReal(rng)*cube_size + cube_size; 921 } 922 } 923 break; 924 case 3: 925 { 926 double r[3]; 927 Mat rot_vec( 3, 1, CV_64F, r ); 928 Mat rot_mat( 3, 3, CV_64F, t, 4*sizeof(t[0]) ); 929 r[0] = cvtest::randReal(rng)*CV_PI*2; 930 r[1] = cvtest::randReal(rng)*CV_PI*2; 931 r[2] = cvtest::randReal(rng)*CV_PI*2; 932 933 cvtest::Rodrigues( rot_vec, rot_mat ); 934 t[3] = cvtest::randReal(rng)*cube_size; 935 t[7] = cvtest::randReal(rng)*cube_size; 936 t[11] = cvtest::randReal(rng)*cube_size; 937 Mat( 3, 4, CV_64F, t ).convertTo(arr, arr.type()); 938 } 939 break; 940 case 4: 941 case 5: 942 t[0] = t[4] = cvtest::randReal(rng)*(max_f - min_f) + min_f; 943 t[2] = (img_size*0.5 + cvtest::randReal(rng)*4. - 2.)*t[0]; 944 t[5] = (img_size*0.5 + cvtest::randReal(rng)*4. - 2.)*t[4]; 945 t[8] = 1.; 946 Mat( 3, 3, CV_64F, t ).convertTo( arr, arr.type() ); 947 break; 948 } 949 } 950 951 952 int CV_FundamentalMatTest::prepare_test_case( int test_case_idx ) 953 { 954 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); 955 if( code > 0 ) 956 { 957 const Mat& _3d = test_mat[INPUT][2]; 958 RNG& rng = ts->get_rng(); 959 double Idata[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0 }; 960 Mat I( 3, 4, CV_64F, Idata ); 961 int k; 962 963 for( k = 0; k < 2; k++ ) 964 { 965 const Mat& Rt = k == 0 ? I : test_mat[INPUT][3]; 966 const Mat& A = test_mat[INPUT][k == 0 ? 4 : 5]; 967 Mat& _2d = test_mat[INPUT][k]; 968 969 test_projectPoints( _3d, Rt, A, _2d, &rng, sigma ); 970 } 971 } 972 973 return code; 974 } 975 976 void CV_FundamentalMatTest::run_func() 977 { 978 // cvFindFundamentalMat calls cv::findFundamentalMat 979 CvMat _input0 = test_mat[INPUT][0], _input1 = test_mat[INPUT][1]; 980 CvMat F = test_mat[TEMP][0], mask = test_mat[TEMP][1]; 981 f_result = cvFindFundamentalMat( &_input0, &_input1, &F, method, MAX(sigma*3, 0.01), 0, &mask ); 982 } 983 984 985 void CV_FundamentalMatTest::prepare_to_validation( int test_case_idx ) 986 { 987 const Mat& Rt = test_mat[INPUT][3]; 988 const Mat& A1 = test_mat[INPUT][4]; 989 const Mat& A2 = test_mat[INPUT][5]; 990 double f0[9], f[9]; 991 Mat F0(3, 3, CV_64FC1, f0), F(3, 3, CV_64F, f); 992 993 Mat invA1, invA2, R=Rt.colRange(0, 3), T; 994 995 cv::invert(A1, invA1, CV_SVD); 996 cv::invert(A2, invA2, CV_SVD); 997 998 double tx = Rt.at<double>(0, 3); 999 double ty = Rt.at<double>(1, 3); 1000 double tz = Rt.at<double>(2, 3); 1001 1002 double _t_x[] = { 0, -tz, ty, tz, 0, -tx, -ty, tx, 0 }; 1003 1004 // F = (A2^-T)*[t]_x*R*(A1^-1) 1005 cv::gemm( invA2, Mat( 3, 3, CV_64F, _t_x ), 1, Mat(), 0, T, CV_GEMM_A_T ); 1006 cv::gemm( R, invA1, 1, Mat(), 0, invA2 ); 1007 cv::gemm( T, invA2, 1, Mat(), 0, F0 ); 1008 F0 *= 1./f0[8]; 1009 1010 uchar* status = test_mat[TEMP][1].ptr(); 1011 double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 ); 1012 uchar* mtfm1 = test_mat[REF_OUTPUT][1].ptr(); 1013 uchar* mtfm2 = test_mat[OUTPUT][1].ptr(); 1014 double* f_prop1 = test_mat[REF_OUTPUT][0].ptr<double>(); 1015 double* f_prop2 = test_mat[OUTPUT][0].ptr<double>(); 1016 1017 int i, pt_count = test_mat[INPUT][2].cols; 1018 Mat p1( 1, pt_count, CV_64FC2 ); 1019 Mat p2( 1, pt_count, CV_64FC2 ); 1020 1021 test_convertHomogeneous( test_mat[INPUT][0], p1 ); 1022 test_convertHomogeneous( test_mat[INPUT][1], p2 ); 1023 1024 cvtest::convert(test_mat[TEMP][0], F, F.type()); 1025 1026 if( method <= CV_FM_8POINT ) 1027 memset( status, 1, pt_count ); 1028 1029 for( i = 0; i < pt_count; i++ ) 1030 { 1031 double x1 = p1.at<Point2d>(i).x; 1032 double y1 = p1.at<Point2d>(i).y; 1033 double x2 = p2.at<Point2d>(i).x; 1034 double y2 = p2.at<Point2d>(i).y; 1035 double n1 = 1./sqrt(x1*x1 + y1*y1 + 1); 1036 double n2 = 1./sqrt(x2*x2 + y2*y2 + 1); 1037 double t0 = fabs(f0[0]*x2*x1 + f0[1]*x2*y1 + f0[2]*x2 + 1038 f0[3]*y2*x1 + f0[4]*y2*y1 + f0[5]*y2 + 1039 f0[6]*x1 + f0[7]*y1 + f0[8])*n1*n2; 1040 double t = fabs(f[0]*x2*x1 + f[1]*x2*y1 + f[2]*x2 + 1041 f[3]*y2*x1 + f[4]*y2*y1 + f[5]*y2 + 1042 f[6]*x1 + f[7]*y1 + f[8])*n1*n2; 1043 mtfm1[i] = 1; 1044 mtfm2[i] = !status[i] || t0 > err_level || t < err_level; 1045 } 1046 1047 f_prop1[0] = 1; 1048 f_prop1[1] = 1; 1049 f_prop1[2] = 0; 1050 1051 f_prop2[0] = f_result != 0; 1052 f_prop2[1] = f[8]; 1053 f_prop2[2] = cv::determinant( F ); 1054 } 1055 /******************************* find essential matrix ***********************************/ 1056 class CV_EssentialMatTest : public cvtest::ArrayTest 1057 { 1058 public: 1059 CV_EssentialMatTest(); 1060 1061 protected: 1062 int read_params( CvFileStorage* fs ); 1063 void fill_array( int test_case_idx, int i, int j, Mat& arr ); 1064 int prepare_test_case( int test_case_idx ); 1065 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 1066 double get_success_error_level( int test_case_idx, int i, int j ); 1067 void run_func(); 1068 void prepare_to_validation( int ); 1069 1070 double sampson_error(const double* f, double x1, double y1, double x2, double y2); 1071 1072 int method; 1073 int img_size; 1074 int cube_size; 1075 int dims; 1076 double min_f, max_f; 1077 double sigma; 1078 }; 1079 1080 1081 CV_EssentialMatTest::CV_EssentialMatTest() 1082 { 1083 // input arrays: 1084 // 0, 1 - arrays of 2d points that are passed to %func%. 1085 // Can have different data type, layout, be stored in homogeneous coordinates or not. 1086 // 2 - array of 3d points that are projected to both view planes 1087 // 3 - [R|t] matrix for the second view plane (for the first one it is [I|0] 1088 // 4 - intrinsic matrix for both camera 1089 test_array[INPUT].push_back(NULL); 1090 test_array[INPUT].push_back(NULL); 1091 test_array[INPUT].push_back(NULL); 1092 test_array[INPUT].push_back(NULL); 1093 test_array[INPUT].push_back(NULL); 1094 test_array[TEMP].push_back(NULL); 1095 test_array[TEMP].push_back(NULL); 1096 test_array[TEMP].push_back(NULL); 1097 test_array[TEMP].push_back(NULL); 1098 test_array[TEMP].push_back(NULL); 1099 test_array[OUTPUT].push_back(NULL); // Essential Matrix singularity 1100 test_array[OUTPUT].push_back(NULL); // Inliers mask 1101 test_array[OUTPUT].push_back(NULL); // Translation error 1102 test_array[OUTPUT].push_back(NULL); // Positive depth count 1103 test_array[REF_OUTPUT].push_back(NULL); 1104 test_array[REF_OUTPUT].push_back(NULL); 1105 test_array[REF_OUTPUT].push_back(NULL); 1106 test_array[REF_OUTPUT].push_back(NULL); 1107 1108 element_wise_relative_error = false; 1109 1110 method = 0; 1111 img_size = 10; 1112 cube_size = 10; 1113 dims = 0; 1114 min_f = 1; 1115 max_f = 3; 1116 sigma = 0; 1117 } 1118 1119 1120 int CV_EssentialMatTest::read_params( CvFileStorage* fs ) 1121 { 1122 int code = cvtest::ArrayTest::read_params( fs ); 1123 return code; 1124 } 1125 1126 1127 void CV_EssentialMatTest::get_test_array_types_and_sizes( int /*test_case_idx*/, 1128 vector<vector<Size> >& sizes, vector<vector<int> >& types ) 1129 { 1130 RNG& rng = ts->get_rng(); 1131 int pt_depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 1132 double pt_count_exp = cvtest::randReal(rng)*6 + 1; 1133 int pt_count = MAX(5, cvRound(exp(pt_count_exp))); 1134 1135 dims = cvtest::randInt(rng) % 2 + 2; 1136 dims = 2; 1137 method = CV_LMEDS << (cvtest::randInt(rng) % 2); 1138 1139 types[INPUT][0] = CV_MAKETYPE(pt_depth, 1); 1140 1141 if( 0 && cvtest::randInt(rng) % 2 ) 1142 sizes[INPUT][0] = cvSize(pt_count, dims); 1143 else 1144 { 1145 sizes[INPUT][0] = cvSize(dims, pt_count); 1146 if( cvtest::randInt(rng) % 2 ) 1147 { 1148 types[INPUT][0] = CV_MAKETYPE(pt_depth, dims); 1149 if( cvtest::randInt(rng) % 2 ) 1150 sizes[INPUT][0] = cvSize(pt_count, 1); 1151 else 1152 sizes[INPUT][0] = cvSize(1, pt_count); 1153 } 1154 } 1155 1156 sizes[INPUT][1] = sizes[INPUT][0]; 1157 types[INPUT][1] = types[INPUT][0]; 1158 1159 sizes[INPUT][2] = cvSize(pt_count, 1 ); 1160 types[INPUT][2] = CV_64FC3; 1161 1162 sizes[INPUT][3] = cvSize(4,3); 1163 types[INPUT][3] = CV_64FC1; 1164 1165 sizes[INPUT][4] = cvSize(3,3); 1166 types[INPUT][4] = CV_MAKETYPE(CV_64F, 1); 1167 1168 sizes[TEMP][0] = cvSize(3,3); 1169 types[TEMP][0] = CV_64FC1; 1170 sizes[TEMP][1] = cvSize(pt_count,1); 1171 types[TEMP][1] = CV_8UC1; 1172 sizes[TEMP][2] = cvSize(3,3); 1173 types[TEMP][2] = CV_64FC1; 1174 sizes[TEMP][3] = cvSize(3, 1); 1175 types[TEMP][3] = CV_64FC1; 1176 sizes[TEMP][4] = cvSize(pt_count,1); 1177 types[TEMP][4] = CV_8UC1; 1178 1179 sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,1); 1180 types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC1; 1181 sizes[OUTPUT][1] = sizes[REF_OUTPUT][1] = cvSize(pt_count,1); 1182 types[OUTPUT][1] = types[REF_OUTPUT][1] = CV_8UC1; 1183 sizes[OUTPUT][2] = sizes[REF_OUTPUT][2] = cvSize(1,1); 1184 types[OUTPUT][2] = types[REF_OUTPUT][2] = CV_64FC1; 1185 sizes[OUTPUT][3] = sizes[REF_OUTPUT][3] = cvSize(1,1); 1186 types[OUTPUT][3] = types[REF_OUTPUT][3] = CV_8UC1; 1187 1188 } 1189 1190 1191 double CV_EssentialMatTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) 1192 { 1193 return 1e-2; 1194 } 1195 1196 1197 void CV_EssentialMatTest::fill_array( int test_case_idx, int i, int j, Mat& arr ) 1198 { 1199 double t[12]={0}; 1200 RNG& rng = ts->get_rng(); 1201 1202 if( i != INPUT ) 1203 { 1204 cvtest::ArrayTest::fill_array( test_case_idx, i, j, arr ); 1205 return; 1206 } 1207 1208 switch( j ) 1209 { 1210 case 0: 1211 case 1: 1212 return; // fill them later in prepare_test_case 1213 case 2: 1214 { 1215 double* p = arr.ptr<double>(); 1216 for( i = 0; i < arr.cols*3; i += 3 ) 1217 { 1218 p[i] = cvtest::randReal(rng)*cube_size; 1219 p[i+1] = cvtest::randReal(rng)*cube_size; 1220 p[i+2] = cvtest::randReal(rng)*cube_size + cube_size; 1221 } 1222 } 1223 break; 1224 case 3: 1225 { 1226 double r[3]; 1227 Mat rot_vec( 3, 1, CV_64F, r ); 1228 Mat rot_mat( 3, 3, CV_64F, t, 4*sizeof(t[0]) ); 1229 r[0] = cvtest::randReal(rng)*CV_PI*2; 1230 r[1] = cvtest::randReal(rng)*CV_PI*2; 1231 r[2] = cvtest::randReal(rng)*CV_PI*2; 1232 1233 cvtest::Rodrigues( rot_vec, rot_mat ); 1234 t[3] = cvtest::randReal(rng)*cube_size; 1235 t[7] = cvtest::randReal(rng)*cube_size; 1236 t[11] = cvtest::randReal(rng)*cube_size; 1237 Mat( 3, 4, CV_64F, t ).convertTo(arr, arr.type()); 1238 } 1239 break; 1240 case 4: 1241 t[0] = t[4] = cvtest::randReal(rng)*(max_f - min_f) + min_f; 1242 t[2] = (img_size*0.5 + cvtest::randReal(rng)*4. - 2.)*t[0]; 1243 t[5] = (img_size*0.5 + cvtest::randReal(rng)*4. - 2.)*t[4]; 1244 t[8] = 1.; 1245 Mat( 3, 3, CV_64F, t ).convertTo( arr, arr.type() ); 1246 break; 1247 } 1248 } 1249 1250 1251 int CV_EssentialMatTest::prepare_test_case( int test_case_idx ) 1252 { 1253 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); 1254 if( code > 0 ) 1255 { 1256 const Mat& _3d = test_mat[INPUT][2]; 1257 RNG& rng = ts->get_rng(); 1258 double Idata[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0 }; 1259 Mat I( 3, 4, CV_64F, Idata ); 1260 int k; 1261 1262 for( k = 0; k < 2; k++ ) 1263 { 1264 const Mat& Rt = k == 0 ? I : test_mat[INPUT][3]; 1265 const Mat& A = test_mat[INPUT][4]; 1266 Mat& _2d = test_mat[INPUT][k]; 1267 1268 test_projectPoints( _3d, Rt, A, _2d, &rng, sigma ); 1269 } 1270 } 1271 1272 return code; 1273 } 1274 1275 1276 void CV_EssentialMatTest::run_func() 1277 { 1278 Mat _input0(test_mat[INPUT][0]), _input1(test_mat[INPUT][1]); 1279 Mat K(test_mat[INPUT][4]); 1280 double focal(K.at<double>(0, 0)); 1281 cv::Point2d pp(K.at<double>(0, 2), K.at<double>(1, 2)); 1282 1283 RNG& rng = ts->get_rng(); 1284 Mat E, mask1(test_mat[TEMP][1]); 1285 E = cv::findEssentialMat( _input0, _input1, focal, pp, method, 0.99, MAX(sigma*3, 0.0001), mask1 ); 1286 if (E.rows > 3) 1287 { 1288 int count = E.rows / 3; 1289 int row = (cvtest::randInt(rng) % count) * 3; 1290 E = E.rowRange(row, row + 3) * 1.0; 1291 } 1292 1293 E.copyTo(test_mat[TEMP][0]); 1294 1295 Mat R, t, mask2; 1296 recoverPose( E, _input0, _input1, R, t, focal, pp, mask2 ); 1297 R.copyTo(test_mat[TEMP][2]); 1298 t.copyTo(test_mat[TEMP][3]); 1299 mask2.copyTo(test_mat[TEMP][4]); 1300 } 1301 1302 double CV_EssentialMatTest::sampson_error(const double * f, double x1, double y1, double x2, double y2) 1303 { 1304 double Fx1[3] = { 1305 f[0] * x1 + f[1] * y1 + f[2], 1306 f[3] * x1 + f[4] * y1 + f[5], 1307 f[6] * x1 + f[7] * y1 + f[8] 1308 }; 1309 double Ftx2[3] = { 1310 f[0] * x2 + f[3] * y2 + f[6], 1311 f[1] * x2 + f[4] * y2 + f[7], 1312 f[2] * x2 + f[5] * y2 + f[8] 1313 }; 1314 double x2tFx1 = Fx1[0] * x2 + Fx1[1] * y2 + Fx1[2]; 1315 1316 double error = x2tFx1 * x2tFx1 / (Fx1[0] * Fx1[0] + Fx1[1] * Fx1[1] + Ftx2[0] * Ftx2[0] + Ftx2[1] * Ftx2[1]); 1317 error = sqrt(error); 1318 return error; 1319 1320 } 1321 1322 void CV_EssentialMatTest::prepare_to_validation( int test_case_idx ) 1323 { 1324 const Mat& Rt0 = test_mat[INPUT][3]; 1325 const Mat& A = test_mat[INPUT][4]; 1326 double f0[9], f[9], e[9]; 1327 Mat F0(3, 3, CV_64FC1, f0), F(3, 3, CV_64F, f); 1328 Mat E(3, 3, CV_64F, e); 1329 1330 Mat invA, R=Rt0.colRange(0, 3), T1, T2; 1331 1332 cv::invert(A, invA, CV_SVD); 1333 1334 double tx = Rt0.at<double>(0, 3); 1335 double ty = Rt0.at<double>(1, 3); 1336 double tz = Rt0.at<double>(2, 3); 1337 1338 double _t_x[] = { 0, -tz, ty, tz, 0, -tx, -ty, tx, 0 }; 1339 1340 // F = (A2^-T)*[t]_x*R*(A1^-1) 1341 cv::gemm( invA, Mat( 3, 3, CV_64F, _t_x ), 1, Mat(), 0, T1, CV_GEMM_A_T ); 1342 cv::gemm( R, invA, 1, Mat(), 0, T2 ); 1343 cv::gemm( T1, T2, 1, Mat(), 0, F0 ); 1344 F0 *= 1./f0[8]; 1345 1346 uchar* status = test_mat[TEMP][1].ptr(); 1347 double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 ); 1348 uchar* mtfm1 = test_mat[REF_OUTPUT][1].ptr(); 1349 uchar* mtfm2 = test_mat[OUTPUT][1].ptr(); 1350 double* e_prop1 = test_mat[REF_OUTPUT][0].ptr<double>(); 1351 double* e_prop2 = test_mat[OUTPUT][0].ptr<double>(); 1352 Mat E_prop2 = Mat(3, 1, CV_64F, e_prop2); 1353 1354 int i, pt_count = test_mat[INPUT][2].cols; 1355 Mat p1( 1, pt_count, CV_64FC2 ); 1356 Mat p2( 1, pt_count, CV_64FC2 ); 1357 1358 test_convertHomogeneous( test_mat[INPUT][0], p1 ); 1359 test_convertHomogeneous( test_mat[INPUT][1], p2 ); 1360 1361 cvtest::convert(test_mat[TEMP][0], E, E.type()); 1362 cv::gemm( invA, E, 1, Mat(), 0, T1, CV_GEMM_A_T ); 1363 cv::gemm( T1, invA, 1, Mat(), 0, F ); 1364 1365 for( i = 0; i < pt_count; i++ ) 1366 { 1367 double x1 = p1.at<Point2d>(i).x; 1368 double y1 = p1.at<Point2d>(i).y; 1369 double x2 = p2.at<Point2d>(i).x; 1370 double y2 = p2.at<Point2d>(i).y; 1371 // double t0 = sampson_error(f0, x1, y1, x2, y2); 1372 // double t = sampson_error(f, x1, y1, x2, y2); 1373 double n1 = 1./sqrt(x1*x1 + y1*y1 + 1); 1374 double n2 = 1./sqrt(x2*x2 + y2*y2 + 1); 1375 double t0 = fabs(f0[0]*x2*x1 + f0[1]*x2*y1 + f0[2]*x2 + 1376 f0[3]*y2*x1 + f0[4]*y2*y1 + f0[5]*y2 + 1377 f0[6]*x1 + f0[7]*y1 + f0[8])*n1*n2; 1378 double t = fabs(f[0]*x2*x1 + f[1]*x2*y1 + f[2]*x2 + 1379 f[3]*y2*x1 + f[4]*y2*y1 + f[5]*y2 + 1380 f[6]*x1 + f[7]*y1 + f[8])*n1*n2; 1381 mtfm1[i] = 1; 1382 mtfm2[i] = !status[i] || t0 > err_level || t < err_level; 1383 } 1384 1385 e_prop1[0] = sqrt(0.5); 1386 e_prop1[1] = sqrt(0.5); 1387 e_prop1[2] = 0; 1388 1389 e_prop2[0] = 0; 1390 e_prop2[1] = 0; 1391 e_prop2[2] = 0; 1392 SVD::compute(E, E_prop2); 1393 1394 1395 1396 double* pose_prop1 = test_mat[REF_OUTPUT][2].ptr<double>(); 1397 double* pose_prop2 = test_mat[OUTPUT][2].ptr<double>(); 1398 double terr1 = cvtest::norm(Rt0.col(3) / norm(Rt0.col(3)) + test_mat[TEMP][3], NORM_L2); 1399 double terr2 = cvtest::norm(Rt0.col(3) / norm(Rt0.col(3)) - test_mat[TEMP][3], NORM_L2); 1400 Mat rvec; 1401 Rodrigues(Rt0.colRange(0, 3), rvec); 1402 pose_prop1[0] = 0; 1403 // No check for CV_LMeDS on translation. Since it 1404 // involves with some degraded problem, when data is exact inliers. 1405 pose_prop2[0] = method == CV_LMEDS || pt_count == 5 ? 0 : MIN(terr1, terr2); 1406 1407 1408 // int inliers_count = countNonZero(test_mat[TEMP][1]); 1409 // int good_count = countNonZero(test_mat[TEMP][4]); 1410 test_mat[OUTPUT][3] = true; //good_count >= inliers_count / 2; 1411 test_mat[REF_OUTPUT][3] = true; 1412 1413 1414 } 1415 1416 1417 /********************************** convert homogeneous *********************************/ 1418 1419 class CV_ConvertHomogeneousTest : public cvtest::ArrayTest 1420 { 1421 public: 1422 CV_ConvertHomogeneousTest(); 1423 1424 protected: 1425 int read_params( CvFileStorage* fs ); 1426 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 1427 void fill_array( int test_case_idx, int i, int j, Mat& arr ); 1428 double get_success_error_level( int test_case_idx, int i, int j ); 1429 void run_func(); 1430 void prepare_to_validation( int ); 1431 1432 int dims1, dims2; 1433 int pt_count; 1434 }; 1435 1436 1437 CV_ConvertHomogeneousTest::CV_ConvertHomogeneousTest() 1438 { 1439 test_array[INPUT].push_back(NULL); 1440 test_array[OUTPUT].push_back(NULL); 1441 test_array[REF_OUTPUT].push_back(NULL); 1442 element_wise_relative_error = false; 1443 1444 pt_count = dims1 = dims2 = 0; 1445 } 1446 1447 1448 int CV_ConvertHomogeneousTest::read_params( CvFileStorage* fs ) 1449 { 1450 int code = cvtest::ArrayTest::read_params( fs ); 1451 return code; 1452 } 1453 1454 1455 void CV_ConvertHomogeneousTest::get_test_array_types_and_sizes( int /*test_case_idx*/, 1456 vector<vector<Size> >& sizes, vector<vector<int> >& types ) 1457 { 1458 RNG& rng = ts->get_rng(); 1459 int pt_depth1 = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 1460 int pt_depth2 = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 1461 double pt_count_exp = cvtest::randReal(rng)*6 + 1; 1462 int t; 1463 1464 pt_count = cvRound(exp(pt_count_exp)); 1465 pt_count = MAX( pt_count, 5 ); 1466 1467 dims1 = 2 + (cvtest::randInt(rng) % 3); 1468 dims2 = 2 + (cvtest::randInt(rng) % 3); 1469 1470 if( dims1 == dims2 + 2 ) 1471 dims1--; 1472 else if( dims1 == dims2 - 2 ) 1473 dims1++; 1474 1475 if( cvtest::randInt(rng) % 2 ) 1476 CV_SWAP( dims1, dims2, t ); 1477 1478 types[INPUT][0] = CV_MAKETYPE(pt_depth1, 1); 1479 1480 if( cvtest::randInt(rng) % 2 ) 1481 sizes[INPUT][0] = cvSize(pt_count, dims1); 1482 else 1483 { 1484 sizes[INPUT][0] = cvSize(dims1, pt_count); 1485 if( cvtest::randInt(rng) % 2 ) 1486 { 1487 types[INPUT][0] = CV_MAKETYPE(pt_depth1, dims1); 1488 if( cvtest::randInt(rng) % 2 ) 1489 sizes[INPUT][0] = cvSize(pt_count, 1); 1490 else 1491 sizes[INPUT][0] = cvSize(1, pt_count); 1492 } 1493 } 1494 1495 types[OUTPUT][0] = CV_MAKETYPE(pt_depth2, 1); 1496 1497 if( cvtest::randInt(rng) % 2 ) 1498 sizes[OUTPUT][0] = cvSize(pt_count, dims2); 1499 else 1500 { 1501 sizes[OUTPUT][0] = cvSize(dims2, pt_count); 1502 if( cvtest::randInt(rng) % 2 ) 1503 { 1504 types[OUTPUT][0] = CV_MAKETYPE(pt_depth2, dims2); 1505 if( cvtest::randInt(rng) % 2 ) 1506 sizes[OUTPUT][0] = cvSize(pt_count, 1); 1507 else 1508 sizes[OUTPUT][0] = cvSize(1, pt_count); 1509 } 1510 } 1511 1512 types[REF_OUTPUT][0] = types[OUTPUT][0]; 1513 sizes[REF_OUTPUT][0] = sizes[OUTPUT][0]; 1514 } 1515 1516 1517 double CV_ConvertHomogeneousTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) 1518 { 1519 return 1e-5; 1520 } 1521 1522 1523 void CV_ConvertHomogeneousTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, Mat& arr ) 1524 { 1525 Mat temp( 1, pt_count, CV_MAKETYPE(CV_64FC1,dims1) ); 1526 RNG& rng = ts->get_rng(); 1527 CvScalar low = cvScalarAll(0), high = cvScalarAll(10); 1528 1529 if( dims1 > dims2 ) 1530 low.val[dims1-1] = 1.; 1531 1532 cvtest::randUni( rng, temp, low, high ); 1533 test_convertHomogeneous( temp, arr ); 1534 } 1535 1536 1537 void CV_ConvertHomogeneousTest::run_func() 1538 { 1539 CvMat _input = test_mat[INPUT][0], _output = test_mat[OUTPUT][0]; 1540 cvConvertPointsHomogeneous( &_input, &_output ); 1541 } 1542 1543 1544 void CV_ConvertHomogeneousTest::prepare_to_validation( int /*test_case_idx*/ ) 1545 { 1546 test_convertHomogeneous( test_mat[INPUT][0], test_mat[REF_OUTPUT][0] ); 1547 } 1548 1549 1550 /************************** compute corresponding epipolar lines ************************/ 1551 1552 class CV_ComputeEpilinesTest : public cvtest::ArrayTest 1553 { 1554 public: 1555 CV_ComputeEpilinesTest(); 1556 1557 protected: 1558 int read_params( CvFileStorage* fs ); 1559 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types ); 1560 void fill_array( int test_case_idx, int i, int j, Mat& arr ); 1561 double get_success_error_level( int test_case_idx, int i, int j ); 1562 void run_func(); 1563 void prepare_to_validation( int ); 1564 1565 int which_image; 1566 int dims; 1567 int pt_count; 1568 }; 1569 1570 1571 CV_ComputeEpilinesTest::CV_ComputeEpilinesTest() 1572 { 1573 test_array[INPUT].push_back(NULL); 1574 test_array[INPUT].push_back(NULL); 1575 test_array[OUTPUT].push_back(NULL); 1576 test_array[REF_OUTPUT].push_back(NULL); 1577 element_wise_relative_error = false; 1578 1579 pt_count = dims = which_image = 0; 1580 } 1581 1582 1583 int CV_ComputeEpilinesTest::read_params( CvFileStorage* fs ) 1584 { 1585 int code = cvtest::ArrayTest::read_params( fs ); 1586 return code; 1587 } 1588 1589 1590 void CV_ComputeEpilinesTest::get_test_array_types_and_sizes( int /*test_case_idx*/, 1591 vector<vector<Size> >& sizes, vector<vector<int> >& types ) 1592 { 1593 RNG& rng = ts->get_rng(); 1594 int fm_depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 1595 int pt_depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 1596 int ln_depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F; 1597 double pt_count_exp = cvtest::randReal(rng)*6; 1598 1599 which_image = 1 + (cvtest::randInt(rng) % 2); 1600 1601 pt_count = cvRound(exp(pt_count_exp)); 1602 pt_count = MAX( pt_count, 1 ); 1603 bool few_points = pt_count < 5; 1604 1605 dims = 2 + (cvtest::randInt(rng) % 2); 1606 1607 types[INPUT][0] = CV_MAKETYPE(pt_depth, 1); 1608 1609 if( cvtest::randInt(rng) % 2 && !few_points ) 1610 sizes[INPUT][0] = cvSize(pt_count, dims); 1611 else 1612 { 1613 sizes[INPUT][0] = cvSize(dims, pt_count); 1614 if( cvtest::randInt(rng) % 2 || few_points ) 1615 { 1616 types[INPUT][0] = CV_MAKETYPE(pt_depth, dims); 1617 if( cvtest::randInt(rng) % 2 ) 1618 sizes[INPUT][0] = cvSize(pt_count, 1); 1619 else 1620 sizes[INPUT][0] = cvSize(1, pt_count); 1621 } 1622 } 1623 1624 types[INPUT][1] = CV_MAKETYPE(fm_depth, 1); 1625 sizes[INPUT][1] = cvSize(3, 3); 1626 1627 types[OUTPUT][0] = CV_MAKETYPE(ln_depth, 1); 1628 1629 if( cvtest::randInt(rng) % 2 && !few_points ) 1630 sizes[OUTPUT][0] = cvSize(pt_count, 3); 1631 else 1632 { 1633 sizes[OUTPUT][0] = cvSize(3, pt_count); 1634 if( cvtest::randInt(rng) % 2 || few_points ) 1635 { 1636 types[OUTPUT][0] = CV_MAKETYPE(ln_depth, 3); 1637 if( cvtest::randInt(rng) % 2 ) 1638 sizes[OUTPUT][0] = cvSize(pt_count, 1); 1639 else 1640 sizes[OUTPUT][0] = cvSize(1, pt_count); 1641 } 1642 } 1643 1644 types[REF_OUTPUT][0] = types[OUTPUT][0]; 1645 sizes[REF_OUTPUT][0] = sizes[OUTPUT][0]; 1646 } 1647 1648 1649 double CV_ComputeEpilinesTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) 1650 { 1651 return 1e-5; 1652 } 1653 1654 1655 void CV_ComputeEpilinesTest::fill_array( int test_case_idx, int i, int j, Mat& arr ) 1656 { 1657 RNG& rng = ts->get_rng(); 1658 1659 if( i == INPUT && j == 0 ) 1660 { 1661 Mat temp( 1, pt_count, CV_MAKETYPE(CV_64FC1,dims) ); 1662 cvtest::randUni( rng, temp, cvScalar(0,0,1), cvScalarAll(10) ); 1663 test_convertHomogeneous( temp, arr ); 1664 } 1665 else if( i == INPUT && j == 1 ) 1666 cvtest::randUni( rng, arr, cvScalarAll(0), cvScalarAll(10) ); 1667 else 1668 cvtest::ArrayTest::fill_array( test_case_idx, i, j, arr ); 1669 } 1670 1671 1672 void CV_ComputeEpilinesTest::run_func() 1673 { 1674 CvMat _points = test_mat[INPUT][0], _F = test_mat[INPUT][1], _lines = test_mat[OUTPUT][0]; 1675 cvComputeCorrespondEpilines( &_points, which_image, &_F, &_lines ); 1676 } 1677 1678 1679 void CV_ComputeEpilinesTest::prepare_to_validation( int /*test_case_idx*/ ) 1680 { 1681 Mat pt( 1, pt_count, CV_MAKETYPE(CV_64F, 3) ); 1682 Mat lines( 1, pt_count, CV_MAKETYPE(CV_64F, 3) ); 1683 double f[9]; 1684 Mat F( 3, 3, CV_64F, f ); 1685 1686 test_convertHomogeneous( test_mat[INPUT][0], pt ); 1687 test_mat[INPUT][1].convertTo(F, CV_64F); 1688 if( which_image == 2 ) 1689 cv::transpose( F, F ); 1690 1691 for( int i = 0; i < pt_count; i++ ) 1692 { 1693 double* p = pt.ptr<double>() + i*3; 1694 double* l = lines.ptr<double>() + i*3; 1695 double t0 = f[0]*p[0] + f[1]*p[1] + f[2]*p[2]; 1696 double t1 = f[3]*p[0] + f[4]*p[1] + f[5]*p[2]; 1697 double t2 = f[6]*p[0] + f[7]*p[1] + f[8]*p[2]; 1698 double d = sqrt(t0*t0 + t1*t1); 1699 d = d ? 1./d : 1.; 1700 l[0] = t0*d; l[1] = t1*d; l[2] = t2*d; 1701 } 1702 1703 test_convertHomogeneous( lines, test_mat[REF_OUTPUT][0] ); 1704 } 1705 1706 TEST(Calib3d_Rodrigues, accuracy) { CV_RodriguesTest test; test.safe_run(); } 1707 TEST(Calib3d_FindFundamentalMat, accuracy) { CV_FundamentalMatTest test; test.safe_run(); } 1708 TEST(Calib3d_ConvertHomogeneoous, accuracy) { CV_ConvertHomogeneousTest test; test.safe_run(); } 1709 TEST(Calib3d_ComputeEpilines, accuracy) { CV_ComputeEpilinesTest test; test.safe_run(); } 1710 TEST(Calib3d_FindEssentialMat, accuracy) { CV_EssentialMatTest test; test.safe_run(); } 1711 1712 TEST(Calib3d_FindFundamentalMat, correctMatches) 1713 { 1714 double fdata[] = {0, 0, 0, 0, 0, -1, 0, 1, 0}; 1715 double p1data[] = {200, 0, 1}; 1716 double p2data[] = {170, 0, 1}; 1717 1718 Mat F(3, 3, CV_64F, fdata); 1719 Mat p1(1, 1, CV_64FC2, p1data); 1720 Mat p2(1, 1, CV_64FC2, p2data); 1721 Mat np1, np2; 1722 1723 correctMatches(F, p1, p2, np1, np2); 1724 1725 cout << np1 << endl; 1726 cout << np2 << endl; 1727 } 1728 1729 /* End of file. */ 1730
