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