1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
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.
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.
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.
42 #include "precomp.hpp"
43 #include "_modelest.h"
51 CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
53 modelPoints = _modelPoints;
54 modelSize = _modelSize;
55 maxBasicSolutions = _maxBasicSolutions;
56 checkPartialSubsets = true;
60 CvModelEstimator2::~CvModelEstimator2()
64 void CvModelEstimator2::setSeed( int64 seed )
70 int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
71 const CvMat* model, CvMat* _err,
72 CvMat* _mask, double threshold )
74 int i, count = _err->rows*_err->cols, goodCount = 0;
75 const float* err = _err->data.fl;
76 uchar* mask = _mask->data.ptr;
78 computeReprojError( m1, m2, model, _err );
79 threshold *= threshold;
80 for( i = 0; i < count; i++ )
81 goodCount += mask[i] = err[i] <= threshold;
87 cvRANSACUpdateNumIters( double p, double ep,
88 int model_points, int max_iters )
90 if( model_points <= 0 )
91 CV_Error( CV_StsOutOfRange, "the number of model points should be positive" );
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 )
107 return denom >= 0 || -num >= max_iters*(-denom) ?
108 max_iters : cvRound(num/denom);
111 bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
112 CvMat* mask0, double reprojThreshold,
113 double confidence, int maxIters )
116 cv::Ptr<CvMat> mask = cvCloneMat(mask0);
117 cv::Ptr<CvMat> models, err, tmask;
118 cv::Ptr<CvMat> ms1, ms2;
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) );
124 if( count < modelPoints )
127 models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
128 err = cvCreateMat( 1, count, CV_32FC1 );
129 tmask = cvCreateMat( 1, count, CV_8UC1 );
131 if( count > modelPoints )
133 ms1 = cvCreateMat( 1, modelPoints, m1->type );
134 ms2 = cvCreateMat( 1, modelPoints, m2->type );
139 ms1 = cvCloneMat(m1);
140 ms2 = cvCloneMat(m2);
143 for( iter = 0; iter < niters; iter++ )
145 int i, goodCount, nmodels;
146 if( count > modelPoints )
148 bool found = getSubset( m1, m2, ms1, ms2, 300 );
157 nmodels = runKernel( ms1, ms2, models );
160 for( i = 0; i < nmodels; i++ )
163 cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
164 goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
166 if( goodCount > MAX(maxGoodCount, modelPoints-1) )
168 std::swap(tmask, mask);
169 cvCopy( &model_i, model );
170 maxGoodCount = goodCount;
171 niters = cvRANSACUpdateNumIters( confidence,
172 (double)(count - goodCount)/count, modelPoints, niters );
177 if( maxGoodCount > 0 )
180 cvCopy( mask, mask0 );
188 static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
190 bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
191 CvMat* mask, double confidence, int maxIters )
193 const double outlierRatio = 0.45;
195 cv::Ptr<CvMat> models;
196 cv::Ptr<CvMat> ms1, ms2;
199 int iter, niters = maxIters;
200 int count = m1->rows*m1->cols;
201 double minMedian = DBL_MAX, sigma;
203 CV_Assert( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
205 if( count < modelPoints )
208 models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
209 err = cvCreateMat( 1, count, CV_32FC1 );
211 if( count > modelPoints )
213 ms1 = cvCreateMat( 1, modelPoints, m1->type );
214 ms2 = cvCreateMat( 1, modelPoints, m2->type );
219 ms1 = cvCloneMat(m1);
220 ms2 = cvCloneMat(m2);
223 niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
224 niters = MIN( MAX(niters, 3), maxIters );
226 for( iter = 0; iter < niters; iter++ )
229 if( count > modelPoints )
231 bool found = getSubset( m1, m2, ms1, ms2, 300 );
240 nmodels = runKernel( ms1, ms2, models );
243 for( i = 0; i < nmodels; i++ )
246 cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
247 computeReprojError( m1, m2, &model_i, err );
248 icvSortDistances( err->data.i, count, 0 );
250 double median = count % 2 != 0 ?
251 err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
253 if( median < minMedian )
256 cvCopy( &model_i, model );
261 if( minMedian < DBL_MAX )
263 sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian);
264 sigma = MAX( sigma, 0.001 );
266 count = findInliers( m1, m2, model, err, mask, sigma );
267 result = count >= modelPoints;
274 bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
275 CvMat* ms1, CvMat* ms2, int maxAttempts )
277 cv::AutoBuffer<int> _idx(modelPoints);
279 int i = 0, j, k, idx_i, iters = 0;
280 int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
281 const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
282 int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
283 int count = m1->cols*m1->rows;
285 assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
286 elemSize /= sizeof(int);
288 for(; iters < maxAttempts; iters++)
290 for( i = 0; i < modelPoints && iters < maxAttempts; )
292 idx[i] = idx_i = cvRandInt(&rng) % count;
293 for( j = 0; j < i; j++ )
294 if( idx_i == idx[j] )
298 for( k = 0; k < elemSize; k++ )
300 ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
301 ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
303 if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
310 if( !checkPartialSubsets && i == modelPoints &&
311 (!checkSubset( ms1, i ) || !checkSubset( ms2, i )))
316 return i == modelPoints && iters < maxAttempts;
320 bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
326 CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
328 assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
330 if( checkPartialSubsets )
333 i0 = 0, i1 = count - 1;
335 for( i = i0; i <= i1; i++ )
337 // check that the i-th selected point does not belong
338 // to a line connecting some previously selected points
339 for( j = 0; j < i; j++ )
341 double dx1 = ptr[j].x - ptr[i].x;
342 double dy1 = ptr[j].y - ptr[i].y;
343 for( k = 0; k < j; k++ )
345 double dx2 = ptr[k].x - ptr[i].x;
346 double dy2 = ptr[k].y - ptr[i].y;
347 if( fabs(dx2*dy1 - dy2*dx1) <= FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
364 class Affine3DEstimator : public CvModelEstimator2
367 Affine3DEstimator() : CvModelEstimator2(4, cvSize(4, 3), 1) {}
368 virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
370 virtual void computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error );
371 virtual bool checkSubset( const CvMat* ms1, int count );
376 int cv::Affine3DEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
378 const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
379 const Point3d* to = reinterpret_cast<const Point3d*>(m2->data.ptr);
381 Mat A(12, 12, CV_64F);
382 Mat B(12, 1, CV_64F);
385 for(int i = 0; i < modelPoints; ++i)
387 *B.ptr<Point3d>(3*i) = to[i];
389 double *aptr = A.ptr<double>(3*i);
390 for(int k = 0; k < 3; ++k)
393 *reinterpret_cast<Point3d*>(aptr) = from[i];
401 cvReshape(model, &cvX, 1, 12);
402 cvSolve(&cvA, &cvB, &cvX, CV_SVD );
407 void cv::Affine3DEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error )
409 int count = m1->rows * m1->cols;
410 const Point3d* from = reinterpret_cast<const Point3d*>(m1->data.ptr);
411 const Point3d* to = reinterpret_cast<const Point3d*>(m2->data.ptr);
412 const double* F = model->data.db;
413 float* err = error->data.fl;
415 for(int i = 0; i < count; i++ )
417 const Point3d& f = from[i];
418 const Point3d& t = to[i];
420 double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
421 double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
422 double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
424 err[i] = (float)sqrt(a*a + b*b + c*c);
428 bool cv::Affine3DEstimator::checkSubset( const CvMat* ms1, int count )
430 CV_Assert( CV_MAT_TYPE(ms1->type) == CV_64FC3 );
432 int j, k, i = count - 1;
433 const Point3d* ptr = reinterpret_cast<const Point3d*>(ms1->data.ptr);
435 // check that the i-th selected point does not belong
436 // to a line connecting some previously selected points
438 for(j = 0; j < i; ++j)
440 Point3d d1 = ptr[j] - ptr[i];
441 double n1 = norm(d1);
443 for(k = 0; k < j; ++k)
445 Point3d d2 = ptr[k] - ptr[i];
446 double n = norm(d2) * n1;
448 if (fabs(d1.dot(d2) / n) > 0.996)
458 int cv::estimateAffine3D(InputArray _from, InputArray _to,
459 OutputArray _out, OutputArray _inliers,
460 double param1, double param2)
462 Mat from = _from.getMat(), to = _to.getMat();
463 int count = from.checkVector(3);
465 CV_Assert( count >= 0 && to.checkVector(3) == count );
467 _out.create(3, 4, CV_64F);
468 Mat out = _out.getMat();
470 Mat inliers(1, count, CV_8U);
471 inliers = Scalar::all(1);
474 from.convertTo(dFrom, CV_64F);
475 to.convertTo(dTo, CV_64F);
476 dFrom = dFrom.reshape(3, 1);
477 dTo = dTo.reshape(3, 1);
480 CvMat mask = inliers;
484 const double epsilon = numeric_limits<double>::epsilon();
485 param1 = param1 <= 0 ? 3 : param1;
486 param2 = (param2 < epsilon) ? 0.99 : (param2 > 1 - epsilon) ? 0.99 : param2;
488 int ok = Affine3DEstimator().runRANSAC(&m1, &m2, &F3x4, &mask, param1, param2 );
489 if( _inliers.needed() )
490 transpose(inliers, _inliers);