git.maemo.org Git - opencv/blob - tests/cv/src/afundam.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "cvtest.h"
  43
  44 int cvTsRodrigues( const CvMat* src, CvMat* dst, CvMat* jacobian )
  45 {
  46     int depth;
  47     int i;
  48     float Jf[27];
  49     double J[27];
  50     CvMat _Jf, _J = cvMat( 3, 9, CV_64F, J );
  51
  52     depth = CV_MAT_DEPTH(src->type);
  53
  54     if( jacobian )
  55     {
  56         assert( jacobian->rows == 9 && jacobian->cols == 3 ||
  57                 jacobian->rows == 3 && jacobian->cols == 9 );
  58     }
  59
  60     if( src->cols == 1 || src->rows == 1 )
  61     {
  62         double r[3], theta;
  63         CvMat _r = cvMat( src->rows, src->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(src->type)), r);
  64
  65         assert( dst->rows == 3 && dst->cols == 3 );
  66
  67         cvConvert( src, &_r );
  68
  69         theta = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
  70         if( theta < DBL_EPSILON )
  71         {
  72             cvSetIdentity( dst );
  73
  74             if( jacobian )
  75             {
  76                 memset( J, 0, sizeof(J) );
  77                 J[5] = J[15] = J[19] = 1;
  78                 J[7] = J[11] = J[21] = -1;
  79             }
  80         }
  81         else
  82         {
  83             // omega = r/theta (~[w1, w2, w3])
  84             double itheta = 1./theta;
  85             double w1 = r[0]*itheta, w2 = r[1]*itheta, w3 = r[2]*itheta;
  86             double alpha = cos(theta);
  87             double beta = sin(theta);
  88             double gamma = 1 - alpha;
  89             double omegav[] =
  90             {
  91                 0, -w3, w2,
  92                 w3, 0, -w1,
  93                 -w2, w1, 0
  94             };
  95             double A[] =
  96             {
  97                 w1*w1, w1*w2, w1*w3,
  98                 w2*w1, w2*w2, w2*w3,
  99                 w3*w1, w3*w2, w3*w3
 100             };
 101             double R[9];
 102             CvMat _omegav = cvMat(3, 3, CV_64F, omegav);
 103             CvMat _A = cvMat(3, 3, CV_64F, A);
 104             CvMat _R = cvMat(3, 3, CV_64F, R);
 105
 106             cvSetIdentity( &_R, cvRealScalar(alpha) );
 107             cvScaleAdd( &_omegav, cvRealScalar(beta), &_R, &_R );
 108             cvScaleAdd( &_A, cvRealScalar(gamma), &_R, &_R );
 109             cvConvert( &_R, dst );
 110
 111             if( jacobian )
 112             {
 113                 // m3 = [r, theta]
 114                 double dm3din[] =
 115                 {
 116                     1, 0, 0,
 117                     0, 1, 0,
 118                     0, 0, 1,
 119                     w1, w2, w3
 120                 };
 121                 // m2 = [omega, theta]
 122                 double dm2dm3[] =
 123                 {
 124                     itheta, 0, 0, -w1*itheta,
 125                     0, itheta, 0, -w2*itheta,
 126                     0, 0, itheta, -w3*itheta,
 127                     0, 0, 0, 1
 128                 };
 129                 double t0[9*4];
 130                 double dm1dm2[21*4];
 131                 double dRdm1[9*21];
 132                 CvMat _dm3din = cvMat( 4, 3, CV_64FC1, dm3din );
 133                 CvMat _dm2dm3 = cvMat( 4, 4, CV_64FC1, dm2dm3 );
 134                 CvMat _dm1dm2 = cvMat( 21, 4, CV_64FC1, dm1dm2 );
 135                 CvMat _dRdm1 = cvMat( 9, 21, CV_64FC1, dRdm1 );
 136                 CvMat _dRdm1_part;
 137                 CvMat _t0 = cvMat( 9, 4, CV_64FC1, t0 );
 138                 CvMat _t1 = cvMat( 9, 4, CV_64FC1, dRdm1 );
 139
 140                 // m1 = [alpha, beta, gamma, omegav; A]
 141                 memset( dm1dm2, 0, sizeof(dm1dm2) );
 142                 dm1dm2[3] = -beta;
 143                 dm1dm2[7] = alpha;
 144                 dm1dm2[11] = beta;
 145
 146                 // dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
 147                 //                     0 0 -1 0 0 0 1 0 0;
 148                 //                     0 1 0 -1 0 0 0 0 0]'
 149                 //                     -------------------
 150                 //                     0 0 0  0 0 0 0 0 0
 151                 dm1dm2[12 + 6] = dm1dm2[12 + 20] = dm1dm2[12 + 25] = 1;
 152                 dm1dm2[12 + 9] = dm1dm2[12 + 14] = dm1dm2[12 + 28] = -1;
 153
 154                 double dm1dw[] =
 155                 {
 156                     2*w1, w2, w3, w2, 0, 0, w3, 0, 0,
 157                     0, w1, 0, w1, 2*w2, w3, 0, w3, 0,
 158                     0, 0, w1, 0, 0, w2, w1, w2, 2*w3
 159                 };
 160
 161                 CvMat _dm1dw = cvMat( 3, 9, CV_64FC1, dm1dw );
 162                 CvMat _dm1dm2_part;
 163
 164                 cvGetSubRect( &_dm1dm2, &_dm1dm2_part, cvRect(0,12,3,9) );
 165                 cvTranspose( &_dm1dw, &_dm1dm2_part );
 166
 167                 memset( dRdm1, 0, sizeof(dRdm1) );
 168                 dRdm1[0*21] = dRdm1[4*21] = dRdm1[8*21] = 1;
 169
 170                 cvGetCol( &_dRdm1, &_dRdm1_part, 1 );
 171                 cvTranspose( &_omegav, &_omegav );
 172                 cvReshape( &_omegav, &_omegav, 1, 1 );
 173                 cvTranspose( &_omegav, &_dRdm1_part );
 174
 175                 cvGetCol( &_dRdm1, &_dRdm1_part, 2 );
 176                 cvReshape( &_A, &_A, 1, 1 );
 177                 cvTranspose( &_A, &_dRdm1_part );
 178
 179                 cvGetSubRect( &_dRdm1, &_dRdm1_part, cvRect(3,0,9,9) );
 180                 cvSetIdentity( &_dRdm1_part, cvScalarAll(beta) );
 181
 182                 cvGetSubRect( &_dRdm1, &_dRdm1_part, cvRect(12,0,9,9) );
 183                 cvSetIdentity( &_dRdm1_part, cvScalarAll(gamma) );
 184
 185                 _J = cvMat( 9, 3, CV_64FC1, J );
 186
 187                 cvMatMul( &_dRdm1, &_dm1dm2, &_t0 );
 188                 cvMatMul( &_t0, &_dm2dm3, &_t1 );
 189                 cvMatMul( &_t1, &_dm3din, &_J );
 190
 191                 _t0 = cvMat( 3, 9, CV_64FC1, t0 );
 192                 cvTranspose( &_J, &_t0 );
 193
 194                 for( i = 0; i < 3; i++ )
 195                 {
 196                     _t1 = cvMat( 3, 3, CV_64FC1, t0 + i*9 );
 197                     cvTranspose( &_t1, &_t1 );
 198                 }
 199
 200                 cvTranspose( &_t0, &_J );
 201             }
 202         }
 203     }
 204     else if( src->cols == 3 && src->rows == 3 )
 205     {
 206         double R[9], A[9], I[9], r[3], W[3], U[9], V[9];
 207         double tr, alpha, beta, theta;
 208         CvMat _R = cvMat( 3, 3, CV_64F, R );
 209         CvMat _A = cvMat( 3, 3, CV_64F, A );
 210         CvMat _I = cvMat( 3, 3, CV_64F, I );
 211         CvMat _r = cvMat( dst->rows, dst->cols, CV_MAKETYPE(CV_64F, CV_MAT_CN(dst->type)), r );
 212         CvMat _W = cvMat( 1, 3, CV_64F, W );
 213         CvMat _U = cvMat( 3, 3, CV_64F, U );
 214         CvMat _V = cvMat( 3, 3, CV_64F, V );
 215
 216         cvConvert( src, &_R );
 217         cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 218         cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
 219
 220         cvMulTransposed( &_R, &_A, 0 );
 221         cvSetIdentity( &_I );
 222
 223         if( cvNorm( &_A, &_I, CV_C ) > 1e-3 ||
 224             fabs( cvDet(&_R) - 1 ) > 1e-3 )
 225             return 0;
 226
 227         tr = (cvTrace(&_R).val[0] - 1.)*0.5;
 228         tr = tr > 1. ? 1. : tr < -1. ? -1. : tr;
 229         theta = acos(tr);
 230         alpha = cos(theta);
 231         beta = sin(theta);
 232
 233         if( beta >= 1e-5 )
 234         {
 235             double dtheta_dtr = -1./sqrt(1 - tr*tr);
 236             double vth = 1/(2*beta);
 237
 238             // om1 = [R(3,2) - R(2,3), R(1,3) - R(3,1), R(2,1) - R(1,2)]'
 239             double om1[] = { R[7] - R[5], R[2] - R[6], R[3] - R[1] };
 240             // om = om1*vth
 241             // r = om*theta
 242             double d3 = vth*theta;
 243
 244             r[0] = om1[0]*d3; r[1] = om1[1]*d3; r[2] = om1[2]*d3;
 245             cvConvert( &_r, dst );
 246
 247             if( jacobian )
 248             {
 249                 // var1 = [vth;theta]
 250                 // var = [om1;var1] = [om1;vth;theta]
 251                 double dvth_dtheta = -vth*alpha/beta;
 252                 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
 253                 double d2 = 0.5*dtheta_dtr;
 254                 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
 255                 double dvardR[5*9] =
 256                 {
 257                     0, 0, 0, 0, 0, 1, 0, -1, 0,
 258                     0, 0, -1, 0, 0, 0, 1, 0, 0,
 259                     0, 1, 0, -1, 0, 0, 0, 0, 0,
 260                     d1, 0, 0, 0, d1, 0, 0, 0, d1,
 261                     d2, 0, 0, 0, d2, 0, 0, 0, d2
 262                 };
 263                 // var2 = [om;theta]
 264                 double dvar2dvar[] =
 265                 {
 266                     vth, 0, 0, om1[0], 0,
 267                     0, vth, 0, om1[1], 0,
 268                     0, 0, vth, om1[2], 0,
 269                     0, 0, 0, 0, 1
 270                 };
 271                 double domegadvar2[] =
 272                 {
 273                     theta, 0, 0, om1[0]*vth,
 274                     0, theta, 0, om1[1]*vth,
 275                     0, 0, theta, om1[2]*vth
 276                 };
 277
 278                 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
 279                 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
 280                 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
 281                 double t0[3*5];
 282                 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
 283
 284                 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
 285                 cvMatMul( &_t0, &_dvardR, &_J );
 286             }
 287         }
 288         else if( tr > 0 )
 289         {
 290             cvZero( dst );
 291             if( jacobian )
 292             {
 293                 memset( J, 0, sizeof(J) );
 294                 J[5] = J[15] = J[19] = 0.5;
 295                 J[7] = J[11] = J[21] = -0.5;
 296             }
 297         }
 298         else
 299         {
 300             r[0] = theta*sqrt((R[0] + 1)*0.5);
 301             r[1] = theta*sqrt((R[4] + 1)*0.5)*(R[1] >= 0 ? 1 : -1);
 302             r[2] = theta*sqrt((R[8] + 1)*0.5)*(R[2] >= 0 ? 1 : -1);
 303             cvConvert( &_r, dst );
 304
 305             if( jacobian )
 306                 memset( J, 0, sizeof(J) );
 307         }
 308
 309         if( jacobian )
 310         {
 311             for( i = 0; i < 3; i++ )
 312             {
 313                 CvMat t = cvMat( 3, 3, CV_64F, J + i*9 );
 314                 cvTranspose( &t, &t );
 315             }
 316         }
 317     }
 318     else
 319     {
 320         assert(0);
 321         return 0;
 322     }
 323
 324     if( jacobian )
 325     {
 326         if( depth == CV_32F )
 327         {
 328             if( jacobian->rows == _J.rows )
 329                 cvConvert( &_J, jacobian );
 330             else
 331             {
 332                 _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf );
 333                 cvConvert( &_J, &_Jf );
 334                 cvTranspose( &_Jf, jacobian );
 335             }
 336         }
 337         else if( jacobian->rows == _J.rows )
 338             cvCopy( &_J, jacobian );
 339         else
 340             cvTranspose( &_J, jacobian );
 341     }
 342
 343     return 1;
 344 }
 345
 346
 347 void
 348 cvTsConvertHomogenious( const CvMat* src, CvMat* dst )
 349 {
 350     CvMat* src_buf = 0;
 351     CvMat* dst_buf = 0;
 352     CvMat* dst0 = dst;
 353     int i, count, sdims, ddims;
 354     int sstep1, sstep2, dstep1, dstep2;
 355     double *s, *d;
 356
 357     if( CV_MAT_DEPTH(src->type) != CV_64F )
 358     {
 359         src_buf = cvCreateMat( src->rows, src->cols, CV_MAKETYPE(CV_64F, CV_MAT_CN(src->type)) );
 360         cvTsConvert( src, src_buf );
 361         src = src_buf;
 362     }
 363
 364     if( CV_MAT_DEPTH(dst->type) != CV_64F )
 365     {
 366         dst_buf = cvCreateMat( dst->rows, dst->cols, CV_MAKETYPE(CV_64F, CV_MAT_CN(dst->type)) );
 367         dst = dst_buf;
 368     }
 369
 370     if( src->rows > src->cols )
 371     {
 372         count = src->rows;
 373         sdims = CV_MAT_CN(src->type)*src->cols;
 374         sstep1 = src->step/sizeof(double);
 375         sstep2 = 1;
 376     }
 377     else
 378     {
 379         count = src->cols;
 380         sdims = CV_MAT_CN(src->type)*src->rows;
 381         if( src->rows == 1 )
 382         {
 383             sstep1 = sdims;
 384             sstep2 = 1;
 385         }
 386         else
 387         {
 388             sstep1 = 1;
 389             sstep2 = src->step/sizeof(double);
 390         }
 391     }
 392
 393     if( dst->rows > dst->cols )
 394     {
 395         assert( count == dst->rows );
 396         ddims = CV_MAT_CN(dst->type)*dst->cols;
 397         dstep1 = dst->step/sizeof(double);
 398         dstep2 = 1;
 399     }
 400     else
 401     {
 402         assert( count == dst->cols );
 403         ddims = CV_MAT_CN(dst->type)*dst->rows;
 404         if( dst->rows == 1 )
 405         {
 406             dstep1 = ddims;
 407             dstep2 = 1;
 408         }
 409         else
 410         {
 411             dstep1 = 1;
 412             dstep2 = dst->step/sizeof(double);
 413         }
 414     }
 415
 416     s = src->data.db;
 417     d = dst->data.db;
 418
 419     if( sdims <= ddims )
 420     {
 421         int wstep = dstep2*(ddims - 1);
 422
 423         for( i = 0; i < count; i++, s += sstep1, d += dstep1 )
 424         {
 425             double x = s[0];
 426             double y = s[sstep2];
 427
 428             d[wstep] = 1;
 429             d[0] = x;
 430             d[dstep2] = y;
 431
 432             if( sdims >= 3 )
 433             {
 434                 d[dstep2*2] = s[sstep2*2];
 435                 if( sdims == 4 )
 436                     d[dstep2*3] = s[sstep2*3];
 437             }
 438         }
 439     }
 440     else
 441     {
 442         int wstep = sstep2*(sdims - 1);
 443
 444         for( i = 0; i < count; i++, s += sstep1, d += dstep1 )
 445         {
 446             double w = s[wstep];
 447             double x = s[0];
 448             double y = s[sstep2];
 449
 450             w = w ? 1./w : 1;
 451
 452             d[0] = x*w;
 453             d[dstep2] = y*w;
 454
 455             if( ddims == 3 )
 456                 d[dstep2*2] = s[sstep2*2]*w;
 457         }
 458     }
 459
 460     if( dst != dst0 )
 461         cvTsConvert( dst, dst0 );
 462
 463     cvReleaseMat( &src_buf );
 464     cvReleaseMat( &dst_buf );
 465 }
 466
 467
 468 void
 469 cvTsProjectPoints( const CvMat* _3d, const CvMat* Rt, const CvMat* A,
 470                    CvMat* _2d, CvRNG* rng, double sigma )
 471 {
 472     double p[12];
 473     CvMat P = cvMat( 3, 4, CV_64F, p );
 474
 475     int i, count = _3d->cols;
 476
 477     CvMat* temp;
 478     CvMat* noise = 0;
 479
 480     cvMatMul( A, Rt, &P );
 481
 482     if( rng )
 483     {
 484         if( sigma == 0 )
 485             rng = 0;
 486         else
 487         {
 488             noise = cvCreateMat( 1, _3d->cols, CV_64FC2 );
 489             cvRandArr( rng, noise, CV_RAND_NORMAL, cvScalarAll(0), cvScalarAll(sigma) );
 490         }
 491     }
 492
 493     temp = cvCreateMat( 1, count, CV_64FC3 );
 494
 495     for( i = 0; i < count; i++ )
 496     {
 497         const double* M = _3d->data.db + i*3;
 498         double* m = temp->data.db + i*3;
 499         double X = M[0], Y = M[1], Z = M[2];
 500         double u = p[0]*X + p[1]*Y + p[2]*Z + p[3];
 501         double v = p[4]*X + p[5]*Y + p[6]*Z + p[7];
 502         double s = p[8]*X + p[9]*Y + p[10]*Z + p[11];
 503
 504         if( noise )
 505         {
 506             u += noise->data.db[i*2]*s;
 507             v += noise->data.db[i*2+1]*s;
 508         }
 509
 510         m[0] = u;
 511         m[1] = v;
 512         m[2] = s;
 513     }
 514
 515     cvTsConvertHomogenious( temp, _2d );
 516     cvReleaseMat( &noise );
 517     cvReleaseMat( &temp );
 518 }
 519
 520
 521 /********************************** Rodrigues transform ********************************/
 522
 523 class CV_RodriguesTest : public CvArrTest
 524 {
 525 public:
 526     CV_RodriguesTest();
 527
 528 protected:
 529     int read_params( CvFileStorage* fs );
 530     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
 531     int prepare_test_case( int test_case_idx );
 532     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
 533     double get_success_error_level( int test_case_idx, int i, int j );
 534     void run_func();
 535     void prepare_to_validation( int );
 536
 537     bool calc_jacobians;
 538 };
 539
 540
 541 CV_RodriguesTest::CV_RodriguesTest()
 542     : CvArrTest( "_3d-rodrigues", "cvRodrigues2", "" )
 543 {
 544     test_array[INPUT].push(NULL);  // rotation vector
 545     test_array[OUTPUT].push(NULL); // rotation matrix
 546     test_array[OUTPUT].push(NULL); // jacobian (J)
 547     test_array[OUTPUT].push(NULL); // rotation vector (backward transform result)
 548     test_array[OUTPUT].push(NULL); // inverse transform jacobian (J1)
 549     test_array[OUTPUT].push(NULL); // J*J1 (or J1*J) == I(3x3)
 550     test_array[REF_OUTPUT].push(NULL);
 551     test_array[REF_OUTPUT].push(NULL);
 552     test_array[REF_OUTPUT].push(NULL);
 553     test_array[REF_OUTPUT].push(NULL);
 554     test_array[REF_OUTPUT].push(NULL);
 555
 556     element_wise_relative_error = false;
 557     calc_jacobians = false;
 558
 559     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
 560     default_timing_param_names = 0;
 561 }
 562
 563
 564 int CV_RodriguesTest::read_params( CvFileStorage* fs )
 565 {
 566     int code = CvArrTest::read_params( fs );
 567     return code;
 568 }
 569
 570
 571 void CV_RodriguesTest::get_test_array_types_and_sizes(
 572     int /*test_case_idx*/, CvSize** sizes, int** types )
 573 {
 574     CvRNG* rng = ts->get_rng();
 575     int depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
 576     int i, code;
 577
 578     code = cvTsRandInt(rng) % 3;
 579     types[INPUT][0] = CV_MAKETYPE(depth, 1);
 580
 581     if( code == 0 )
 582     {
 583         sizes[INPUT][0] = cvSize(1,1);
 584         types[INPUT][0] = CV_MAKETYPE(depth, 3);
 585     }
 586     else if( code == 1 )
 587         sizes[INPUT][0] = cvSize(3,1);
 588     else
 589         sizes[INPUT][0] = cvSize(1,3);
 590
 591     sizes[OUTPUT][0] = cvSize(3, 3);
 592     types[OUTPUT][0] = CV_MAKETYPE(depth, 1);
 593
 594     types[OUTPUT][1] = CV_MAKETYPE(depth, 1);
 595
 596     if( cvTsRandInt(rng) % 2 )
 597         sizes[OUTPUT][1] = cvSize(3,9);
 598     else
 599         sizes[OUTPUT][1] = cvSize(9,3);
 600
 601     types[OUTPUT][2] = types[INPUT][0];
 602     sizes[OUTPUT][2] = sizes[INPUT][0];
 603
 604     types[OUTPUT][3] = types[OUTPUT][1];
 605     sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width);
 606
 607     types[OUTPUT][4] = types[OUTPUT][1];
 608     sizes[OUTPUT][4] = cvSize(3,3);
 609
 610     calc_jacobians = 1;//cvTsRandInt(rng) % 3 != 0;
 611     if( !calc_jacobians )
 612         sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0);
 613
 614     for( i = 0; i < 5; i++ )
 615     {
 616         types[REF_OUTPUT][i] = types[OUTPUT][i];
 617         sizes[REF_OUTPUT][i] = sizes[OUTPUT][i];
 618     }
 619 }
 620
 621
 622 double CV_RodriguesTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j )
 623 {
 624     return j == 4 ? 1e-2 : 1e-2;
 625 }
 626
 627
 628 void CV_RodriguesTest::fill_array( int test_case_idx, int i, int j, CvMat* arr )
 629 {
 630     if( i == INPUT && j == 0 )
 631     {
 632         double r[3], theta0, theta1, f;
 633         CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r );
 634         CvRNG* rng = ts->get_rng();
 635
 636         r[0] = cvTsRandReal(rng)*CV_PI*2;
 637         r[1] = cvTsRandReal(rng)*CV_PI*2;
 638         r[2] = cvTsRandReal(rng)*CV_PI*2;
 639
 640         theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
 641         theta1 = fmod(theta0, CV_PI*2);
 642
 643         if( theta1 > CV_PI )
 644             theta1 = -(CV_PI*2 - theta1);
 645
 646         f = theta1/(theta0 ? theta0 : 1);
 647         r[0] *= f;
 648         r[1] *= f;
 649         r[2] *= f;
 650
 651         cvTsConvert( &_r, arr );
 652     }
 653     else
 654         CvArrTest::fill_array( test_case_idx, i, j, arr );
 655 }
 656
 657
 658 int CV_RodriguesTest::prepare_test_case( int test_case_idx )
 659 {
 660     int code = CvArrTest::prepare_test_case( test_case_idx );
 661     return code;
 662 }
 663
 664
 665 void CV_RodriguesTest::run_func()
 666 {
 667     CvMat *v2m_jac = 0, *m2v_jac = 0;
 668     if( calc_jacobians )
 669     {
 670         v2m_jac = &test_mat[OUTPUT][1];
 671         m2v_jac = &test_mat[OUTPUT][3];
 672     }
 673
 674     cvRodrigues2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac );
 675     cvRodrigues2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac );
 676 }
 677
 678
 679 void CV_RodriguesTest::prepare_to_validation( int /*test_case_idx*/ )
 680 {
 681     const CvMat* vec = &test_mat[INPUT][0];
 682     CvMat* m = &test_mat[REF_OUTPUT][0];
 683     CvMat* vec2 = &test_mat[REF_OUTPUT][2];
 684     CvMat* v2m_jac = 0, *m2v_jac = 0;
 685     double theta0, theta1;
 686
 687     if( calc_jacobians )
 688     {
 689         v2m_jac = &test_mat[REF_OUTPUT][1];
 690         m2v_jac = &test_mat[REF_OUTPUT][3];
 691     }
 692
 693
 694     cvTsRodrigues( vec, m, v2m_jac );
 695     cvTsRodrigues( m, vec2, m2v_jac );
 696     cvTsCopy( vec, vec2 );
 697
 698     theta0 = cvNorm( vec2, 0, CV_L2 );
 699     theta1 = fmod( theta0, CV_PI*2 );
 700
 701     if( theta1 > CV_PI )
 702         theta1 = -(CV_PI*2 - theta1);
 703     cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) );
 704
 705     if( calc_jacobians )
 706     {
 707         //cvInvert( v2m_jac, m2v_jac, CV_SVD );
 708         if( cvNorm(&test_mat[OUTPUT][3],0,CV_C) < 1000 )
 709         {
 710             cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3],
 711                       1, 0, 0, &test_mat[OUTPUT][4],
 712                       v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T );
 713         }
 714         else
 715         {
 716             cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) );
 717             cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] );
 718         }
 719         cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) );
 720     }
 721 }
 722
 723
 724 CV_RodriguesTest rodrigues_test;
 725
 726
 727 /********************************** fundamental matrix *********************************/
 728
 729 class CV_FundamentalMatTest : public CvArrTest
 730 {
 731 public:
 732     CV_FundamentalMatTest();
 733
 734 protected:
 735     int read_params( CvFileStorage* fs );
 736     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
 737     int prepare_test_case( int test_case_idx );
 738     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
 739     double get_success_error_level( int test_case_idx, int i, int j );
 740     void run_func();
 741     void prepare_to_validation( int );
 742
 743     int method;
 744     int img_size;
 745     int cube_size;
 746     int dims;
 747     int f_result;
 748     double min_f, max_f;
 749     double sigma;
 750 };
 751
 752
 753 CV_FundamentalMatTest::CV_FundamentalMatTest()
 754     : CvArrTest( "_3d-fundam", "cvFindFundamentalMatrix", "" )
 755 {
 756     // input arrays:
 757     //   0, 1 - arrays of 2d points that are passed to %func%.
 758     //          Can have different data type, layout, be stored in homogenious coordinates or not.
 759     //   2 - array of 3d points that are projected to both view planes
 760     //   3 - [R|t] matrix for the second view plane (for the first one it is [I|0]
 761     //   4, 5 - intrinsic matrices
 762     test_array[INPUT].push(NULL);
 763     test_array[INPUT].push(NULL);
 764     test_array[INPUT].push(NULL);
 765     test_array[INPUT].push(NULL);
 766     test_array[INPUT].push(NULL);
 767     test_array[INPUT].push(NULL);
 768     test_array[TEMP].push(NULL);
 769     test_array[TEMP].push(NULL);
 770     test_array[OUTPUT].push(NULL);
 771     test_array[OUTPUT].push(NULL);
 772     test_array[REF_OUTPUT].push(NULL);
 773     test_array[REF_OUTPUT].push(NULL);
 774
 775     element_wise_relative_error = false;
 776
 777     method = 0;
 778     img_size = 10;
 779     cube_size = 10;
 780     min_f = 1;
 781     max_f = 3;
 782     sigma = 0;//0.1;
 783     f_result = 0;
 784
 785     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
 786     default_timing_param_names = 0;
 787 }
 788
 789
 790 int CV_FundamentalMatTest::read_params( CvFileStorage* fs )
 791 {
 792     int code = CvArrTest::read_params( fs );
 793     return code;
 794 }
 795
 796
 797 void CV_FundamentalMatTest::get_test_array_types_and_sizes( int /*test_case_idx*/,
 798                                                 CvSize** sizes, int** types )
 799 {
 800     CvRNG* rng = ts->get_rng();
 801     int pt_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
 802     double pt_count_exp = cvTsRandReal(rng)*6 + 1;
 803     int pt_count = cvRound(exp(pt_count_exp));
 804
 805     dims = cvTsRandInt(rng) % 2 + 2;
 806     method = 1 << (cvTsRandInt(rng) % 4);
 807
 808     if( method == CV_FM_7POINT )
 809         pt_count = 7;
 810     else
 811     {
 812         pt_count = MAX( pt_count, 8 + (method == CV_FM_8POINT) );
 813         if( pt_count >= 8 && cvTsRandInt(rng) % 2 )
 814             method |= CV_FM_8POINT;
 815     }
 816
 817     types[INPUT][0] = CV_MAKETYPE(pt_depth, 1);
 818
 819     if( cvTsRandInt(rng) % 2 )
 820         sizes[INPUT][0] = cvSize(pt_count, dims);
 821     else
 822     {
 823         sizes[INPUT][0] = cvSize(dims, pt_count);
 824         if( cvTsRandInt(rng) % 2 )
 825         {
 826             types[INPUT][0] = CV_MAKETYPE(pt_depth, dims);
 827             if( cvTsRandInt(rng) % 2 )
 828                 sizes[INPUT][0] = cvSize(pt_count, 1);
 829             else
 830                 sizes[INPUT][0] = cvSize(1, pt_count);
 831         }
 832     }
 833
 834     sizes[INPUT][1] = sizes[INPUT][0];
 835     types[INPUT][1] = types[INPUT][0];
 836
 837     sizes[INPUT][2] = cvSize(pt_count, 1 );
 838     types[INPUT][2] = CV_64FC3;
 839
 840     sizes[INPUT][3] = cvSize(4,3);
 841     types[INPUT][3] = CV_64FC1;
 842
 843     sizes[INPUT][4] = sizes[INPUT][5] = cvSize(3,3);
 844     types[INPUT][4] = types[INPUT][5] = CV_MAKETYPE(CV_64F, 1);
 845
 846     sizes[TEMP][0] = cvSize(3,3);
 847     types[TEMP][0] = CV_64FC1;
 848     sizes[TEMP][1] = cvSize(pt_count,1);
 849     types[TEMP][1] = CV_8UC1;
 850
 851     sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,1);
 852     types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC1;
 853     sizes[OUTPUT][1] = sizes[REF_OUTPUT][1] = cvSize(pt_count,1);
 854     types[OUTPUT][1] = types[REF_OUTPUT][1] = CV_8UC1;
 855 }
 856
 857
 858 double CV_FundamentalMatTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
 859 {
 860     return 3e-2;
 861 }
 862
 863
 864 void CV_FundamentalMatTest::fill_array( int test_case_idx, int i, int j, CvMat* arr )
 865 {
 866     double t[12];
 867     CvMat T;
 868     double* p = arr->data.db;
 869     CvRNG* rng = ts->get_rng();
 870
 871     if( i != INPUT )
 872     {
 873         CvArrTest::fill_array( test_case_idx, i, j, arr );
 874         return;
 875     }
 876
 877     switch( j )
 878     {
 879     case 0:
 880     case 1:
 881         return; // fill them later in prepare_test_case
 882     case 2:
 883         for( i = 0; i < arr->cols*3; i += 3 )
 884         {
 885             p[i] = cvTsRandReal(rng)*cube_size;
 886             p[i+1] = cvTsRandReal(rng)*cube_size;
 887             p[i+2] = cvTsRandReal(rng)*cube_size + cube_size;
 888         }
 889         break;
 890     case 3:
 891         {
 892         double r[3];
 893         CvMat rot_vec = cvMat( 3, 1, CV_64F, r );
 894         CvMat rot_mat = cvMat( 3, 3, CV_64F );
 895         r[0] = cvTsRandReal(rng)*CV_PI*2;
 896         r[1] = cvTsRandReal(rng)*CV_PI*2;
 897         r[2] = cvTsRandReal(rng)*CV_PI*2;
 898
 899         cvSetData( &rot_mat, t, 4*sizeof(t[0]) );
 900         cvTsRodrigues( &rot_vec, &rot_mat );
 901         t[3] = cvTsRandReal(rng)*cube_size;
 902         t[7] = cvTsRandReal(rng)*cube_size;
 903         t[11] = cvTsRandReal(rng)*cube_size;
 904         T = cvMat( 3, 4, CV_64F, t );
 905         cvTsConvert( &T, arr );
 906         }
 907         break;
 908     case 4:
 909     case 5:
 910         memset( t, 0, sizeof(t) );
 911         t[0] = t[4] = cvTsRandReal(rng)*(max_f - min_f) + min_f;
 912         t[2] = (img_size*0.5 + cvTsRandReal(rng)*4. - 2.)*t[0];
 913         t[5] = (img_size*0.5 + cvTsRandReal(rng)*4. - 2.)*t[4];
 914         t[8] = 1.;
 915         T = cvMat( 3, 3, CV_64F, t );
 916         cvTsConvert( &T, arr );
 917         break;
 918     }
 919 }
 920
 921
 922 int CV_FundamentalMatTest::prepare_test_case( int test_case_idx )
 923 {
 924     int code = CvArrTest::prepare_test_case( test_case_idx );
 925     if( code > 0 )
 926     {
 927         const CvMat* _3d = &test_mat[INPUT][2];
 928         CvRNG* rng = ts->get_rng();
 929         double Idata[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0 };
 930         CvMat I = cvMat( 3, 4, CV_64F, Idata );
 931         int k;
 932
 933         for( k = 0; k < 2; k++ )
 934         {
 935             const CvMat* Rt = k == 0 ? &I : &test_mat[INPUT][3];
 936             const CvMat* A = &test_mat[INPUT][k == 0 ? 4 : 5];
 937             CvMat* _2d = &test_mat[INPUT][k];
 938
 939             cvTsProjectPoints( _3d, Rt, A, _2d, rng, sigma );
 940         }
 941     }
 942
 943     return code;
 944 }
 945
 946
 947 void CV_FundamentalMatTest::run_func()
 948 {
 949     f_result = cvFindFundamentalMat( &test_mat[INPUT][0], &test_mat[INPUT][1],
 950             &test_mat[TEMP][0], method, MAX(sigma*3, 0.01), 0, &test_mat[TEMP][1] );
 951 }
 952
 953
 954 void CV_FundamentalMatTest::prepare_to_validation( int test_case_idx )
 955 {
 956     const CvMat* Rt = &test_mat[INPUT][3];
 957     const CvMat* A1 = &test_mat[INPUT][4];
 958     const CvMat* A2 = &test_mat[INPUT][5];
 959     double f0[9];
 960     CvMat F0 = cvMat( 3, 3, CV_64FC1, f0 );
 961
 962     double _invA1[9], _invA2[9], temp[9];
 963     CvMat invA1 = cvMat( 3, 3, CV_64F, _invA1 );
 964     CvMat invA2 = cvMat( 3, 3, CV_64F, _invA2 );
 965     CvMat R = cvMat( 3, 3, CV_64F );
 966     CvMat T = cvMat( 3, 3, CV_64F, temp );
 967
 968     cvSetData( &R, Rt->data.db, Rt->step ); // R = Rt(:,1:3)
 969
 970     // F = (A2^-T)*[t]_x*R*(A1^-1)
 971     cvInvert( A1, &invA1, CV_SVD );
 972     cvInvert( A2, &invA2, CV_SVD );
 973
 974     {
 975     double tx = ((double*)(Rt->data.ptr))[3];
 976     double ty = ((double*)(Rt->data.ptr + Rt->step))[3];
 977     double tz = ((double*)(Rt->data.ptr + Rt->step*2))[3];
 978
 979     double _t_x[] = { 0, -tz, ty, tz, 0, -tx, -ty, tx, 0 };
 980     CvMat t_x = cvMat( 3, 3, CV_64F, _t_x );
 981
 982     cvGEMM( &invA2, &t_x, 1, 0, 0, &T, CV_GEMM_A_T );
 983     cvMatMul( &R, &invA1, &invA2 );
 984     cvMatMul( &T, &invA2, &F0 );
 985     cvScale( &F0, &F0, f0[8] );
 986     }
 987
 988     double f[9];
 989     CvMat F = cvMat(3, 3, CV_64F, f);
 990     uchar* status = test_mat[TEMP][1].data.ptr;
 991     double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 );
 992     uchar* mtfm1 = test_mat[REF_OUTPUT][1].data.ptr;
 993     uchar* mtfm2 = test_mat[OUTPUT][1].data.ptr;
 994     double* f_prop1 = test_mat[REF_OUTPUT][0].data.db;
 995     double* f_prop2 = test_mat[OUTPUT][0].data.db;
 996
 997     int i, pt_count = test_mat[INPUT][2].cols;
 998     CvMat* p1 = cvCreateMat( 1, pt_count, CV_64FC2 );
 999     CvMat* p2 = cvCreateMat( 1, pt_count, CV_64FC2 );
