opencv/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, matJ = 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 matA = cvMat(3, 3, CV_64F, A);
 104             CvMat matR = cvMat(3, 3, CV_64F, R);
 105
 106             cvSetIdentity( &matR, cvRealScalar(alpha) );
 107             cvScaleAdd( &_omegav, cvRealScalar(beta), &matR, &matR );
 108             cvScaleAdd( &matA, cvRealScalar(gamma), &matR, &matR );
 109             cvConvert( &matR, 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( &matA, &matA, 1, 1 );
 177                 cvTranspose( &matA, &_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                 matJ = cvMat( 9, 3, CV_64FC1, J );
 186
 187                 cvMatMul( &_dRdm1, &_dm1dm2, &_t0 );
 188                 cvMatMul( &_t0, &_dm2dm3, &_t1 );
 189                 cvMatMul( &_t1, &_dm3din, &matJ );
 190
 191                 _t0 = cvMat( 3, 9, CV_64FC1, t0 );
 192                 cvTranspose( &matJ, &_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, &matJ );
 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 matR = cvMat( 3, 3, CV_64F, R );
 209         CvMat matA = cvMat( 3, 3, CV_64F, A );
 210         CvMat matI = 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 matW = cvMat( 1, 3, CV_64F, W );
 213         CvMat matU = cvMat( 3, 3, CV_64F, U );
 214         CvMat matV = cvMat( 3, 3, CV_64F, V );
 215
 216         cvConvert( src, &matR );
 217         cvSVD( &matR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 218         cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
 219
 220         cvMulTransposed( &matR, &matA, 0 );
 221         cvSetIdentity( &matI );
 222
 223         if( cvNorm( &matA, &matI, CV_C ) > 1e-3 ||
 224             fabs( cvDet(&matR) - 1 ) > 1e-3 )
 225             return 0;
 226
 227         tr = (cvTrace(&matR).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, &matJ );
 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 == matJ.rows )
 329                 cvConvert( &matJ, jacobian );
 330             else
 331             {
 332                 _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf );
 333                 cvConvert( &matJ, &_Jf );
 334                 cvTranspose( &_Jf, jacobian );
 335             }
 336         }
 337         else if( jacobian->rows == matJ.rows )
 338             cvCopy( &matJ, jacobian );
 339         else
 340             cvTranspose( &matJ, jacobian );
 341     }
 342
 343     return 1;
 344 }
 345
 346
 347 void
 348 cvTsConvertHomogeneous( 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     cvTsConvertHomogeneous( 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     bool test_cpp;
 539 };
 540
 541
 542 CV_RodriguesTest::CV_RodriguesTest()
 543     : CvArrTest( "_3d-rodrigues", "cvRodrigues2", "" )
 544 {
 545     test_array[INPUT].push(NULL);  // rotation vector
 546     test_array[OUTPUT].push(NULL); // rotation matrix
 547     test_array[OUTPUT].push(NULL); // jacobian (J)
 548     test_array[OUTPUT].push(NULL); // rotation vector (backward transform result)
 549     test_array[OUTPUT].push(NULL); // inverse transform jacobian (J1)
 550     test_array[OUTPUT].push(NULL); // J*J1 (or J1*J) == I(3x3)
 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     test_array[REF_OUTPUT].push(NULL);
 556
 557     element_wise_relative_error = false;
 558     calc_jacobians = false;
 559
 560     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
 561     default_timing_param_names = 0;
 562     test_cpp = false;
 563 }
 564
 565
 566 int CV_RodriguesTest::read_params( CvFileStorage* fs )
 567 {
 568     int code = CvArrTest::read_params( fs );
 569     return code;
 570 }
 571
 572
 573 void CV_RodriguesTest::get_test_array_types_and_sizes(
 574     int /*test_case_idx*/, CvSize** sizes, int** types )
 575 {
 576     CvRNG* rng = ts->get_rng();
 577     int depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
 578     int i, code;
 579
 580     code = cvTsRandInt(rng) % 3;
 581     types[INPUT][0] = CV_MAKETYPE(depth, 1);
 582
 583     if( code == 0 )
 584     {
 585         sizes[INPUT][0] = cvSize(1,1);
 586         types[INPUT][0] = CV_MAKETYPE(depth, 3);
 587     }
 588     else if( code == 1 )
 589         sizes[INPUT][0] = cvSize(3,1);
 590     else
 591         sizes[INPUT][0] = cvSize(1,3);
 592
 593     sizes[OUTPUT][0] = cvSize(3, 3);
 594     types[OUTPUT][0] = CV_MAKETYPE(depth, 1);
 595
 596     types[OUTPUT][1] = CV_MAKETYPE(depth, 1);
 597
 598     if( cvTsRandInt(rng) % 2 )
 599         sizes[OUTPUT][1] = cvSize(3,9);
 600     else
 601         sizes[OUTPUT][1] = cvSize(9,3);
 602
 603     types[OUTPUT][2] = types[INPUT][0];
 604     sizes[OUTPUT][2] = sizes[INPUT][0];
 605
 606     types[OUTPUT][3] = types[OUTPUT][1];
 607     sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width);
 608
 609     types[OUTPUT][4] = types[OUTPUT][1];
 610     sizes[OUTPUT][4] = cvSize(3,3);
 611
 612     calc_jacobians = cvTsRandInt(rng) % 3 != 0;
 613     if( !calc_jacobians )
 614         sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0);
 615
 616     for( i = 0; i < 5; i++ )
 617     {
 618         types[REF_OUTPUT][i] = types[OUTPUT][i];
 619         sizes[REF_OUTPUT][i] = sizes[OUTPUT][i];
 620     }
 621     test_cpp = (cvTsRandInt(rng) & 256) == 0;
 622 }
 623
 624
 625 double CV_RodriguesTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j )
 626 {
 627     return j == 4 ? 1e-2 : 1e-2;
 628 }
 629
 630
 631 void CV_RodriguesTest::fill_array( int test_case_idx, int i, int j, CvMat* arr )
 632 {
 633     if( i == INPUT && j == 0 )
 634     {
 635         double r[3], theta0, theta1, f;
 636         CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r );
 637         CvRNG* rng = ts->get_rng();
 638
 639         r[0] = cvTsRandReal(rng)*CV_PI*2;
 640         r[1] = cvTsRandReal(rng)*CV_PI*2;
 641         r[2] = cvTsRandReal(rng)*CV_PI*2;
 642
 643         theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
 644         theta1 = fmod(theta0, CV_PI*2);
 645
 646         if( theta1 > CV_PI )
 647             theta1 = -(CV_PI*2 - theta1);
 648
 649         f = theta1/(theta0 ? theta0 : 1);
 650         r[0] *= f;
 651         r[1] *= f;
 652         r[2] *= f;
 653
 654         cvTsConvert( &_r, arr );
 655     }
 656     else
 657         CvArrTest::fill_array( test_case_idx, i, j, arr );
 658 }
 659
 660
 661 int CV_RodriguesTest::prepare_test_case( int test_case_idx )
 662 {
 663     int code = CvArrTest::prepare_test_case( test_case_idx );
 664     return code;
 665 }
 666
 667
 668 void CV_RodriguesTest::run_func()
 669 {
 670     CvMat *v2m_jac = 0, *m2v_jac = 0;
 671
 672     if( calc_jacobians )
 673     {
 674         v2m_jac = &test_mat[OUTPUT][1];
 675         m2v_jac = &test_mat[OUTPUT][3];
 676     }
 677
 678     if( !test_cpp )
 679     {
 680         cvRodrigues2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac );
 681         cvRodrigues2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac );
 682     }
 683     else
 684     {
 685         cv::Mat v(&test_mat[INPUT][0]), M(&test_mat[OUTPUT][0]), v2(&test_mat[OUTPUT][2]);
 686         cv::Mat M0 = M, v2_0 = v2;
 687         if( !calc_jacobians )
 688         {
 689             cv::Rodrigues(v, M);
 690             cv::Rodrigues(M, v2);
 691         }
 692         else
 693         {
 694             cv::Mat J1(&test_mat[OUTPUT][1]), J2(&test_mat[OUTPUT][3]);
 695             cv::Mat J1_0 = J1, J2_0 = J2;
 696             cv::Rodrigues(v, M, J1);
 697             cv::Rodrigues(M, v2, J2);
 698             if( J1.data != J1_0.data )
 699             {
 700                 if( J1.size() != J1_0.size() )
 701                     J1 = J1.t();
 702                 J1.convertTo(J1_0, J1_0.type());
 703             }
 704             if( J2.data != J2_0.data )
 705             {
 706                 if( J2.size() != J2_0.size() )
 707                     J2 = J2.t();
 708                 J2.convertTo(J2_0, J2_0.type());
 709             }
 710         }
 711         if( M.data != M0.data )
 712             M.reshape(M0.channels(), M0.rows).convertTo(M0, M0.type());
 713         if( v2.data != v2_0.data )
 714             v2.reshape(v2_0.channels(), v2_0.rows).convertTo(v2_0, v2_0.type());
 715     }
 716 }
 717
 718
 719 void CV_RodriguesTest::prepare_to_validation( int /*test_case_idx*/ )
 720 {
 721     const CvMat* vec = &test_mat[INPUT][0];
 722     CvMat* m = &test_mat[REF_OUTPUT][0];
 723     CvMat* vec2 = &test_mat[REF_OUTPUT][2];
 724     CvMat* v2m_jac = 0, *m2v_jac = 0;
 725     double theta0, theta1;
 726
 727     if( calc_jacobians )
 728     {
 729         v2m_jac = &test_mat[REF_OUTPUT][1];
 730         m2v_jac = &test_mat[REF_OUTPUT][3];
 731     }
 732
 733
 734     cvTsRodrigues( vec, m, v2m_jac );
 735     cvTsRodrigues( m, vec2, m2v_jac );
 736     cvTsCopy( vec, vec2 );
 737
 738     theta0 = cvNorm( vec2, 0, CV_L2 );
 739     theta1 = fmod( theta0, CV_PI*2 );
 740
 741     if( theta1 > CV_PI )
 742         theta1 = -(CV_PI*2 - theta1);
 743     cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) );
 744
 745     if( calc_jacobians )
 746     {
 747         //cvInvert( v2m_jac, m2v_jac, CV_SVD );
 748         if( cvNorm(&test_mat[OUTPUT][3],0,CV_C) < 1000 )
 749         {
 750             cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3],
 751                       1, 0, 0, &test_mat[OUTPUT][4],
 752                       v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T );
 753         }
 754         else
 755         {
 756             cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) );
 757             cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] );
 758         }
 759         cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) );
 760     }
 761 }
 762
 763
 764 CV_RodriguesTest rodrigues_test;
 765
 766
 767 /********************************** fundamental matrix *********************************/
 768
 769 class CV_FundamentalMatTest : public CvArrTest
 770 {
 771 public:
 772     CV_FundamentalMatTest();
 773
 774 protected:
 775     int read_params( CvFileStorage* fs );
 776     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
 777     int prepare_test_case( int test_case_idx );
 778     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
 779     double get_success_error_level( int test_case_idx, int i, int j );
 780     void run_func();
 781     void prepare_to_validation( int );
 782
 783     int method;
 784     int img_size;
 785     int cube_size;
 786     int dims;
 787     int f_result;
 788     double min_f, max_f;
 789     double sigma;
 790     bool test_cpp;
 791 };
 792
 793
 794 CV_FundamentalMatTest::CV_FundamentalMatTest()
 795     : CvArrTest( "_3d-fundam", "cvFindFundamentalMatrix", "" )
 796 {
 797     // input arrays:
 798     //   0, 1 - arrays of 2d points that are passed to %func%.
 799     //          Can have different data type, layout, be stored in homogeneous coordinates or not.
 800     //   2 - array of 3d points that are projected to both view planes
 801     //   3 - [R|t] matrix for the second view plane (for the first one it is [I|0]
 802     //   4, 5 - intrinsic matrices
 803     test_array[INPUT].push(NULL);
 804     test_array[INPUT].push(NULL);
 805     test_array[INPUT].push(NULL);
 806     test_array[INPUT].push(NULL);
 807     test_array[INPUT].push(NULL);
 808     test_array[INPUT].push(NULL);
 809     test_array[TEMP].push(NULL);
 810     test_array[TEMP].push(NULL);
 811     test_array[OUTPUT].push(NULL);
 812     test_array[OUTPUT].push(NULL);
 813     test_array[REF_OUTPUT].push(NULL);
 814     test_array[REF_OUTPUT].push(NULL);
 815
 816     element_wise_relative_error = false;
 817
 818     method = 0;
 819     img_size = 10;
 820     cube_size = 10;
 821     min_f = 1;
 822     max_f = 3;
 823     sigma = 0;//0.1;
 824     f_result = 0;
 825
 826     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
 827     default_timing_param_names = 0;
 828     test_cpp = false;
 829 }
 830
 831
 832 int CV_FundamentalMatTest::read_params( CvFileStorage* fs )
 833 {
 834     int code = CvArrTest::read_params( fs );
 835     return code;
 836 }
 837
 838
 839 void CV_FundamentalMatTest::get_test_array_types_and_sizes( int /*test_case_idx*/,
 840                                                 CvSize** sizes, int** types )
 841 {
 842     CvRNG* rng = ts->get_rng();
 843     int pt_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
 844     double pt_count_exp = cvTsRandReal(rng)*6 + 1;
 845     int pt_count = cvRound(exp(pt_count_exp));
 846
 847     dims = cvTsRandInt(rng) % 2 + 2;
 848     method = 1 << (cvTsRandInt(rng) % 4);
 849
 850     if( method == CV_FM_7POINT )
 851         pt_count = 7;
 852     else
 853     {
 854         pt_count = MAX( pt_count, 8 + (method == CV_FM_8POINT) );
 855         if( pt_count >= 8 && cvTsRandInt(rng) % 2 )
 856             method |= CV_FM_8POINT;
 857     }
 858
 859     types[INPUT][0] = CV_MAKETYPE(pt_depth, 1);
 860
 861     if( cvTsRandInt(rng) % 2 )
 862         sizes[INPUT][0] = cvSize(pt_count, dims);
 863     else
 864     {
 865         sizes[INPUT][0] = cvSize(dims, pt_count);
 866         if( cvTsRandInt(rng) % 2 )
 867         {
 868             types[INPUT][0] = CV_MAKETYPE(pt_depth, dims);
 869             if( cvTsRandInt(rng) % 2 )
 870                 sizes[INPUT][0] = cvSize(pt_count, 1);
 871             else
 872                 sizes[INPUT][0] = cvSize(1, pt_count);
 873         }
 874     }
 875
 876     sizes[INPUT][1] = sizes[INPUT][0];
 877     types[INPUT][1] = types[INPUT][0];
 878
 879     sizes[INPUT][2] = cvSize(pt_count, 1 );
 880     types[INPUT][2] = CV_64FC3;
 881
 882     sizes[INPUT][3] = cvSize(4,3);
 883     types[INPUT][3] = CV_64FC1;
 884
 885     sizes[INPUT][4] = sizes[INPUT][5] = cvSize(3,3);
 886     types[INPUT][4] = types[INPUT][5] = CV_MAKETYPE(CV_64F, 1);
 887
 888     sizes[TEMP][0] = cvSize(3,3);
 889     types[TEMP][0] = CV_64FC1;
 890     sizes[TEMP][1] = cvSize(pt_count,1);
 891     types[TEMP][1] = CV_8UC1;
 892
 893     sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,1);
 894     types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC1;
 895     sizes[OUTPUT][1] = sizes[REF_OUTPUT][1] = cvSize(pt_count,1);
 896     types[OUTPUT][1] = types[REF_OUTPUT][1] = CV_8UC1;
 897
 898     test_cpp = (cvTsRandInt(rng) & 256) == 0;
 899 }
 900
 901
 902 double CV_FundamentalMatTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
 903 {
 904     return 3e-2;
 905 }
 906
 907
 908 void CV_FundamentalMatTest::fill_array( int test_case_idx, int i, int j, CvMat* arr )
 909 {
 910     double t[12];
 911     CvMat T;
 912     double* p = arr->data.db;
 913     CvRNG* rng = ts->get_rng();
 914
 915     if( i != INPUT )
 916     {
 917         CvArrTest::fill_array( test_case_idx, i, j, arr );
 918         return;
 919     }
 920
 921     switch( j )
 922     {
 923     case 0:
 924     case 1:
 925         return; // fill them later in prepare_test_case
 926     case 2:
 927         for( i = 0; i < arr->cols*3; i += 3 )
 928         {
 929             p[i] = cvTsRandReal(rng)*cube_size;
 930             p[i+1] = cvTsRandReal(rng)*cube_size;
 931             p[i+2] = cvTsRandReal(rng)*cube_size + cube_size;
 932         }
 933         break;
 934     case 3:
 935         {
 936         double r[3];
 937         CvMat rot_vec = cvMat( 3, 1, CV_64F, r );
 938         CvMat rot_mat = cvMat( 3, 3, CV_64F );
 939         r[0] = cvTsRandReal(rng)*CV_PI*2;
 940         r[1] = cvTsRandReal(rng)*CV_PI*2;
 941         r[2] = cvTsRandReal(rng)*CV_PI*2;
 942
 943         cvSetData( &rot_mat, t, 4*sizeof(t[0]) );
 944         cvTsRodrigues( &rot_vec, &rot_mat );
 945         t[3] = cvTsRandReal(rng)*cube_size;
 946         t[7] = cvTsRandReal(rng)*cube_size;
 947         t[11] = cvTsRandReal(rng)*cube_size;
 948         T = cvMat( 3, 4, CV_64F, t );
 949         cvTsConvert( &T, arr );
 950         }
 951         break;
 952     case 4:
 953     case 5:
 954         memset( t, 0, sizeof(t) );
 955         t[0] = t[4] = cvTsRandReal(rng)*(max_f - min_f) + min_f;
 956         t[2] = (img_size*0.5 + cvTsRandReal(rng)*4. - 2.)*t[0];
 957         t[5] = (img_size*0.5 + cvTsRandReal(rng)*4. - 2.)*t[4];
 958         t[8] = 1.;
 959         T = cvMat( 3, 3, CV_64F, t );
 960         cvTsConvert( &T, arr );
 961         break;
 962     }
 963 }
 964
 965
 966 int CV_FundamentalMatTest::prepare_test_case( int test_case_idx )
 967 {
 968     int code = CvArrTest::prepare_test_case( test_case_idx );
 969     if( code > 0 )
 970     {
 971         const CvMat* _3d = &test_mat[INPUT][2];
 972         CvRNG* rng = ts->get_rng();
 973         double Idata[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0 };
 974         CvMat I = cvMat( 3, 4, CV_64F, Idata );
 975         int k;
 976
 977         for( k = 0; k < 2; k++ )
 978         {
 979             const CvMat* Rt = k == 0 ? &I : &test_mat[INPUT][3];
 980             const CvMat* A = &test_mat[INPUT][k == 0 ? 4 : 5];
 981             CvMat* _2d = &test_mat[INPUT][k];
 982
 983             cvTsProjectPoints( _3d, Rt, A, _2d, rng, sigma );
 984         }
 985     }
 986
 987     return code;
 988 }
 989
 990
 991 void CV_FundamentalMatTest::run_func()
 992 {
 993     //if(!test_cpp)
 994     {
 995         f_result = cvFindFundamentalMat( &test_mat[INPUT][0], &test_mat[INPUT][1],
 996                     &test_mat[TEMP][0], method, MAX(sigma*3, 0.01), 0, &test_mat[TEMP][1] );
 997     }
 998     /*else
 999     {
1000         cv::findFundamentalMat(const Mat& points1, const Mat& points2,
1001         vector<uchar>& mask, int method=FM_RANSAC,
1002         double param1=3., double param2=0.99 );
1003
1004         CV_EXPORTS Mat findFundamentalMat( const Mat& points1, const Mat& points2,
1005                                           int method=FM_RANSAC,
1006                                           double param1=3., double param2=0.99 );
1007     }*/
1008
1009 }
1010
1011
1012 void CV_FundamentalMatTest::prepare_to_validation( int test_case_idx )
1013 {
1014     const CvMat* Rt = &test_mat[INPUT][3];
1015     const CvMat* A1 = &test_mat[INPUT][4];
1016     const CvMat* A2 = &test_mat[INPUT][5];
1017     double f0[9];
1018     CvMat F0 = cvMat( 3, 3, CV_64FC1, f0 );
1019
1020     double _invA1[9], _invA2[9], temp[9];
1021     CvMat invA1 = cvMat( 3, 3, CV_64F, _invA1 );
1022     CvMat invA2 = cvMat( 3, 3, CV_64F, _invA2 );
1023     CvMat R = cvMat( 3, 3, CV_64F );
1024     CvMat T = cvMat( 3, 3, CV_64F, temp );
1025
1026     cvSetData( &R, Rt->data.db, Rt->step ); // R = Rt(:,1:3)
1027
1028     // F = (A2^-T)*[t]_x*R*(A1^-1)
1029     cvInvert( A1, &invA1, CV_SVD );
1030     cvInvert( A2, &invA2, CV_SVD );
1031
1032     {
1033     double tx = ((double*)(Rt->data.ptr))[3];
1034     double ty = ((double*)(Rt->data.ptr + Rt->step))[3];
1035     double tz = ((double*)(Rt->data.ptr + Rt->step*2))[3];
1036
1037     double _t_x[] = { 0, -tz, ty, tz, 0, -tx, -ty, tx, 0 };
1038     CvMat t_x = cvMat( 3, 3, CV_64F, _t_x );
1039
1040     cvGEMM( &invA2, &t_x, 1, 0, 0, &T, CV_GEMM_A_T );
1041     cvMatMul( &R, &invA1, &invA2 );
1042     cvMatMul( &T, &invA2, &F0 );
1043     cvScale( &F0, &F0, f0[8] );
1044     }
1045
1046     double f[9];
1047     CvMat F = cvMat(3, 3, CV_64F, f);
1048     uchar* status = test_mat[TEMP][1].data.ptr;
1049     double err_level = get_success_error_level( test_case_idx, OUTPUT, 1 );
1050     uchar* mtfm1 = test_mat[REF_OUTPUT][1].data.ptr;
1051     uchar* mtfm2 = test_mat[OUTPUT][1].data.ptr;
1052     double* f_prop1 = test_mat[REF_OUTPUT][0].data.db;
1053     double* f_prop2 = test_mat[OUTPUT][0].data.db;
1054
1055     int i, pt_count = test_mat[INPUT][2].cols;
1056     CvMat* p1 = cvCreateMat( 1, pt_count, CV_64FC2 );
1057     CvMat* p2 = cvCreateMat( 1, pt_count, CV_64FC2 );
1058
1059     cvTsConvertHomogeneous( &test_mat[INPUT][0], p1 );
1060     cvTsConvertHomogeneous( &test_mat[INPUT][1], p2 );
1061
1062     cvTsConvert( &test_mat[TEMP][0], &F );
1063
1064     if( method <= CV_FM_8POINT )
1065         memset( status, 1, pt_count );
1066
1067     for( i = 0; i < pt_count; i++ )
1068     {
1069         double x1 = p1->data.db[i*2];
1070         double y1 = p1->data.db[i*2+1];
1071         double x2 = p2->data.db[i*2];
1072         double y2 = p2->data.db[i*2+1];
1073         double t0 = fabs(f0[0]*x2*x1 + f0[1]*x2*y1 + f0[2]*x2 +
1074                    f0[3]*y2*x1 + f0[4]*y2*y1 + f0[5]*y2 +
1075                    f0[6]*x1 + f0[7]*y1 + f0[8]);
1076         double t = fabs(f[0]*x2*x1 + f[1]*x2*y1 + f[2]*x2 +
1077                    f[3]*y2*x1 + f[4]*y2*y1 + f[5]*y2 +
1078                    f[6]*x1 + f[7]*y1 + f[8]);
1079         mtfm1[i] = 1;
1080         mtfm2[i] = !status[i] || t0 > err_level || t < err_level;
1081     }
1082
1083     f_prop1[0] = 1;
1084     f_prop1[1] = 1;
1085     f_prop1[2] = 0;
1086
1087     f_prop2[0] = f_result != 0;
1088     f_prop2[1] = f[8];
1089     f_prop2[2] = cvDet( &F );
1090
1091     cvReleaseMat( &p1 );
1092     cvReleaseMat( &p2 );
1093 }
1094
1095
1096 CV_FundamentalMatTest fmatrix_test;
1097
1098
1099 /********************************** convert homogeneous *********************************/
1100
1101 class CV_ConvertHomogeneousTest : public CvArrTest
1102 {
1103 public:
1104     CV_ConvertHomogeneousTest();
1105
1106 protected:
1107     int read_params( CvFileStorage* fs );
1108     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
1109     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
1110     double get_success_error_level( int test_case_idx, int i, int j );
1111     void run_func();
1112     void prepare_to_validation( int );
1113
1114     int dims1, dims2;
1115     int pt_count;
1116 };
1117
1118
1119 CV_ConvertHomogeneousTest::CV_ConvertHomogeneousTest()
1120     : CvArrTest( "_3d-cvt-homogen", "cvConvertPointsHomogeniuos", "" )
1121 {
1122     test_array[INPUT].push(NULL);
1123     test_array[OUTPUT].push(NULL);
1124     test_array[REF_OUTPUT].push(NULL);
1125     element_wise_relative_error = false;
1126
1127     pt_count = dims1 = dims2 = 0;
1128
1129     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
1130     default_timing_param_names = 0;
1131 }
1132
1133
1134 int CV_ConvertHomogeneousTest::read_params( CvFileStorage* fs )
1135 {
1136     int code = CvArrTest::read_params( fs );
1137     return code;
1138 }
1139
1140
1141 void CV_ConvertHomogeneousTest::get_test_array_types_and_sizes( int /*test_case_idx*/,
1142                                                 CvSize** sizes, int** types )
1143 {
1144     CvRNG* rng = ts->get_rng();
1145     int pt_depth1 = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1146     int pt_depth2 = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1147     double pt_count_exp = cvTsRandReal(rng)*6 + 1;
1148     int t;
1149
1150     pt_count = cvRound(exp(pt_count_exp));
1151     pt_count = MAX( pt_count, 5 );
1152
1153     dims1 = 2 + (cvTsRandInt(rng) % 3);
1154     dims2 = 2 + (cvTsRandInt(rng) % 3);
1155
1156     if( dims1 == dims2 + 2 )
1157         dims1--;
1158     else if( dims1 == dims2 - 2 )
1159         dims1++;
1160
1161     if( cvTsRandInt(rng) % 2 )
1162         CV_SWAP( dims1, dims2, t );
1163
1164     types[INPUT][0] = CV_MAKETYPE(pt_depth1, 1);
1165
1166     if( cvTsRandInt(rng) % 2 )
1167         sizes[INPUT][0] = cvSize(pt_count, dims1);
1168     else
1169     {
1170         sizes[INPUT][0] = cvSize(dims1, pt_count);
1171         if( cvTsRandInt(rng) % 2 )
1172         {
1173             types[INPUT][0] = CV_MAKETYPE(pt_depth1, dims1);
1174             if( cvTsRandInt(rng) % 2 )
1175                 sizes[INPUT][0] = cvSize(pt_count, 1);
1176             else
1177                 sizes[INPUT][0] = cvSize(1, pt_count);
1178         }
1179     }
1180
1181     types[OUTPUT][0] = CV_MAKETYPE(pt_depth2, 1);
1182
1183     if( cvTsRandInt(rng) % 2 )
1184         sizes[OUTPUT][0] = cvSize(pt_count, dims2);
1185     else
1186     {
1187         sizes[OUTPUT][0] = cvSize(dims2, pt_count);
1188         if( cvTsRandInt(rng) % 2 )
1189         {
1190             types[OUTPUT][0] = CV_MAKETYPE(pt_depth2, dims2);
1191             if( cvTsRandInt(rng) % 2 )
1192                 sizes[OUTPUT][0] = cvSize(pt_count, 1);
1193             else
1194                 sizes[OUTPUT][0] = cvSize(1, pt_count);
1195         }
1196     }
1197
1198     types[REF_OUTPUT][0] = types[OUTPUT][0];
1199     sizes[REF_OUTPUT][0] = sizes[OUTPUT][0];
1200 }
1201
1202
1203 double CV_ConvertHomogeneousTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
1204 {
1205     return 1e-5;
1206 }
1207
1208
1209 void CV_ConvertHomogeneousTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr )
1210 {
1211     CvMat* temp = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64FC1,dims1) );
1212     CvRNG* rng = ts->get_rng();
1213     CvScalar low = cvScalarAll(0), high = cvScalarAll(10);
1214
1215     if( dims1 > dims2 )
1216         low.val[dims1-1] = 1.;
1217
1218     cvRandArr( rng, temp, CV_RAND_UNI, low, high );
1219     cvTsConvertHomogeneous( temp, arr );
1220     cvReleaseMat( &temp );
1221 }
1222
1223
1224 void CV_ConvertHomogeneousTest::run_func()
1225 {
1226     cvConvertPointsHomogeneous( &test_mat[INPUT][0], &test_mat[OUTPUT][0] );
1227 }
1228
1229
1230 void CV_ConvertHomogeneousTest::prepare_to_validation( int /*test_case_idx*/ )
1231 {
1232     cvTsConvertHomogeneous( &test_mat[INPUT][0], &test_mat[REF_OUTPUT][0] );
1233 }
1234
1235
1236 CV_ConvertHomogeneousTest cvt_homogen_test;
1237
1238
1239 /************************** compute corresponding epipolar lines ************************/
1240
1241 class CV_ComputeEpilinesTest : public CvArrTest
1242 {
1243 public:
1244     CV_ComputeEpilinesTest();
1245
1246 protected:
1247     int read_params( CvFileStorage* fs );
1248     void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
1249     void fill_array( int test_case_idx, int i, int j, CvMat* arr );
1250     double get_success_error_level( int test_case_idx, int i, int j );
1251     void run_func();
1252     void prepare_to_validation( int );
1253
1254     int which_image;
1255     int dims;
1256     int pt_count;
1257 };
1258
1259
1260 CV_ComputeEpilinesTest::CV_ComputeEpilinesTest()
1261     : CvArrTest( "_3d-epilines", "cvComputeCorrespondingEpilines", "" )
1262 {
1263     test_array[INPUT].push(NULL);
1264     test_array[INPUT].push(NULL);
1265     test_array[OUTPUT].push(NULL);
1266     test_array[REF_OUTPUT].push(NULL);
1267     element_wise_relative_error = false;
1268
1269     pt_count = dims = which_image = 0;
1270
1271     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
1272     default_timing_param_names = 0;
1273 }
1274
1275
1276 int CV_ComputeEpilinesTest::read_params( CvFileStorage* fs )
1277 {
1278     int code = CvArrTest::read_params( fs );
1279     return code;
1280 }
1281
1282
1283 void CV_ComputeEpilinesTest::get_test_array_types_and_sizes( int /*test_case_idx*/,
1284                                                 CvSize** sizes, int** types )
1285 {
1286     CvRNG* rng = ts->get_rng();
1287     int fm_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1288     int pt_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1289     int ln_depth = cvTsRandInt(rng) % 2 == 0 ? CV_32F : CV_64F;
1290     double pt_count_exp = cvTsRandReal(rng)*6 + 1;
1291
1292     which_image = 1 + (cvTsRandInt(rng) % 2);
1293
1294     pt_count = cvRound(exp(pt_count_exp));
1295     pt_count = MAX( pt_count, 5 );
1296
1297     dims = 2 + (cvTsRandInt(rng) % 2);
1298
1299     types[INPUT][0] = CV_MAKETYPE(pt_depth, 1);
1300
1301     if( cvTsRandInt(rng) % 2 )
1302         sizes[INPUT][0] = cvSize(pt_count, dims);
1303     else
1304     {
1305         sizes[INPUT][0] = cvSize(dims, pt_count);
1306         if( cvTsRandInt(rng) % 2 )
1307         {
1308             types[INPUT][0] = CV_MAKETYPE(pt_depth, dims);
1309             if( cvTsRandInt(rng) % 2 )
1310                 sizes[INPUT][0] = cvSize(pt_count, 1);
1311             else
1312                 sizes[INPUT][0] = cvSize(1, pt_count);
1313         }
1314     }
1315
1316     types[INPUT][1] = CV_MAKETYPE(fm_depth, 1);
1317     sizes[INPUT][1] = cvSize(3, 3);
1318
1319     types[OUTPUT][0] = CV_MAKETYPE(ln_depth, 1);
1320
1321     if( cvTsRandInt(rng) % 2 )
1322         sizes[OUTPUT][0] = cvSize(pt_count, 3);
1323     else
1324     {
1325         sizes[OUTPUT][0] = cvSize(3, pt_count);
1326         if( cvTsRandInt(rng) % 2 )
1327         {
1328             types[OUTPUT][0] = CV_MAKETYPE(ln_depth, 3);
1329             if( cvTsRandInt(rng) % 2 )
1330                 sizes[OUTPUT][0] = cvSize(pt_count, 1);
1331             else
1332                 sizes[OUTPUT][0] = cvSize(1, pt_count);
1333         }
1334     }
1335
1336     types[REF_OUTPUT][0] = types[OUTPUT][0];
1337     sizes[REF_OUTPUT][0] = sizes[OUTPUT][0];
1338 }
1339
1340
1341 double CV_ComputeEpilinesTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
1342 {
1343     return 1e-5;
1344 }
1345
1346
1347 void CV_ComputeEpilinesTest::fill_array( int test_case_idx, int i, int j, CvMat* arr )
1348 {
1349     CvRNG* rng = ts->get_rng();
1350
1351     if( i == INPUT && j == 0 )
1352     {
1353         CvMat* temp = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64FC1,dims) );
1354         cvRandArr( rng, temp, CV_RAND_UNI, cvScalar(0,0,1), cvScalarAll(10) );
1355         cvTsConvertHomogeneous( temp, arr );
1356         cvReleaseMat( &temp );
1357     }
1358     else if( i == INPUT && j == 1 )
1359         cvRandArr( rng, arr, CV_RAND_UNI, cvScalarAll(0), cvScalarAll(10) );
1360     else
1361         CvArrTest::fill_array( test_case_idx, i, j, arr );
1362 }
1363
1364
1365 void CV_ComputeEpilinesTest::run_func()
1366 {
1367     cvComputeCorrespondEpilines( &test_mat[INPUT][0], which_image,
1368                                  &test_mat[INPUT][1], &test_mat[OUTPUT][0] );
1369 }
1370
1371
1372 void CV_ComputeEpilinesTest::prepare_to_validation( int /*test_case_idx*/ )
1373 {
1374     CvMat* pt = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64F, 3) );
1375     CvMat* lines = cvCreateMat( 1, pt_count, CV_MAKETYPE(CV_64F, 3) );
1376     double f[9];
1377     CvMat F = cvMat( 3, 3, CV_64F, f );
1378     int i;
1379
1380     cvTsConvertHomogeneous( &test_mat[INPUT][0], pt );
1381     cvTsConvert( &test_mat[INPUT][1], &F );
1382     if( which_image == 2 )
1383         cvTranspose( &F, &F );
1384
1385     for( i = 0; i < pt_count; i++ )
1386     {
1387         double* p = pt->data.db + i*3;
1388         double* l = lines->data.db + i*3;
1389         double t0 = f[0]*p[0] + f[1]*p[1] + f[2]*p[2];
1390         double t1 = f[3]*p[0] + f[4]*p[1] + f[5]*p[2];
1391         double t2 = f[6]*p[0] + f[7]*p[1] + f[8]*p[2];
1392         double d = sqrt(t0*t0 + t1*t1);
1393         d = d ? 1./d : 1.;
1394         l[0] = t0*d; l[1] = t1*d; l[2] = t2*d;
1395     }
1396
1397     cvTsConvertHomogeneous( lines, &test_mat[REF_OUTPUT][0] );
1398     cvReleaseMat( &pt );
1399     cvReleaseMat( &lines );
1400 }
1401
1402
1403 CV_ComputeEpilinesTest epilines_test;
1404
1405
1406 /* End of file. */