opencv/src/cv/cvfundam.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 "_cv.h"
  43
  44 template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
  45 {
  46     int i, j;
  47     for( i = j = 0; i < count; i++ )
  48         if( mask[i*mstep] )
  49         {
  50             if( i > j )
  51                 ptr[j] = ptr[i];
  52             j++;
  53         }
  54     return j;
  55 }
  56
  57 class CvModelEstimator2
  58 {
  59 public:
  60     CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions);
  61     virtual ~CvModelEstimator2();
  62
  63     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0;
  64     virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
  65                            CvMat* mask, double confidence=0.99, int maxIters=1000 );
  66     virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
  67                             CvMat* mask, double threshold,
  68                             double confidence=0.99, int maxIters=1000 );
  69     virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; }
  70     virtual void setSeed( int64 seed );
  71
  72 protected:
  73     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
  74                                      const CvMat* model, CvMat* error ) = 0;
  75     virtual int findInliers( const CvMat* m1, const CvMat* m2,
  76                              const CvMat* model, CvMat* error,
  77                              CvMat* mask, double threshold );
  78     virtual bool getSubset( const CvMat* m1, const CvMat* m2,
  79                             CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );
  80     virtual bool checkSubset( const CvMat* ms1, int count );
  81
  82     CvRNG rng;
  83     int modelPoints;
  84     CvSize modelSize;
  85     int maxBasicSolutions;
  86     bool checkPartialSubsets;
  87 };
  88
  89
  90 CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
  91 {
  92     modelPoints = _modelPoints;
  93     modelSize = _modelSize;
  94     maxBasicSolutions = _maxBasicSolutions;
  95     checkPartialSubsets = true;
  96     rng = cvRNG(-1);
  97 }
  98
  99 CvModelEstimator2::~CvModelEstimator2()
 100 {
 101 }
 102
 103 void CvModelEstimator2::setSeed( int64 seed )
 104 {
 105     rng = cvRNG(seed);
 106 }
 107
 108
 109 int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
 110                                     const CvMat* model, CvMat* _err,
 111                                     CvMat* _mask, double threshold )
 112 {
 113     int i, count = _err->rows*_err->cols, goodCount = 0;
 114     const float* err = _err->data.fl;
 115     uchar* mask = _mask->data.ptr;
 116
 117     computeReprojError( m1, m2, model, _err );
 118     threshold *= threshold;
 119     for( i = 0; i < count; i++ )
 120         goodCount += mask[i] = err[i] <= threshold;
 121     return goodCount;
 122 }
 123
 124
 125 CV_IMPL int
 126 cvRANSACUpdateNumIters( double p, double ep,
 127                         int model_points, int max_iters )
 128 {
 129     int result = 0;
 130
 131     CV_FUNCNAME( "cvRANSACUpdateNumIters" );
 132
 133     __BEGIN__;
 134
 135     double num, denom;
 136
 137     if( model_points <= 0 )
 138         CV_ERROR( CV_StsOutOfRange, "the number of model points should be positive" );
 139
 140     p = MAX(p, 0.);
 141     p = MIN(p, 1.);
 142     ep = MAX(ep, 0.);
 143     ep = MIN(ep, 1.);
 144
 145     // avoid inf's & nan's
 146     num = MAX(1. - p, DBL_MIN);
 147     denom = 1. - pow(1. - ep,model_points);
 148     if( denom < DBL_MIN )
 149         EXIT;
 150
 151     num = log(num);
 152     denom = log(denom);
 153
 154     result = denom >= 0 || -num >= max_iters*(-denom) ?
 155         max_iters : cvRound(num/denom);
 156
 157     __END__;
 158
 159     return result;
 160 }
 161
 162 bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
 163                                         CvMat* mask, double reprojThreshold,
 164                                         double confidence, int maxIters )
 165 {
 166     bool result = false;
 167     CvMat* mask0 = mask, *tmask = 0, *t;
 168     CvMat* models = 0, *err = 0;
 169     CvMat *ms1 = 0, *ms2 = 0;
 170
 171     CV_FUNCNAME( "CvModelEstimator2::estimateRansac" );
 172
 173     __BEGIN__;
 174
 175     int iter, niters = maxIters;
 176     int count = m1->rows*m1->cols, maxGoodCount = 0;
 177     CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
 178
 179     if( count < modelPoints )
 180         EXIT;
 181
 182     models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
 183     err = cvCreateMat( 1, count, CV_32FC1 );
 184     tmask = cvCreateMat( 1, count, CV_8UC1 );
 185
 186     if( count > modelPoints )
 187     {
 188         ms1 = cvCreateMat( 1, modelPoints, m1->type );
 189         ms2 = cvCreateMat( 1, modelPoints, m2->type );
 190     }
 191     else
 192     {
 193         niters = 1;
 194         ms1 = (CvMat*)m1;
 195         ms2 = (CvMat*)m2;
 196     }
 197
 198     for( iter = 0; iter < niters; iter++ )
 199     {
 200         int i, goodCount, nmodels;
 201         if( count > modelPoints )
 202         {
 203             bool found = getSubset( m1, m2, ms1, ms2, modelPoints );
 204             if( !found )
 205             {
 206                 if( iter == 0 )
 207                     EXIT;
 208                 break;
 209             }
 210         }
 211
 212         nmodels = runKernel( ms1, ms2, models );
 213         if( nmodels <= 0 )
 214             continue;
 215         for( i = 0; i < nmodels; i++ )
 216         {
 217             CvMat model_i;
 218             cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
 219             goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
 220
 221             if( goodCount > MAX(maxGoodCount, modelPoints-1) )
 222             {
 223                 CV_SWAP( tmask, mask, t );
 224                 cvCopy( &model_i, model );
 225                 maxGoodCount = goodCount;
 226                 niters = cvRANSACUpdateNumIters( confidence,
 227                     (double)(count - goodCount)/count, modelPoints, niters );
 228             }
 229         }
 230     }
 231
 232     if( maxGoodCount > 0 )
 233     {
 234         if( mask != mask0 )
 235         {
 236             CV_SWAP( tmask, mask, t );
 237             cvCopy( tmask, mask );
 238         }
 239         result = true;
 240     }
 241
 242     __END__;
 243
 244     if( ms1 != m1 )
 245         cvReleaseMat( &ms1 );
 246     if( ms2 != m2 )
 247         cvReleaseMat( &ms2 );
 248     cvReleaseMat( &models );
 249     cvReleaseMat( &err );
 250     cvReleaseMat( &tmask );
 251     return result;
 252 }
 253
 254
 255 static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
 256
 257 bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
 258                                   CvMat* mask, double confidence, int maxIters )
 259 {
 260     const double outlierRatio = 0.45;
 261     bool result = false;
 262     CvMat* models = 0;
 263     CvMat *ms1 = 0, *ms2 = 0;
 264     CvMat* err = 0;
 265
 266     CV_FUNCNAME( "CvModelEstimator2::estimateLMeDS" );
 267
 268     __BEGIN__;
 269
 270     int iter, niters = maxIters;
 271     int count = m1->rows*m1->cols;
 272     double minMedian = DBL_MAX, sigma;
 273
 274     CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
 275
 276     if( count < modelPoints )
 277         EXIT;
 278
 279     models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
 280     err = cvCreateMat( 1, count, CV_32FC1 );
 281
 282     if( count > modelPoints )
 283     {
 284         ms1 = cvCreateMat( 1, modelPoints, m1->type );
 285         ms2 = cvCreateMat( 1, modelPoints, m2->type );
 286     }
 287     else
 288     {
 289         niters = 1;
 290         ms1 = (CvMat*)m1;
 291         ms2 = (CvMat*)m2;
 292     }
 293
 294     niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
 295     niters = MIN( MAX(niters, 3), maxIters );
 296
 297     for( iter = 0; iter < niters; iter++ )
 298     {
 299         int i, nmodels;
 300         if( count > modelPoints )
 301         {
 302             bool found = getSubset( m1, m2, ms1, ms2, 300 );
 303             if( !found )
 304             {
 305                 if( iter == 0 )
 306                     EXIT;
 307                 break;
 308             }
 309         }
 310
 311         nmodels = runKernel( ms1, ms2, models );
 312         if( nmodels <= 0 )
 313             continue;
 314         for( i = 0; i < nmodels; i++ )
 315         {
 316             CvMat model_i;
 317             cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
 318             computeReprojError( m1, m2, &model_i, err );
 319             icvSortDistances( err->data.i, count, 0 );
 320
 321             double median = count % 2 != 0 ?
 322                 err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
 323
 324             if( median < minMedian )
 325             {
 326                 minMedian = median;
 327                 cvCopy( &model_i, model );
 328             }
 329         }
 330     }
 331
 332     if( minMedian < DBL_MAX )
 333     {
 334         sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian);
 335         sigma = MAX( sigma, FLT_EPSILON*100 );
 336
 337         count = findInliers( m1, m2, model, err, mask, sigma );
 338         result = count >= modelPoints;
 339     }
 340
 341     __END__;
 342
 343     if( ms1 != m1 )
 344         cvReleaseMat( &ms1 );
 345     if( ms2 != m2 )
 346         cvReleaseMat( &ms2 );
 347     cvReleaseMat( &models );
 348     cvReleaseMat( &err );
 349     return result;
 350 }
 351
 352
 353 bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
 354                                    CvMat* ms1, CvMat* ms2, int maxAttempts )
 355 {
 356     int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) );
 357     int i, j, k, idx_i, iters = 0;
 358     int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
 359     const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
 360     int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
 361     int count = m1->cols*m1->rows;
 362
 363     assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
 364     elemSize /= sizeof(int);
 365
 366     for(;;)
 367     {
 368         for( i = 0; i < modelPoints && iters < maxAttempts; iters++ )
 369         {
 370             idx[i] = idx_i = cvRandInt(&rng) % count;
 371             for( j = 0; j < i; j++ )
 372                 if( idx_i == idx[j] )
 373                     break;
 374             if( j < i )
 375                 continue;
 376             for( k = 0; k < elemSize; k++ )
 377             {
 378                 ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
 379                 ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
 380             }
 381             if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
 382                 continue;
 383             i++;
 384             iters = 0;
 385         }
 386         if( !checkPartialSubsets && i == modelPoints &&
 387             (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
 388             continue;
 389         break;
 390     }
 391
 392     return i == modelPoints;
 393 }
 394
 395
 396 bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
 397 {
 398     int j, k, i = count-1;
 399     CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
 400
 401     assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
 402
 403     // check that the i-th selected point does not belong
 404     // to a line connecting some previously selected points
 405     for( j = 0; j < i; j++ )
 406     {
 407         double dx1 = ptr[j].x - ptr[i].x;
 408         double dy1 = ptr[j].y - ptr[i].y;
 409         for( k = 0; k < j; k++ )
 410         {
 411             double dx2 = ptr[k].x - ptr[i].x;
 412             double dy2 = ptr[k].y - ptr[i].y;
 413             if( fabs(dx2*dy1 - dy2*dx1) < FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
 414                 break;
 415         }
 416         if( k < j )
 417             break;
 418     }
 419
 420     return j == i;
 421 }
 422
 423
 424 class CvHomographyEstimator : public CvModelEstimator2
 425 {
 426 public:
 427     CvHomographyEstimator( int modelPoints );
 428
 429     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
 430     virtual bool refine( const CvMat* m1, const CvMat* m2,
 431                          CvMat* model, int maxIters );
 432 protected:
 433     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
 434                                      const CvMat* model, CvMat* error );
 435 };
 436
 437
 438 CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
 439     : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
 440 {
 441     assert( _modelPoints == 4 || _modelPoints == 5 );
 442 }
 443
 444 int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
 445 {
 446     int i, count = m1->rows*m1->cols;
 447     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
 448     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
 449
 450     double LtL[9][9], W[9][9], V[9][9];
 451     CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
 452     CvMat _W = cvMat( 9, 9, CV_64F, W );
 453     CvMat _V = cvMat( 9, 9, CV_64F, V );
 454     CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
 455     CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
 456     CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
 457
 458     for( i = 0; i < count; i++ )
 459     {
 460         cm.x += m[i].x; cm.y += m[i].y;
 461         cM.x += M[i].x; cM.y += M[i].y;
 462     }
 463
 464     cm.x /= count; cm.y /= count;
 465     cM.x /= count; cM.y /= count;
 466
 467     for( i = 0; i < count; i++ )
 468     {
 469         sm.x += fabs(m[i].x - cm.x);
 470         sm.y += fabs(m[i].y - cm.y);
 471         sM.x += fabs(M[i].x - cM.x);
 472         sM.y += fabs(M[i].y - cM.y);
 473     }
 474
 475     sm.x = count/sm.x; sm.y = count/sm.y;
 476     sM.x = count/sM.x; sM.y = count/sM.y;
 477
 478     double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
 479     double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
 480     CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
 481     CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
 482
 483     cvZero( &_LtL );
 484     for( i = 0; i < count; i++ )
 485     {
 486         double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
 487         double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
 488         double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
 489         double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
 490         int j, k;
 491         for( j = 0; j < 9; j++ )
 492             for( k = j; k < 9; k++ )
 493                 LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
 494     }
 495     cvCompleteSymm( &_LtL );
 496
 497     cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
 498     cvMatMul( &_invHnorm, &_H0, &_Htemp );
 499     cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
 500     cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
 501
 502     return 1;
 503 }
 504
 505
 506 void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
 507                                                 const CvMat* model, CvMat* _err )
 508 {
 509     int i, count = m1->rows*m1->cols;
 510     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
 511     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
 512     const double* H = model->data.db;
 513     float* err = _err->data.fl;
 514
 515     for( i = 0; i < count; i++ )
 516     {
 517         double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
 518         double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
 519         double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
 520         err[i] = (float)(dx*dx + dy*dy);
 521     }
 522 }
 523
 524 bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
 525 {
 526     CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
 527     int i, j, k, count = m1->rows*m1->cols;
 528     const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
 529     const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
 530     CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
 531     cvCopy( &modelPart, solver.param );
 532
 533     for(;;)
 534     {
 535         const CvMat* _param = 0;
 536         CvMat *_JtJ = 0, *_JtErr = 0;
 537         double* _errNorm = 0;
 538
 539         if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
 540             break;
 541
 542         for( i = 0; i < count; i++ )
 543         {
 544             const double* h = _param->data.db;
 545             double Mx = M[i].x, My = M[i].y;
 546             double ww = 1./(h[6]*Mx + h[7]*My + 1.);
 547             double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
 548             double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
 549             double err[] = { _xi - m[i].x, _yi - m[i].y };
 550             if( _JtJ || _JtErr )
 551             {
 552                 double J[][8] =
 553                 {
 554                     { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
 555                     { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
 556                 };
 557
 558                 for( j = 0; j < 8; j++ )
 559                 {
 560                     for( k = j; k < 8; k++ )
 561                         _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
 562                     _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
 563                 }
 564             }
 565             if( _errNorm )
 566                 *_errNorm += err[0]*err[0] + err[1]*err[1];
 567         }
 568     }
 569
 570     cvCopy( solver.param, &modelPart );
 571     return true;
 572 }
 573
 574
 575 CV_IMPL int
 576 cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
 577                   CvMat* __H, int method, double ransacReprojThreshold,
 578                   CvMat* mask )
 579 {
 580     const double confidence = 0.99;
 581     bool result = false;
 582     CvMat *m = 0, *M = 0, *tempMask = 0;
 583
 584     CV_FUNCNAME( "cvFindHomography" );
 585
 586     __BEGIN__;
 587
 588     double H[9];
 589     CvMat _H = cvMat( 3, 3, CV_64FC1, H );
 590     int count;
 591
 592     CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
 593
 594     count = MAX(imagePoints->cols, imagePoints->rows);
 595     CV_ASSERT( count >= 4 );
 596
 597     m = cvCreateMat( 1, count, CV_64FC2 );
 598     cvConvertPointsHomogeneous( imagePoints, m );
 599
 600     M = cvCreateMat( 1, count, CV_64FC2 );
 601     cvConvertPointsHomogeneous( objectPoints, M );
 602
 603     if( mask )
 604     {
 605         CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
 606             (mask->rows == 1 || mask->cols == 1) &&
 607             mask->rows*mask->cols == count );
 608         tempMask = mask;
 609     }
 610     else if( count > 4 )
 611         tempMask = cvCreateMat( 1, count, CV_8U );
 612
 613     {
 614     CvHomographyEstimator estimator( MIN(count, 5) );
 615     if( count == 4 )
 616         method = 0;
 617     if( method == CV_LMEDS )
 618         result = estimator.runLMeDS( M, m, &_H, tempMask, confidence );
 619     else if( method == CV_RANSAC )
 620         result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence );
 621     else
 622         result = estimator.runKernel( M, m, &_H ) > 0;
 623
 624     if( result && count > 4 )
 625     {
 626         icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
 627         count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
 628         M->cols = m->cols = count;
 629         estimator.refine( M, m, &_H, 10 );
 630     }
 631     }
 632
 633     if( result )
 634         cvConvert( &_H, __H );
 635
 636     __END__;
 637
 638     cvReleaseMat( &m );
 639     cvReleaseMat( &M );
 640     if( tempMask != mask )
 641         cvReleaseMat( &tempMask );
 642
 643     return (int)result;
 644 }
 645
 646
 647 /* Evaluation of Fundamental Matrix from point correspondences.
 648    The original code has been written by Valery Mosyagin */
 649
 650 /* The algorithms (except for RANSAC) and the notation have been taken from
 651    Zhengyou Zhang's research report
 652    "Determining the Epipolar Geometry and its Uncertainty: A Review"
 653    that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
 654
 655 /************************************** 7-point algorithm *******************************/
 656 class CvFMEstimator : public CvModelEstimator2
 657 {
 658 public:
 659     CvFMEstimator( int _modelPoints );
 660
 661     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
 662     virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
 663     virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
 664 protected:
 665     virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
 666                                      const CvMat* model, CvMat* error );
 667 };
 668
 669 CvFMEstimator::CvFMEstimator( int _modelPoints )
 670 : CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
 671 {
 672     assert( _modelPoints == 7 || _modelPoints == 8 );
 673 }
 674
 675
 676 int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
 677 {
 678     return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
 679 }
 680
 681 int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
 682 {
 683     double a[7*9], w[7], v[9*9], c[4], r[3];
 684     double* f1, *f2;
 685     double t0, t1, t2;
 686     CvMat A = cvMat( 7, 9, CV_64F, a );
 687     CvMat V = cvMat( 9, 9, CV_64F, v );
 688     CvMat W = cvMat( 7, 1, CV_64F, w );
 689     CvMat coeffs = cvMat( 1, 4, CV_64F, c );
 690     CvMat roots = cvMat( 1, 3, CV_64F, r );
 691     const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
 692     const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
 693     double* fmatrix = _fmatrix->data.db;
 694     int i, k, n;
 695
 696     // form a linear system: i-th row of A(=a) represents
 697     // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
 698     for( i = 0; i < 7; i++ )
 699     {
 700         double x0 = m1[i].x, y0 = m1[i].y;
 701         double x1 = m2[i].x, y1 = m2[i].y;
 702
 703         a[i*9+0] = x1*x0;
 704         a[i*9+1] = x1*y0;
 705         a[i*9+2] = x1;
 706         a[i*9+3] = y1*x0;
 707         a[i*9+4] = y1*y0;
 708         a[i*9+5] = y1;
 709         a[i*9+6] = x0;
 710         a[i*9+7] = y0;
 711         a[i*9+8] = 1;
 712     }
 713
 714     // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
 715     // the solution is linear subspace of dimensionality 2.
 716     // => use the last two singular vectors as a basis of the space
 717     // (according to SVD properties)
 718     cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
 719     f1 = v + 7*9;
 720     f2 = v + 8*9;
 721
 722     // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
 723     // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
 724     // so f ~ lambda*f1 + (1 - lambda)*f2.
 725     // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
 726     // it will be a cubic equation.
 727     // find c - polynomial coefficients.
 728     for( i = 0; i < 9; i++ )
 729         f1[i] -= f2[i];
 730
 731     t0 = f2[4]*f2[8] - f2[5]*f2[7];
 732     t1 = f2[3]*f2[8] - f2[5]*f2[6];
 733     t2 = f2[3]*f2[7] - f2[4]*f2[6];
 734
 735     c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;
 736
 737     c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
 738            f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
 739            f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
 740            f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
 741            f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
 742            f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
 743            f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);
 744
 745     t0 = f1[4]*f1[8] - f1[5]*f1[7];
 746     t1 = f1[3]*f1[8] - f1[5]*f1[6];
 747     t2 = f1[3]*f1[7] - f1[4]*f1[6];
 748
 749     c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
 750            f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
 751            f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
 752            f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
 753            f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
 754            f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
 755            f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);
 756
 757     c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;
 758
 759     // solve the cubic equation; there can be 1 to 3 roots ...
 760     n = cvSolveCubic( &coeffs, &roots );
 761
 762     if( n < 1 || n > 3 )
 763         return n;
 764
 765     for( k = 0; k < n; k++, fmatrix += 9 )
 766     {
 767         // for each root form the fundamental matrix
 768         double lambda = r[k], mu = 1.;
 769         double s = f1[8]*r[k] + f2[8];
 770
 771         // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
 772         if( fabs(s) > DBL_EPSILON )
 773         {
 774             mu = 1./s;
 775             lambda *= mu;
 776             fmatrix[8] = 1.;
 777         }
 778         else
 779             fmatrix[8] = 0.;
 780
 781         for( i = 0; i < 8; i++ )
 782             fmatrix[i] = f1[i]*lambda + f2[i]*mu;
 783     }
 784
 785     return n;
 786 }
 787
 788
 789 int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
 790 {
 791     double a[9*9], w[9], v[9*9];
 792     CvMat W = cvMat( 1, 9, CV_64F, w );
 793     CvMat V = cvMat( 9, 9, CV_64F, v );
 794     CvMat A = cvMat( 9, 9, CV_64F, a );
 795     CvMat U, F0, TF;
 796
 797     CvPoint2D64f m0c = {0,0}, m1c = {0,0};
 798     double t, scale0 = 0, scale1 = 0;
 799
 800     const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
 801     const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
 802     double* fmatrix = _fmatrix->data.db;
 803     int i, j, k, count = _m1->cols*_m1->rows;
 804
 805     // compute centers and average distances for each of the two point sets
 806     for( i = 0; i < count; i++ )
 807     {
 808         double x = m1[i].x, y = m1[i].y;
 809         m0c.x += x; m0c.y += y;
 810
 811         x = m2[i].x, y = m2[i].y;
 812         m1c.x += x; m1c.y += y;
 813     }
 814
 815     // calculate the normalizing transformations for each of the point sets:
 816     // after the transformation each set will have the mass center at the coordinate origin
 817     // and the average distance from the origin will be ~sqrt(2).
 818     t = 1./count;
 819     m0c.x *= t; m0c.y *= t;
 820     m1c.x *= t; m1c.y *= t;
 821
 822     for( i = 0; i < count; i++ )
 823     {
 824         double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
 825         scale0 += sqrt(x*x + y*y);
 826
 827         x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y);
 828         scale1 += sqrt(x*x + y*y);
 829     }
 830
 831     scale0 *= t;
 832     scale1 *= t;
 833
 834     if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
 835         return 0;
 836
 837     scale0 = sqrt(2.)/scale0;
 838     scale1 = sqrt(2.)/scale1;
 839
 840     cvZero( &A );
 841
 842     // form a linear system Ax=0: for each selected pair of points m1 & m2,
 843     // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
 844     // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
 845     for( i = 0; i < count; i++ )
 846     {
 847         double x0 = (m1[i].x - m0c.x)*scale0;
 848         double y0 = (m1[i].y - m0c.y)*scale0;
 849         double x1 = (m2[i].x - m1c.x)*scale1;
 850         double y1 = (m2[i].y - m1c.y)*scale1;
 851         double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
 852         for( j = 0; j < 9; j++ )
 853             for( k = 0; k < 9; k++ )
 854                 a[j*9+k] += r[j]*r[k];
 855     }
 856
 857     cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
 858
 859     for( i = 0; i < 8; i++ )
 860     {
 861         if( fabs(w[i]) < FLT_EPSILON )
 862             break;
 863     }
 864
 865     if( i < 7 )
 866         return 0;
 867
 868     F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
 869
 870     // make F0 singular (of rank 2) by decomposing it with SVD,
 871     // zeroing the last diagonal element of W and then composing the matrices back.
 872
 873     // use v as a temporary storage for different 3x3 matrices
 874     W = U = V = TF = F0;
 875     W.data.db = v;
 876     U.data.db = v + 9;
 877     V.data.db = v + 18;
 878     TF.data.db = v + 27;
 879
 880     cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 881     W.data.db[8] = 0.;
 882
 883     // F0 <- U*diag([W(1), W(2), 0])*V'
 884     cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
 885     cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );
 886
 887     // apply the transformation that is inverse
 888     // to what we used to normalize the point coordinates
 889     {
 890         double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
 891         double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
 892         CvMat T0, T1;
 893         T0 = T1 = F0;
 894         T0.data.db = tt0;
 895         T1.data.db = tt1;
 896
 897         // F0 <- T1'*F0*T0
 898         cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
 899         F0.data.db = fmatrix;
 900         cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );
 901
 902         // make F(3,3) = 1
 903         if( fabs(F0.data.db[8]) > FLT_EPSILON )
 904             cvScale( &F0, &F0, 1./F0.data.db[8] );
 905     }
 906
 907     return 1;
 908 }
 909
 910
 911 void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
 912                                         const CvMat* model, CvMat* _err )
 913 {
 914     int i, count = _m1->rows*_m1->cols;
 915     const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
 916     const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
 917     const double* F = model->data.db;
 918     float* err = _err->data.fl;
 919
 920     for( i = 0; i < count; i++ )
 921     {
 922         double a, b, c, d1, d2, s1, s2;
 923
 924         a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
 925         b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
 926         c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
 927
 928         s2 = 1./(a*a + b*b);
 929         d2 = m2[i].x*a + m2[i].y*b + c;
 930
 931         a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
 932         b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
 933         c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
 934
 935         s1 = 1./(a*a + b*b);
 936         d1 = m1[i].x*a + m1[i].y*b + c;
 937
 938         err[i] = (float)(d1*d1*s1 + d2*d2*s2);
 939     }
 940 }
 941
 942
 943 CV_IMPL int
 944 cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
 945                       CvMat* fmatrix, int method,
 946                       double param1, double param2, CvMat* mask )
 947 {
 948     int result = 0;
 949     CvMat *m1 = 0, *m2 = 0, *tempMask = 0;
 950
 951     CV_FUNCNAME( "cvFindFundamentalMat" );
 952
 953     __BEGIN__;
 954
 955     double F[3*9];
 956     CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
 957     int count;
 958
 959     CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
 960     CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
 961         (fmatrix->rows == 3 || fmatrix->rows == 9 && method == CV_FM_7POINT) );
 962
 963     count = MAX(points1->cols, points1->rows);
 964     if( count < 7 )
 965         EXIT;
 966
 967     m1 = cvCreateMat( 1, count, CV_64FC2 );
 968     cvConvertPointsHomogeneous( points1, m1 );
 969
 970     m2 = cvCreateMat( 1, count, CV_64FC2 );
 971     cvConvertPointsHomogeneous( points2, m2 );
 972
 973     if( mask )
 974     {
 975         CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
 976             (mask->rows == 1 || mask->cols == 1) &&
 977             mask->rows*mask->cols == count );
 978         tempMask = mask;
 979     }
 980     else if( count > 8 )
 981         tempMask = cvCreateMat( 1, count, CV_8U );
 982
 983     {
 984     CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) );
 985     if( count == 7 )
 986         result = estimator.run7Point(m1, m2, &_F9x3);
 987     else if( count == 8 || method == CV_FM_8POINT )
 988         result = estimator.run8Point(m1, m2, &_F3x3);
 989     else if( count > 8 )
 990     {
 991         if( param1 <= 0 )
 992             param1 = 3;
 993         if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
 994             param2 = 0.99;
 995
 996         if( (method & ~3) == CV_RANSAC )
 997             result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
 998         else
 999             result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
1000         if( result <= 0 )
1001             EXIT;
1002         icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
1003         count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
1004         assert( count >= 8 );
1005         m1->cols = m2->cols = count;
1006         estimator.run8Point(m1, m2, &_F3x3);
1007     }
1008     }
1009
1010     if( result )
1011         cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
1012
1013     __END__;
1014
1015     cvReleaseMat( &m1 );
1016     cvReleaseMat( &m2 );
1017     if( tempMask != mask )
1018         cvReleaseMat( &tempMask );
1019
1020     return result;
1021 }
1022
1023
1024 CV_IMPL void
1025 cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
1026                              const CvMat* fmatrix, CvMat* lines )
1027 {
1028     CV_FUNCNAME( "cvComputeCorrespondEpilines" );
1029
1030     __BEGIN__;
1031
1032     int abc_stride, abc_plane_stride, abc_elem_size;
1033     int plane_stride, stride, elem_size;
1034     int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
1035     uchar *ap, *bp, *cp;
1036     const uchar *xp, *yp, *zp;
1037     double f[9];
1038     CvMat F = cvMat( 3, 3, CV_64F, f );
1039
1040     if( !CV_IS_MAT(points) )
1041         CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
1042
1043     depth = CV_MAT_DEPTH(points->type);
1044     cn = CV_MAT_CN(points->type);
1045     if( depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
1046         CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
1047
1048     if( points->rows > points->cols )
1049     {
1050         dims = cn*points->cols;
1051         count = points->rows;
1052     }
1053     else
1054     {
1055         if( points->rows > 1 && cn > 1 || points->rows == 1 && cn == 1 )
1056             CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
1057         dims = cn * points->rows;
1058         count = points->cols;
1059     }
1060
1061     if( dims != 2 && dims != 3 )
1062         CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
1063
1064     if( !CV_IS_MAT(fmatrix) )
1065         CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
1066
1067     if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
1068         CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
1069
1070     if( fmatrix->cols != 3 || fmatrix->rows != 3 )
1071         CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );
1072
1073     if( !CV_IS_MAT(lines) )
1074         CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
1075
1076     abc_depth = CV_MAT_DEPTH(lines->type);
1077     abc_cn = CV_MAT_CN(lines->type);
1078     if( abc_depth != CV_32F && abc_depth != CV_64F || abc_cn != 1 && abc_cn != 3 )
1079         CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
1080
1081     if( lines->rows > lines->cols )
1082     {
1083         abc_dims = abc_cn*lines->cols;
1084         abc_count = lines->rows;
1085     }
1086     else
1087     {
1088         if( lines->rows > 1 && abc_cn > 1 || lines->rows == 1 && abc_cn == 1 )
1089             CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
1090         abc_dims = abc_cn * lines->rows;
1091         abc_count = lines->cols;
1092     }
1093
1094     if( abc_dims != 3 )
1095         CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
1096
1097     if( abc_count != count )
1098         CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
1099
1100     elem_size = CV_ELEM_SIZE(depth);
1101     abc_elem_size = CV_ELEM_SIZE(abc_depth);
1102
1103     if( points->rows == dims )
1104     {
1105         plane_stride = points->step;
1106         stride = elem_size;
1107     }
1108     else
1109     {
1110         plane_stride = elem_size;
1111         stride = points->rows == 1 ? dims*elem_size : points->step;
1112     }
1113
1114     if( lines->rows == 3 )
1115     {
1116         abc_plane_stride = lines->step;
1117         abc_stride = abc_elem_size;
1118     }
1119     else
1120     {
1121         abc_plane_stride = abc_elem_size;
1122         abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
1123     }
1124
1125     CV_CALL( cvConvert( fmatrix, &F ));
1126     if( pointImageID == 2 )
1127         cvTranspose( &F, &F );
1128
1129     xp = points->data.ptr;
1130     yp = xp + plane_stride;
1131     zp = dims == 3 ? yp + plane_stride : 0;
1132
1133     ap = lines->data.ptr;
1134     bp = ap + abc_plane_stride;
1135     cp = bp + abc_plane_stride;
1136
1137     for( i = 0; i < count; i++ )
1138     {
1139         double x, y, z = 1.;
1140         double a, b, c, nu;
1141
1142         if( depth == CV_32F )
1143         {
1144             x = *(float*)xp; y = *(float*)yp;
1145             if( zp )
1146                 z = *(float*)zp, zp += stride;
1147         }
1148         else
1149         {
1150             x = *(double*)xp; y = *(double*)yp;
1151             if( zp )
1152                 z = *(double*)zp, zp += stride;
1153         }
1154
1155         xp += stride; yp += stride;
1156
1157         a = f[0]*x + f[1]*y + f[2]*z;
1158         b = f[3]*x + f[4]*y + f[5]*z;
1159         c = f[6]*x + f[7]*y + f[8]*z;
1160         nu = a*a + b*b;
1161         nu = nu ? 1./sqrt(nu) : 1.;
1162         a *= nu; b *= nu; c *= nu;
1163
1164         if( abc_depth == CV_32F )
1165         {
1166             *(float*)ap = (float)a;
1167             *(float*)bp = (float)b;
1168             *(float*)cp = (float)c;
1169         }
1170         else
1171         {
1172             *(double*)ap = a;
1173             *(double*)bp = b;
1174             *(double*)cp = c;
1175         }
1176
1177         ap += abc_stride;
1178         bp += abc_stride;
1179         cp += abc_stride;
1180     }
1181
1182     __END__;
1183 }
1184
1185
1186 CV_IMPL void
1187 cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
1188 {
1189     CvMat* temp = 0;
1190     CvMat* denom = 0;
1191
1192     CV_FUNCNAME( "cvConvertPointsHomogeneous" );
1193
1194     __BEGIN__;
1195
1196     int i, s_count, s_dims, d_count, d_dims;
1197     CvMat _src, _dst, _ones;
1198     CvMat* ones = 0;
1199
1200     if( !CV_IS_MAT(src) )
1201         CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
1202         "The input parameter is not a valid matrix" );
1203
1204     if( !CV_IS_MAT(dst) )
1205         CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
1206         "The output parameter is not a valid matrix" );
1207
1208     if( src == dst || src->data.ptr == dst->data.ptr )
1209     {
1210         if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
1211             CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
1212         EXIT;
1213     }
1214
1215     if( src->rows > src->cols )
1216     {
1217         if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
1218             CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
1219
1220         s_dims = CV_MAT_CN(src->type)*src->cols;
1221         s_count = src->rows;
1222     }
1223     else
1224     {
1225         if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
1226             CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
1227
1228         s_dims = CV_MAT_CN(src->type)*src->rows;
1229         s_count = src->cols;
1230     }
1231
1232     if( src->rows == 1 || src->cols == 1 )
1233         src = cvReshape( src, &_src, 1, s_count );
1234
1235     if( dst->rows > dst->cols )
1236     {
1237         if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
1238             CV_ERROR( CV_StsBadSize,
1239             "Either the number of channels or columns or rows in the input matrix must be =1" );
1240
1241         d_dims = CV_MAT_CN(dst->type)*dst->cols;
1242         d_count = dst->rows;
1243     }
1244     else
1245     {
1246         if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
1247             CV_ERROR( CV_StsBadSize,
1248             "Either the number of channels or columns or rows in the output matrix must be =1" );
1249
1250         d_dims = CV_MAT_CN(dst->type)*dst->rows;
1251         d_count = dst->cols;
1252     }
1253
1254     if( dst->rows == 1 || dst->cols == 1 )
1255         dst = cvReshape( dst, &_dst, 1, d_count );
1256
1257     if( s_count != d_count )
1258         CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
1259
1260     if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
1261         CV_ERROR( CV_StsUnsupportedFormat,
1262         "Both matrices must be floating-point (single or double precision)" );
1263
1264     if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
1265         CV_ERROR( CV_StsOutOfRange,
1266         "Both input and output point dimensionality must be 2, 3 or 4" );
1267
1268     if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
1269         CV_ERROR( CV_StsUnmatchedSizes,
1270         "The dimensionalities of input and output point sets differ too much" );
1271
1272     if( s_dims == d_dims - 1 )
1273     {
1274         if( d_count == dst->rows )
1275         {
1276             ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
1277             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
1278         }
1279         else
1280         {
1281             ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
1282             dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
1283         }
1284     }
1285
1286     if( s_dims <= d_dims )
1287     {
1288         if( src->rows == dst->rows && src->cols == dst->cols )
1289         {
1290             if( CV_ARE_TYPES_EQ( src, dst ) )
1291                 cvCopy( src, dst );
1292             else
1293                 cvConvert( src, dst );
1294         }
1295         else
1296         {
1297             if( !CV_ARE_TYPES_EQ( src, dst ))
1298             {
1299                 CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
1300                 cvConvert( src, temp );
1301                 src = temp;
1302             }
1303             cvTranspose( src, dst );
1304         }
1305
1306         if( ones )
1307             cvSet( ones, cvRealScalar(1.) );
1308     }
1309     else
1310     {
1311         int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
1312
1313         if( !CV_ARE_TYPES_EQ( src, dst ))
1314         {
1315             CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
1316             cvConvert( src, temp );
1317             src = temp;
1318         }
1319
1320         elem_size = CV_ELEM_SIZE(src->type);
1321
1322         if( s_count == src->cols )
1323             s_plane_stride = src->step / elem_size, s_stride = 1;
1324         else
1325             s_stride = src->step / elem_size, s_plane_stride = 1;
1326
1327         if( d_count == dst->cols )
1328             d_plane_stride = dst->step / elem_size, d_stride = 1;
1329         else
1330             d_stride = dst->step / elem_size, d_plane_stride = 1;
1331
1332         CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
1333
1334         if( CV_MAT_DEPTH(dst->type) == CV_32F )
1335         {
1336             const float* xs = src->data.fl;
1337             const float* ys = xs + s_plane_stride;
1338             const float* zs = 0;
1339             const float* ws = xs + (s_dims - 1)*s_plane_stride;
1340
1341             float* iw = denom->data.fl;
1342
1343             float* xd = dst->data.fl;
1344             float* yd = xd + d_plane_stride;
1345             float* zd = 0;
1346
1347             if( d_dims == 3 )
1348             {
1349                 zs = ys + s_plane_stride;
1350                 zd = yd + d_plane_stride;
1351             }
1352
1353             for( i = 0; i < d_count; i++, ws += s_stride )
1354             {
1355                 float t = *ws;
1356                 iw[i] = t ? t : 1.f;
1357             }
1358
1359             cvDiv( 0, denom, denom );
1360
1361             if( d_dims == 3 )
1362                 for( i = 0; i < d_count; i++ )
1363                 {
1364                     float w = iw[i];
1365                     float x = *xs * w, y = *ys * w, z = *zs * w;
1366                     xs += s_stride; ys += s_stride; zs += s_stride;
1367                     *xd = x; *yd = y; *zd = z;
1368                     xd += d_stride; yd += d_stride; zd += d_stride;
1369                 }
1370             else
1371                 for( i = 0; i < d_count; i++ )
1372                 {
1373                     float w = iw[i];
1374                     float x = *xs * w, y = *ys * w;
1375                     xs += s_stride; ys += s_stride;
1376                     *xd = x; *yd = y;
1377                     xd += d_stride; yd += d_stride;
1378                 }
1379         }
1380         else
1381         {
1382             const double* xs = src->data.db;
1383             const double* ys = xs + s_plane_stride;
1384             const double* zs = 0;
1385             const double* ws = xs + (s_dims - 1)*s_plane_stride;
1386
1387             double* iw = denom->data.db;
1388
1389             double* xd = dst->data.db;
1390             double* yd = xd + d_plane_stride;
1391             double* zd = 0;
1392
1393             if( d_dims == 3 )
1394             {
1395                 zs = ys + s_plane_stride;
1396                 zd = yd + d_plane_stride;
1397             }
1398
1399             for( i = 0; i < d_count; i++, ws += s_stride )
1400             {
1401                 double t = *ws;
1402                 iw[i] = t ? t : 1.;
1403             }
1404
1405             cvDiv( 0, denom, denom );
1406
1407             if( d_dims == 3 )
1408                 for( i = 0; i < d_count; i++ )
1409                 {
1410                     double w = iw[i];
1411                     double x = *xs * w, y = *ys * w, z = *zs * w;
1412                     xs += s_stride; ys += s_stride; zs += s_stride;
1413                     *xd = x; *yd = y; *zd = z;
1414                     xd += d_stride; yd += d_stride; zd += d_stride;
1415                 }
1416             else
1417                 for( i = 0; i < d_count; i++ )
1418                 {
1419                     double w = iw[i];
1420                     double x = *xs * w, y = *ys * w;
1421                     xs += s_stride; ys += s_stride;
1422                     *xd = x; *yd = y;
1423                     xd += d_stride; yd += d_stride;
1424                 }
1425         }
1426     }
1427
1428     __END__;
1429
1430     cvReleaseMat( &denom );
1431     cvReleaseMat( &temp );
1432 }
1433
1434 /* End of file. */