git.maemo.org Git - opencv/blob - src/cv/cvmodelest.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 #include "_cvmodelest.h"
  44 #include <algorithm>
  45 #include <limits>
  46
  47 using namespace std;
  48
  49
  50 CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
  51 {
  52     modelPoints = _modelPoints;
  53     modelSize = _modelSize;
  54     maxBasicSolutions = _maxBasicSolutions;
  55     checkPartialSubsets = true;
  56     rng = cvRNG(-1);
  57 }
  58
  59 CvModelEstimator2::~CvModelEstimator2()
  60 {
  61 }
  62
  63 void CvModelEstimator2::setSeed( int64 seed )
  64 {
  65     rng = cvRNG(seed);
  66 }
  67
  68
  69 int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
  70                                     const CvMat* model, CvMat* _err,
  71                                     CvMat* _mask, double threshold )
  72 {
  73     int i, count = _err->rows*_err->cols, goodCount = 0;
  74     const float* err = _err->data.fl;
  75     uchar* mask = _mask->data.ptr;
  76
  77     computeReprojError( m1, m2, model, _err );
  78     threshold *= threshold;
  79     for( i = 0; i < count; i++ )
  80         goodCount += mask[i] = err[i] <= threshold;
  81     return goodCount;
  82 }
  83
  84
  85 CV_IMPL int
  86 cvRANSACUpdateNumIters( double p, double ep,
  87                         int model_points, int max_iters )
  88 {
  89     int result = 0;
  90
  91     CV_FUNCNAME( "cvRANSACUpdateNumIters" );
  92
  93     __BEGIN__;
  94
  95     double num, denom;
  96
  97     if( model_points <= 0 )
  98         CV_ERROR( CV_StsOutOfRange, "the number of model points should be positive" );
  99
 100     p = MAX(p, 0.);
 101     p = MIN(p, 1.);
 102     ep = MAX(ep, 0.);
 103     ep = MIN(ep, 1.);
 104
 105     // avoid inf's & nan's
 106     num = MAX(1. - p, DBL_MIN);
 107     denom = 1. - pow(1. - ep,model_points);
 108     if( denom < DBL_MIN )
 109         EXIT;
 110
 111     num = log(num);
 112     denom = log(denom);
 113
 114     result = denom >= 0 || -num >= max_iters*(-denom) ?
 115         max_iters : cvRound(num/denom);
 116
 117     __END__;
 118
 119     return result;
 120 }
 121
 122 bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
 123                                         CvMat* mask, double reprojThreshold,
 124                                         double confidence, int maxIters )
 125 {
 126     bool result = false;
 127     CvMat* mask0 = mask, *tmask = 0, *t;
 128     CvMat* models = 0, *err = 0;
 129     CvMat *ms1 = 0, *ms2 = 0;
 130
 131     CV_FUNCNAME( "CvModelEstimator2::estimateRansac" );
 132
 133     __BEGIN__;
 134
 135     int iter, niters = maxIters;
 136     int count = m1->rows*m1->cols, maxGoodCount = 0;
 137     CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
 138
 139     if( count < modelPoints )
 140         EXIT;
 141
 142     models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
 143     err = cvCreateMat( 1, count, CV_32FC1 );
 144     tmask = cvCreateMat( 1, count, CV_8UC1 );
 145
 146     if( count > modelPoints )
 147     {
 148         ms1 = cvCreateMat( 1, modelPoints, m1->type );
 149         ms2 = cvCreateMat( 1, modelPoints, m2->type );
 150     }
 151     else
 152     {
 153         niters = 1;
 154         ms1 = (CvMat*)m1;
 155         ms2 = (CvMat*)m2;
 156     }
 157
 158     for( iter = 0; iter < niters; iter++ )
 159     {
 160         int i, goodCount, nmodels;
 161         if( count > modelPoints )
 162         {
 163             bool found = getSubset( m1, m2, ms1, ms2, 300 );
 164             if( !found )
 165             {
 166                 if( iter == 0 )
 167                     EXIT;
 168                 break;
 169             }
 170         }
 171
 172         nmodels = runKernel( ms1, ms2, models );
 173         if( nmodels <= 0 )
 174             continue;
 175         for( i = 0; i < nmodels; i++ )
 176         {
 177             CvMat model_i;
 178             cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
 179             goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
 180
 181             if( goodCount > MAX(maxGoodCount, modelPoints-1) )
 182             {
 183                 CV_SWAP( tmask, mask, t );
 184                 cvCopy( &model_i, model );
 185                 maxGoodCount = goodCount;
 186                 niters = cvRANSACUpdateNumIters( confidence,
 187                     (double)(count - goodCount)/count, modelPoints, niters );
 188             }
 189         }
 190     }
 191
 192     if( maxGoodCount > 0 )
 193     {
 194         if( mask != mask0 )
 195         {
 196             CV_SWAP( tmask, mask, t );
 197             cvCopy( tmask, mask );
 198         }
 199         result = true;
 200     }
 201
 202     __END__;
 203
 204     if( ms1 != m1 )
 205         cvReleaseMat( &ms1 );
 206     if( ms2 != m2 )
 207         cvReleaseMat( &ms2 );
 208     cvReleaseMat( &models );
 209     cvReleaseMat( &err );
 210     cvReleaseMat( &tmask );
 211     return result;
 212 }
 213
 214
 215 static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
 216
 217 bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
 218                                   CvMat* mask, double confidence, int maxIters )
 219 {
 220     const double outlierRatio = 0.45;
 221     bool result = false;
 222     CvMat* models = 0;
 223     CvMat *ms1 = 0, *ms2 = 0;
 224     CvMat* err = 0;
 225
 226     CV_FUNCNAME( "CvModelEstimator2::estimateLMeDS" );
 227
 228     __BEGIN__;
 229
 230     int iter, niters = maxIters;
 231     int count = m1->rows*m1->cols;
 232     double minMedian = DBL_MAX, sigma;
 233
 234     CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
 235
 236     if( count < modelPoints )
 237         EXIT;
 238
 239     models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
 240     err = cvCreateMat( 1, count, CV_32FC1 );
 241
 242     if( count > modelPoints )
 243     {
 244         ms1 = cvCreateMat( 1, modelPoints, m1->type );
 245         ms2 = cvCreateMat( 1, modelPoints, m2->type );
 246     }
 247     else
 248     {
 249         niters = 1;
 250         ms1 = (CvMat*)m1;
 251         ms2 = (CvMat*)m2;
 252     }
 253
 254     niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
 255     niters = MIN( MAX(niters, 3), maxIters );
 256
 257     for( iter = 0; iter < niters; iter++ )
 258     {
 259         int i, nmodels;
 260         if( count > modelPoints )
 261         {
 262             bool found = getSubset( m1, m2, ms1, ms2, 300 );
 263             if( !found )
 264             {
 265                 if( iter == 0 )
 266                     EXIT;
 267                 break;
 268             }
 269         }
 270
 271         nmodels = runKernel( ms1, ms2, models );
 272         if( nmodels <= 0 )
 273             continue;
 274         for( i = 0; i < nmodels; i++ )
 275         {
 276             CvMat model_i;
 277             cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
 278             computeReprojError( m1, m2, &model_i, err );
 279             icvSortDistances( err->data.i, count, 0 );
 280
 281             double median = count % 2 != 0 ?
 282                 err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
 283
 284             if( median < minMedian )
 285             {
 286                 minMedian = median;
 287                 cvCopy( &model_i, model );
 288             }
 289         }
 290     }
 291
 292     if( minMedian < DBL_MAX )
 293     {
 294         sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian);
 295         sigma = MAX( sigma, FLT_EPSILON*100 );
 296
 297         count = findInliers( m1, m2, model, err, mask, sigma );
 298         result = count >= modelPoints;
 299     }
 300
 301     __END__;
 302
 303     if( ms1 != m1 )
 304         cvReleaseMat( &ms1 );
 305     if( ms2 != m2 )
 306         cvReleaseMat( &ms2 );
 307     cvReleaseMat( &models );
 308     cvReleaseMat( &err );
 309     return result;
 310 }
 311
 312
 313 bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
 314                                    CvMat* ms1, CvMat* ms2, int maxAttempts )
 315 {
 316     int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) );
 317     int i = 0, j, k, idx_i, iters = 0;
 318     int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
 319     const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
 320     int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
 321     int count = m1->cols*m1->rows;
 322
 323     assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
 324     elemSize /= sizeof(int);
 325
 326     for(; iters < maxAttempts; iters++)
 327     {
 328         for( i = 0; i < modelPoints && iters < maxAttempts; )
 329         {
 330             idx[i] = idx_i = cvRandInt(&rng) % count;
 331             for( j = 0; j < i; j++ )
 332                 if( idx_i == idx[j] )
 333                     break;
 334             if( j < i )
 335                 continue;
 336             for( k = 0; k < elemSize; k++ )
 337             {
 338                 ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
 339                 ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
 340             }
 341             if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
 342             {
 343                 iters++;
 344                 continue;
 345             }
 346             i++;
 347         }
 348         if( !checkPartialSubsets && i == modelPoints &&
 349             (!checkSubset( ms1, i ) || !checkSubset( ms2, i )))
 350             continue;
 351         break;
 352     }
 353
 354     return i == modelPoints && iters < maxAttempts;
 355 }
 356
 357
 358 bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
 359 {
 360     int j, k, i, i0, i1;
 361     CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
 362
 363     assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
 364
 365     if( checkPartialSubsets )
 366         i0 = i1 = count - 1;
 367     else
 368         i0 = 0, i1 = count - 1;
 369
 370     for( i = i0; i <= i1; i++ )
 371     {
 372         // check that the i-th selected point does not belong
 373         // to a line connecting some previously selected points
 374         for( j = 0; j < i; j++ )
 375         {
 376             double dx1 = ptr[j].x - ptr[i].x;
 377             double dy1 = ptr[j].y - ptr[i].y;
 378             for( k = 0; k < j; k++ )
 379             {
 380                 double dx2 = ptr[k].x - ptr[i].x;
 381                 double dy2 = ptr[k].y - ptr[i].y;
 382                 if( fabs(dx2*dy1 - dy2*dx1) < FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
 383                     break;
 384             }
 385             if( k < j )
 386                 break;
 387         }
 388         if( j < i )
 389             break;
 390     }
 391
 392     return i >= i1;
 393 }
 394
 395
 396 namespace cv
 397 {
 398
 399 class Affine3DEstimator : public CvModelEstimator2
 400 {
 401 public:
 402     Affine3DEstimator() : CvModelEstimator2(4, cvSize(4, 3), 1) {}
 403     virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
 404 protected:
 405     virtual void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error );
 406     virtual bool checkSubset( const CvMat* ms1, int count );
 407 };
 408
 409 }
 410
 411 int cv::Affine3DEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
 412 {
 413     const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
 414     const Point3d* to   = reinterpret_cast<const Point3d*>(m2->data.ptr);
 415
 416     Mat A(12, 12, CV_64F);
 417     Mat B(12, 1, CV_64F);
 418     A = Scalar(0.0);
 419
 420     for(int i = 0; i < modelPoints; ++i)
 421     {
 422         *B.ptr<Point3d>(3*i) = to[i];
 423
 424         double *aptr = A.ptr<double>(3*i);
 425         for(int k = 0; k < 3; ++k)
 426         {
 427             aptr[3] = 1.0;
 428             *reinterpret_cast<Point3d*>(aptr) = from[i];
 429             aptr += 16;
 430         }
 431     }
 432
 433     CvMat cvA = A;
 434     CvMat cvB = B;
 435     CvMat cvX;
 436     cvReshape(model, &cvX, 1, 12);
 437     cvSolve(&cvA, &cvB, &cvX, CV_SVD );
 438
 439     return 1;
 440 }
 441
 442 void cv::Affine3DEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error )
 443 {
 444     int count = m1->rows * m1->cols;
 445     const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
 446     const Point3d* to   = reinterpret_cast<const Point3d*>(m2->data.ptr);
 447     const double* F = model->data.db;
 448     float* err = error->data.fl;
 449
 450     for(int i = 0; i < count; i++ )
 451     {
 452         const Point3d& f = from[i];
 453         const Point3d& t = to[i];
 454
 455         double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
 456         double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
 457         double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
 458
 459         err[i] = (float)sqrt(a*a + b*b + c*c);
 460     }
 461 }
 462
 463 bool cv::Affine3DEstimator::checkSubset( const CvMat* ms1, int count )
 464 {
 465     CV_Assert( CV_MAT_TYPE(ms1->type) == CV_64FC3 );
 466
 467     int j, k, i = count - 1;
 468     const Point3d* ptr = reinterpret_cast<const Point3d*>(ms1->data.ptr);
 469
 470     // check that the i-th selected point does not belong
 471     // to a line connecting some previously selected points
 472
 473     for(j = 0; j < i; ++j)
 474     {
 475         Point3d d1 = ptr[j] - ptr[i];
 476         double n1 = norm(d1);
 477
 478         for(k = 0; k < j; ++k)
 479         {
 480             Point3d d2 = ptr[k] - ptr[i];
 481             double n = norm(d2) * n1;
 482
 483             if (fabs(d1.dot(d2) / n) > 0.996)
 484                 break;
 485         }
 486         if( k < j )
 487             break;
 488     }
 489
 490     return j == i;
 491 }
 492
 493 int cv::estimateAffine3D(const Mat& from, const Mat& to, Mat& out, vector<uchar>& outliers, double param1, double param2)
 494 {
 495     size_t count = from.cols*from.rows*from.channels()/3;
 496
 497     CV_Assert( count >= 4 && from.isContinuous() && to.isContinuous() &&
 498                from.depth() == CV_32F && to.depth() == CV_32F &&
 499                ((from.rows == 1 && from.channels() == 3) || from.cols*from.channels() == 3) &&
 500                ((to.rows == 1 && to.channels() == 3) || to.cols*to.channels() == 3) &&
 501                count == (size_t)to.cols*to.rows*to.channels()/3);
 502
 503     out.create(3, 4, CV_64F);
 504     outliers.resize(count);
 505     fill(outliers.begin(), outliers.end(), (uchar)1);
 506
 507     vector<Point3d> dFrom;
 508     vector<Point3d> dTo;
 509
 510     copy(from.ptr<Point3f>(), from.ptr<Point3f>() + count, back_inserter(dFrom));
 511     copy(to.ptr<Point3f>(), to.ptr<Point3f>() + count, back_inserter(dTo));
 512
 513     CvMat F3x4 = out;
 514     CvMat mask  = cvMat( 1, count, CV_8U, &outliers[0] );
 515     CvMat m1 = cvMat( 1, count, CV_64FC3, &dFrom[0] );
 516     CvMat m2 = cvMat( 1, count, CV_64FC3, &dTo[0] );
 517
 518     const double epsilon = numeric_limits<double>::epsilon();
 519     param1 = param1 <= 0 ? 3 : param1;
 520     param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2;
 521
 522     return Affine3DEstimator().runRANSAC(&m1,& m2, &F3x4, &mask, param1, param2 );
 523 }