1000
1001     cvTsConvertHomogenious( &test_mat[INPUT][0], p1 );
1002     cvTsConvertHomogenious( &test_mat[INPUT][1], p2 );
1003
1004     cvTsConvert( &test_mat[TEMP][0], &F );
1005
1006     if( method <= CV_FM_8POINT )
1007         memset( status, 1, pt_count );
1008
1009     for( i = 0; i < pt_count; i++ )
1010     {
1011         double x1 = p1->data.db[i*2];
1012         double y1 = p1->data.db[i*2+1];
1013         double x2 = p2->data.db[i*2];
1014         double y2 = p2->data.db[i*2+1];
1015         double t0 = fabs(f0[0]*x2*x1 + f0[1]*x2*y1 + f0[2]*x2 +
1016                    f0[3]*y2*x1 + f0[4]*y2*y1 + f0[5]*y2 +
1017                    f0[6]*x1 + f0[7]*y1 + f0[8]);
1018         double t = fabs(f[0]*x2*x1 + f[1]*x2*y1 + f[2]*x2 +
1019                    f[3]*y2*x1 + f[4]*y2*y1 + f[5]*y2 +
1020                    f[6]*x1 + f[7]*y1 + f[8]);
1021         mtfm1[i] = 1;
1022         mtfm2[i] = !status[i] || t0 > err_level || t < err_level;
1023     }
1024
1025     f_prop1[0] = 1;
1026     f_prop1[1] = 1;
1027     f_prop1[2] = 0;
1028
1029     f_prop2[0] = f_result != 0;
1030     f_prop2[1] = f[8];
1031     f_prop2[2] = cvDet( &F );
1032
1033     cvReleaseMat( &p1 );
1034     cvReleaseMat( &p2 );
1035 }
1036
1037
1038 CV_FundamentalMatTest fmatrix_test;
1039
1040
1041 /********************************** convert homogenious *********************************/
1042
1043 class CV_ConvertHomogeniousTest : public CvArrTest
1044 {
1045 public:
1046     CV_ConvertHomogeniousTest();
1047
1048 protected:
1049     int read_params( CvFileStorage* fs );
1050     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
1051     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
1052     double get_success_error_level( int test_case_idx, int i, int j );
1053     void run_func();
1054     void prepare_to_validation( int );
1055
1056     int dims1, dims2;
1057     int pt_count;
1058 };
1059
1060
1061 CV_ConvertHomogeniousTest::CV_ConvertHomogeniousTest()
1062     : CvArrTest( "_3d-cvt-homogen", "cvConvertPointsHomogeniuos", "" )
1063 {
1064     test_array[INPUT].push(NULL);
1065     test_array[OUTPUT].push(NULL);
1066     test_array[REF_OUTPUT].push(NULL);
1067     element_wise_relative_error = false;
1068
1069     pt_count = dims1 = dims2 = 0;
1070
1071     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
1072     default_timing_param_names = 0;
1073 }
1074
1075
1076 int CV_ConvertHomogeniousTest::read_params( CvFileStorage* fs )
1077 {
1078     int code = CvArrTest::read_params( fs );
1079     return code;
1080 }
1081
1082
1083 void CV_ConvertHomogeniousTest::get_test_array_types_and_sizes( int /*test_case_idx*/,
1084                                                 CvSize** sizes, int** types )
1085 {
1086     CvRNG* rng = ts->get_rng();
1087     int pt_depth1 = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1088     int pt_depth2 = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1089     double pt_count_exp = cvTsRandReal(rng)*6 + 1;
1090     int t;
1091
1092     pt_count = cvRound(exp(pt_count_exp));
1093     pt_count = MAX( pt_count, 5 );
1094
1095     dims1 = 2 + (cvTsRandInt(rng) % 3);
1096     dims2 = 2 + (cvTsRandInt(rng) % 3);
1097
1098     if( dims1 == dims2 + 2 )
1099         dims1--;
1100     else if( dims1 == dims2 - 2 )
1101         dims1++;
1102
1103     if( cvTsRandInt(rng) % 2 )
1104         CV_SWAP( dims1, dims2, t );
1105
1106     types[INPUT][0] = CV_MAKETYPE(pt_depth1, 1);
1107
1108     if( cvTsRandInt(rng) % 2 )
1109         sizes[INPUT][0] = cvSize(pt_count, dims1);
1110     else
1111     {
1112         sizes[INPUT][0] = cvSize(dims1, pt_count);
1113         if( cvTsRandInt(rng) % 2 )
1114         {
1115             types[INPUT][0] = CV_MAKETYPE(pt_depth1, dims1);
1116             if( cvTsRandInt(rng) % 2 )
1117                 sizes[INPUT][0] = cvSize(pt_count, 1);
1118             else
1119                 sizes[INPUT][0] = cvSize(1, pt_count);
1120         }
1121     }
1122
1123     types[OUTPUT][0] = CV_MAKETYPE(pt_depth2, 1);
1124
1125     if( cvTsRandInt(rng) % 2 )
1126         sizes[OUTPUT][0] = cvSize(pt_count, dims2);
1127     else
1128     {
1129         sizes[OUTPUT][0] = cvSize(dims2, pt_count);
1130         if( cvTsRandInt(rng) % 2 )
1131         {
1132             types[OUTPUT][0] = CV_MAKETYPE(pt_depth2, dims2);
1133             if( cvTsRandInt(rng) % 2 )
1134                 sizes[OUTPUT][0] = cvSize(pt_count, 1);
1135             else
1136                 sizes[OUTPUT][0] = cvSize(1, pt_count);
1137         }
1138     }
1139
1140     types[REF_OUTPUT][0] = types[OUTPUT][0];
1141     sizes[REF_OUTPUT][0] = sizes[OUTPUT][0];
1142 }
1143
1144
1145 double CV_ConvertHomogeniousTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
1146 {
1147     return 1e-5;
1148 }
1149
1150
1151 void CV_ConvertHomogeniousTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr )
1152 {
1153     CvMat* temp = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64FC1,dims1) );
1154     CvRNG* rng = ts->get_rng();
1155     CvScalar low = cvScalarAll(0), high = cvScalarAll(10);
1156
1157     if( dims1 > dims2 )
1158         low.val[dims1-1] = 1.;
1159
1160     cvRandArr( rng, temp, CV_RAND_UNI, low, high );
1161     cvTsConvertHomogenious( temp, arr );
1162     cvReleaseMat( &temp );
1163 }
1164
1165
1166 void CV_ConvertHomogeniousTest::run_func()
1167 {
1168     cvConvertPointsHomogenious( &test_mat[INPUT][0], &test_mat[OUTPUT][0] );
1169 }
1170
1171
1172 void CV_ConvertHomogeniousTest::prepare_to_validation( int /*test_case_idx*/ )
1173 {
1174     cvTsConvertHomogenious( &test_mat[INPUT][0], &test_mat[REF_OUTPUT][0] );
1175 }
1176
1177
1178 CV_ConvertHomogeniousTest cvt_homogen_test;
1179
1180
1181 /************************** compute corresponding epipolar lines ************************/
1182
1183 class CV_ComputeEpilinesTest : public CvArrTest
1184 {
1185 public:
1186     CV_ComputeEpilinesTest();
1187
1188 protected:
1189     int read_params( CvFileStorage* fs );
1190     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
1191     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
1192     double get_success_error_level( int test_case_idx, int i, int j );
1193     void run_func();
1194     void prepare_to_validation( int );
1195
1196     int which_image;
1197     int dims;
1198     int pt_count;
1199 };
1200
1201
1202 CV_ComputeEpilinesTest::CV_ComputeEpilinesTest()
1203     : CvArrTest( "_3d-epilines", "cvComputeCorrespondingEpilines", "" )
1204 {
1205     test_array[INPUT].push(NULL);
1206     test_array[INPUT].push(NULL);
1207     test_array[OUTPUT].push(NULL);
1208     test_array[REF_OUTPUT].push(NULL);
1209     element_wise_relative_error = false;
1210
1211     pt_count = dims = which_image = 0;
1212
1213     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
1214     default_timing_param_names = 0;
1215 }
1216
1217
1218 int CV_ComputeEpilinesTest::read_params( CvFileStorage* fs )
1219 {
1220     int code = CvArrTest::read_params( fs );
1221     return code;
1222 }
1223
1224
1225 void CV_ComputeEpilinesTest::get_test_array_types_and_sizes( int /*test_case_idx*/,
1226                                                 CvSize** sizes, int** types )
1227 {
1228     CvRNG* rng = ts->get_rng();
1229     int fm_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1230     int pt_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1231     int ln_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1232     double pt_count_exp = cvTsRandReal(rng)*6 + 1;
1233
1234     which_image = 1 + (cvTsRandInt(rng) % 2);
1235
1236     pt_count = cvRound(exp(pt_count_exp));
1237     pt_count = MAX( pt_count, 5 );
1238
1239     dims = 2 + (cvTsRandInt(rng) % 2);
1240
1241     types[INPUT][0] = CV_MAKETYPE(pt_depth, 1);
1242
1243     if( cvTsRandInt(rng) % 2 )
1244         sizes[INPUT][0] = cvSize(pt_count, dims);
1245     else
1246     {
1247         sizes[INPUT][0] = cvSize(dims, pt_count);
1248         if( cvTsRandInt(rng) % 2 )
1249         {
1250             types[INPUT][0] = CV_MAKETYPE(pt_depth, dims);
1251             if( cvTsRandInt(rng) % 2 )
1252                 sizes[INPUT][0] = cvSize(pt_count, 1);
1253             else
1254                 sizes[INPUT][0] = cvSize(1, pt_count);
1255         }
1256     }
1257
1258     types[INPUT][1] = CV_MAKETYPE(fm_depth, 1);
1259     sizes[INPUT][1] = cvSize(3, 3);
1260
1261     types[OUTPUT][0] = CV_MAKETYPE(ln_depth, 1);
1262
1263     if( cvTsRandInt(rng) % 2 )
1264         sizes[OUTPUT][0] = cvSize(pt_count, 3);
1265     else
1266     {
1267         sizes[OUTPUT][0] = cvSize(3, pt_count);
1268         if( cvTsRandInt(rng) % 2 )
1269         {
1270             types[OUTPUT][0] = CV_MAKETYPE(ln_depth, 3);
1271             if( cvTsRandInt(rng) % 2 )
1272                 sizes[OUTPUT][0] = cvSize(pt_count, 1);
1273             else
1274                 sizes[OUTPUT][0] = cvSize(1, pt_count);
1275         }
1276     }
1277
1278     types[REF_OUTPUT][0] = types[OUTPUT][0];
1279     sizes[REF_OUTPUT][0] = sizes[OUTPUT][0];
1280 }
1281
1282
1283 double CV_ComputeEpilinesTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
1284 {
1285     return 1e-5;
1286 }
1287
1288
1289 void CV_ComputeEpilinesTest::fill_array( int test_case_idx, int i, int j, CvMat* arr )
1290 {
1291     CvRNG* rng = ts->get_rng();
1292
1293     if( i == INPUT && j == 0 )
1294     {
1295         CvMat* temp = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64FC1,dims) );
1296         cvRandArr( rng, temp, CV_RAND_UNI, cvScalar(0,0,1), cvScalarAll(10) );
1297         cvTsConvertHomogenious( temp, arr );
1298         cvReleaseMat( &temp );
1299     }
1300     else if( i == INPUT && j == 1 )
1301         cvRandArr( rng, arr, CV_RAND_UNI, cvScalarAll(0), cvScalarAll(10) );
1302     else
1303         CvArrTest::fill_array( test_case_idx, i, j, arr );
1304 }
1305
1306
1307 void CV_ComputeEpilinesTest::run_func()
1308 {
1309     cvComputeCorrespondEpilines( &test_mat[INPUT][0], which_image,
1310                                  &test_mat[INPUT][1], &test_mat[OUTPUT][0] );
1311 }
1312
1313
1314 void CV_ComputeEpilinesTest::prepare_to_validation( int /*test_case_idx*/ )
1315 {
1316     CvMat* pt = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64F, 3) );
1317     CvMat* lines = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64F, 3) );
1318     double f[9];
1319     CvMat F = cvMat( 3, 3, CV_64F, f );
1320     int i;
1321
1322     cvTsConvertHomogenious( &test_mat[INPUT][0], pt );
1323     cvTsConvert( &test_mat[INPUT][1], &F );
1324     if( which_image == 2 )
1325         cvTranspose( &F, &F );
1326
1327     for( i = 0; i < pt_count; i++ )
1328     {
1329         double* p = pt->data.db + i*3;
1330         double* l = lines->data.db + i*3;
1331         double t0 = f[0]*p[0] + f[1]*p[1] + f[2]*p[2];
1332         double t1 = f[3]*p[0] + f[4]*p[1] + f[5]*p[2];
1333         double t2 = f[6]*p[0] + f[7]*p[1] + f[8]*p[2];
1334         double d = sqrt(t0*t0 + t1*t1);
1335         d = d ? 1./d : 1.;
1336         l[0] = t0*d; l[1] = t1*d; l[2] = t2*d;
1337     }
1338
1339     cvTsConvertHomogenious( lines, &test_mat[REF_OUTPUT][0] );
1340     cvReleaseMat( &pt );
1341     cvReleaseMat( &lines );
1342 }
1343
1344
1345 CV_ComputeEpilinesTest epilines_test;
1346
1347
1348 /* End of file. */