//M*/
#include "_cv.h"
+#include "_cvmodelest.h"
+
+using namespace cv;
template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
{
return j;
}
-class CvModelEstimator2
-{
-public:
- CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions);
- virtual ~CvModelEstimator2();
-
- virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )=0;
- virtual bool runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
- CvMat* mask, double confidence=0.99, int maxIters=1000 );
- virtual bool runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
- CvMat* mask, double threshold,
- double confidence=0.99, int maxIters=1000 );
- virtual bool refine( const CvMat*, const CvMat*, CvMat*, int ) { return true; }
- virtual void setSeed( int64 seed );
-
-protected:
- virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
- const CvMat* model, CvMat* error ) = 0;
- virtual int findInliers( const CvMat* m1, const CvMat* m2,
- const CvMat* model, CvMat* error,
- CvMat* mask, double threshold );
- virtual bool getSubset( const CvMat* m1, const CvMat* m2,
- CvMat* ms1, CvMat* ms2, int maxAttempts=1000 );
- virtual bool checkSubset( const CvMat* ms1, int count );
-
- CvRNG rng;
- int modelPoints;
- CvSize modelSize;
- int maxBasicSolutions;
- bool checkPartialSubsets;
-};
-
-
-CvModelEstimator2::CvModelEstimator2(int _modelPoints, CvSize _modelSize, int _maxBasicSolutions)
-{
- modelPoints = _modelPoints;
- modelSize = _modelSize;
- maxBasicSolutions = _maxBasicSolutions;
- checkPartialSubsets = true;
- rng = cvRNG(-1);
-}
-
-CvModelEstimator2::~CvModelEstimator2()
-{
-}
-
-void CvModelEstimator2::setSeed( int64 seed )
-{
- rng = cvRNG(seed);
-}
-
-
-int CvModelEstimator2::findInliers( const CvMat* m1, const CvMat* m2,
- const CvMat* model, CvMat* _err,
- CvMat* _mask, double threshold )
-{
- int i, count = _err->rows*_err->cols, goodCount = 0;
- const float* err = _err->data.fl;
- uchar* mask = _mask->data.ptr;
-
- computeReprojError( m1, m2, model, _err );
- threshold *= threshold;
- for( i = 0; i < count; i++ )
- goodCount += mask[i] = err[i] <= threshold;
- return goodCount;
-}
-
-
-CV_IMPL int
-cvRANSACUpdateNumIters( double p, double ep,
- int model_points, int max_iters )
-{
- int result = 0;
-
- CV_FUNCNAME( "cvRANSACUpdateNumIters" );
-
- __BEGIN__;
-
- double num, denom;
-
- if( model_points <= 0 )
- CV_ERROR( CV_StsOutOfRange, "the number of model points should be positive" );
-
- p = MAX(p, 0.);
- p = MIN(p, 1.);
- ep = MAX(ep, 0.);
- ep = MIN(ep, 1.);
-
- // avoid inf's & nan's
- num = MAX(1. - p, DBL_MIN);
- denom = 1. - pow(1. - ep,model_points);
- if( denom < DBL_MIN )
- EXIT;
-
- num = log(num);
- denom = log(denom);
-
- result = denom >= 0 || -num >= max_iters*(-denom) ?
- max_iters : cvRound(num/denom);
-
- __END__;
-
- return result;
-}
-
-bool CvModelEstimator2::runRANSAC( const CvMat* m1, const CvMat* m2, CvMat* model,
- CvMat* mask, double reprojThreshold,
- double confidence, int maxIters )
-{
- bool result = false;
- CvMat* mask0 = mask, *tmask = 0, *t;
- CvMat* models = 0, *err = 0;
- CvMat *ms1 = 0, *ms2 = 0;
-
- CV_FUNCNAME( "CvModelEstimator2::estimateRansac" );
-
- __BEGIN__;
-
- int iter, niters = maxIters;
- int count = m1->rows*m1->cols, maxGoodCount = 0;
- CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
-
- if( count < modelPoints )
- EXIT;
-
- models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
- err = cvCreateMat( 1, count, CV_32FC1 );
- tmask = cvCreateMat( 1, count, CV_8UC1 );
-
- if( count > modelPoints )
- {
- ms1 = cvCreateMat( 1, modelPoints, m1->type );
- ms2 = cvCreateMat( 1, modelPoints, m2->type );
- }
- else
- {
- niters = 1;
- ms1 = (CvMat*)m1;
- ms2 = (CvMat*)m2;
- }
-
- for( iter = 0; iter < niters; iter++ )
- {
- int i, goodCount, nmodels;
- if( count > modelPoints )
- {
- bool found = getSubset( m1, m2, ms1, ms2, modelPoints );
- if( !found )
- {
- if( iter == 0 )
- EXIT;
- break;
- }
- }
-
- nmodels = runKernel( ms1, ms2, models );
- if( nmodels <= 0 )
- continue;
- for( i = 0; i < nmodels; i++ )
- {
- CvMat model_i;
- cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
- goodCount = findInliers( m1, m2, &model_i, err, tmask, reprojThreshold );
-
- if( goodCount > MAX(maxGoodCount, modelPoints-1) )
- {
- CV_SWAP( tmask, mask, t );
- cvCopy( &model_i, model );
- maxGoodCount = goodCount;
- niters = cvRANSACUpdateNumIters( confidence,
- (double)(count - goodCount)/count, modelPoints, niters );
- }
- }
- }
-
- if( maxGoodCount > 0 )
- {
- if( mask != mask0 )
- {
- CV_SWAP( tmask, mask, t );
- cvCopy( tmask, mask );
- }
- result = true;
- }
-
- __END__;
-
- if( ms1 != m1 )
- cvReleaseMat( &ms1 );
- if( ms2 != m2 )
- cvReleaseMat( &ms2 );
- cvReleaseMat( &models );
- cvReleaseMat( &err );
- cvReleaseMat( &tmask );
- return result;
-}
-
-
-static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
-
-bool CvModelEstimator2::runLMeDS( const CvMat* m1, const CvMat* m2, CvMat* model,
- CvMat* mask, double confidence, int maxIters )
-{
- const double outlierRatio = 0.45;
- bool result = false;
- CvMat* models = 0;
- CvMat *ms1 = 0, *ms2 = 0;
- CvMat* err = 0;
-
- CV_FUNCNAME( "CvModelEstimator2::estimateLMeDS" );
-
- __BEGIN__;
-
- int iter, niters = maxIters;
- int count = m1->rows*m1->cols;
- double minMedian = DBL_MAX, sigma;
-
- CV_ASSERT( CV_ARE_SIZES_EQ(m1, m2) && CV_ARE_SIZES_EQ(m1, mask) );
-
- if( count < modelPoints )
- EXIT;
-
- models = cvCreateMat( modelSize.height*maxBasicSolutions, modelSize.width, CV_64FC1 );
- err = cvCreateMat( 1, count, CV_32FC1 );
-
- if( count > modelPoints )
- {
- ms1 = cvCreateMat( 1, modelPoints, m1->type );
- ms2 = cvCreateMat( 1, modelPoints, m2->type );
- }
- else
- {
- niters = 1;
- ms1 = (CvMat*)m1;
- ms2 = (CvMat*)m2;
- }
-
- niters = cvRound(log(1-confidence)/log(1-pow(1-outlierRatio,(double)modelPoints)));
- niters = MIN( MAX(niters, 3), maxIters );
-
- for( iter = 0; iter < niters; iter++ )
- {
- int i, nmodels;
- if( count > modelPoints )
- {
- bool found = getSubset( m1, m2, ms1, ms2, 300 );
- if( !found )
- {
- if( iter == 0 )
- EXIT;
- break;
- }
- }
-
- nmodels = runKernel( ms1, ms2, models );
- if( nmodels <= 0 )
- continue;
- for( i = 0; i < nmodels; i++ )
- {
- CvMat model_i;
- cvGetRows( models, &model_i, i*modelSize.height, (i+1)*modelSize.height );
- computeReprojError( m1, m2, &model_i, err );
- icvSortDistances( err->data.i, count, 0 );
-
- double median = count % 2 != 0 ?
- err->data.fl[count/2] : (err->data.fl[count/2-1] + err->data.fl[count/2])*0.5;
-
- if( median < minMedian )
- {
- minMedian = median;
- cvCopy( &model_i, model );
- }
- }
- }
-
- if( minMedian < DBL_MAX )
- {
- sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*sqrt(minMedian);
- sigma = MAX( sigma, FLT_EPSILON*100 );
-
- count = findInliers( m1, m2, model, err, mask, sigma );
- result = count >= modelPoints;
- }
-
- __END__;
-
- if( ms1 != m1 )
- cvReleaseMat( &ms1 );
- if( ms2 != m2 )
- cvReleaseMat( &ms2 );
- cvReleaseMat( &models );
- cvReleaseMat( &err );
- return result;
-}
-
-
-bool CvModelEstimator2::getSubset( const CvMat* m1, const CvMat* m2,
- CvMat* ms1, CvMat* ms2, int maxAttempts )
-{
- int* idx = (int*)cvStackAlloc( modelPoints*sizeof(idx[0]) );
- int i, j, k, idx_i, iters = 0;
- int type = CV_MAT_TYPE(m1->type), elemSize = CV_ELEM_SIZE(type);
- const int *m1ptr = m1->data.i, *m2ptr = m2->data.i;
- int *ms1ptr = ms1->data.i, *ms2ptr = ms2->data.i;
- int count = m1->cols*m1->rows;
-
- assert( CV_IS_MAT_CONT(m1->type & m2->type) && (elemSize % sizeof(int) == 0) );
- elemSize /= sizeof(int);
-
- for(;;)
- {
- for( i = 0; i < modelPoints && iters < maxAttempts; iters++ )
- {
- idx[i] = idx_i = cvRandInt(&rng) % count;
- for( j = 0; j < i; j++ )
- if( idx_i == idx[j] )
- break;
- if( j < i )
- continue;
- for( k = 0; k < elemSize; k++ )
- {
- ms1ptr[i*elemSize + k] = m1ptr[idx_i*elemSize + k];
- ms2ptr[i*elemSize + k] = m2ptr[idx_i*elemSize + k];
- }
- if( checkPartialSubsets && (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
- continue;
- i++;
- iters = 0;
- }
- if( !checkPartialSubsets && i == modelPoints &&
- (!checkSubset( ms1, i+1 ) || !checkSubset( ms2, i+1 )))
- continue;
- break;
- }
-
- return i == modelPoints;
-}
-
-
-bool CvModelEstimator2::checkSubset( const CvMat* m, int count )
-{
- int j, k, i = count-1;
- CvPoint2D64f* ptr = (CvPoint2D64f*)m->data.ptr;
-
- assert( CV_MAT_TYPE(m->type) == CV_64FC2 );
-
- // check that the i-th selected point does not belong
- // to a line connecting some previously selected points
- for( j = 0; j < i; j++ )
- {
- double dx1 = ptr[j].x - ptr[i].x;
- double dy1 = ptr[j].y - ptr[i].y;
- for( k = 0; k < j; k++ )
- {
- double dx2 = ptr[k].x - ptr[i].x;
- double dy2 = ptr[k].y - ptr[i].y;
- if( fabs(dx2*dy1 - dy2*dx1) < FLT_EPSILON*(fabs(dx1) + fabs(dy1) + fabs(dx2) + fabs(dy2)))
- break;
- }
- if( k < j )
- break;
- }
-
- return j == i;
-}
-
-
class CvHomographyEstimator : public CvModelEstimator2
{
public:
: CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
{
assert( _modelPoints == 4 || _modelPoints == 5 );
+ checkPartialSubsets = false;
}
int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
double LtL[9][9], W[9][9], V[9][9];
CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
- CvMat _W = cvMat( 9, 9, CV_64F, W );
- CvMat _V = cvMat( 9, 9, CV_64F, V );
+ CvMat matW = cvMat( 9, 9, CV_64F, W );
+ CvMat matV = cvMat( 9, 9, CV_64F, V );
CvMat _H0 = cvMat( 3, 3, CV_64F, V[8] );
CvMat _Htemp = cvMat( 3, 3, CV_64F, V[7] );
CvPoint2D64f cM={0,0}, cm={0,0}, sM={0,0}, sm={0,0};
sM.y += fabs(M[i].y - cM.y);
}
+ if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
+ fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )
+ return 0;
sm.x = count/sm.x; sm.y = count/sm.y;
sM.x = count/sM.x; sM.y = count/sM.y;
}
cvCompleteSymm( &_LtL );
- cvSVD( &_LtL, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
+ //cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
+ cvEigenVV( &_LtL, &matV, &matW );
cvMatMul( &_invHnorm, &_H0, &_Htemp );
cvMatMul( &_Htemp, &_Hnorm2, &_H0 );
cvConvertScale( &_H0, H, 1./_H0.data.db[8] );
CvMat* __H, int method, double ransacReprojThreshold,
CvMat* mask )
{
- const double confidence = 0.99;
+ const double confidence = 0.995;
+ const int maxIters = 2000;
bool result = false;
- CvMat *m = 0, *M = 0, *tempMask = 0;
-
- CV_FUNCNAME( "cvFindHomography" );
-
- __BEGIN__;
+ Ptr<CvMat> m, M, tempMask;
double H[9];
- CvMat _H = cvMat( 3, 3, CV_64FC1, H );
+ CvMat matH = cvMat( 3, 3, CV_64FC1, H );
int count;
- CV_ASSERT( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
+ CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
count = MAX(imagePoints->cols, imagePoints->rows);
- CV_ASSERT( count >= 4 );
+ CV_Assert( count >= 4 );
m = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( imagePoints, m );
if( mask )
{
- CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
+ CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
(mask->rows == 1 || mask->cols == 1) &&
mask->rows*mask->cols == count );
- tempMask = mask;
+ tempMask = cvCloneMat(mask);
}
else if( count > 4 )
tempMask = cvCreateMat( 1, count, CV_8U );
- if( tempMask )
+ if( !tempMask.empty() )
cvSet( tempMask, cvScalarAll(1.) );
- {
- CvHomographyEstimator estimator( MIN(count, 5) );
+ CvHomographyEstimator estimator( MIN(count, 4) );
if( count == 4 )
method = 0;
if( method == CV_LMEDS )
- result = estimator.runLMeDS( M, m, &_H, tempMask, confidence );
+ result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );
else if( method == CV_RANSAC )
- result = estimator.runRANSAC( M, m, &_H, tempMask, ransacReprojThreshold, confidence );
+ result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
else
- result = estimator.runKernel( M, m, &_H ) > 0;
+ result = estimator.runKernel( M, m, &matH ) > 0;
if( result && count > 4 )
{
icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
M->cols = m->cols = count;
- estimator.refine( M, m, &_H, 10 );
- }
+ estimator.refine( M, m, &matH, 10 );
}
if( result )
- cvConvert( &_H, __H );
-
- __END__;
-
- cvReleaseMat( &m );
- cvReleaseMat( &M );
- if( tempMask != mask )
- cvReleaseMat( &tempMask );
+ cvConvert( &matH, __H );
+
+ if( mask && tempMask )
+ cvCopy( tempMask, mask );
return (int)result;
}
s1 = 1./(a*a + b*b);
d1 = m1[i].x*a + m1[i].y*b + c;
- err[i] = (float)(d1*d1*s1 + d2*d2*s2);
+ err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
}
}
-CV_IMPL int
-cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
- CvMat* fmatrix, int method,
- double param1, double param2, CvMat* mask )
+CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
+ CvMat* fmatrix, int method,
+ double param1, double param2, CvMat* mask )
{
int result = 0;
- CvMat *m1 = 0, *m2 = 0, *tempMask = 0;
-
- CV_FUNCNAME( "cvFindFundamentalMat" );
-
- __BEGIN__;
+ Ptr<CvMat> m1, m2, tempMask;
double F[3*9];
CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
int count;
- CV_ASSERT( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
- CV_ASSERT( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
- (fmatrix->rows == 3 || fmatrix->rows == 9 && method == CV_FM_7POINT) );
+ CV_Assert( CV_IS_MAT(points1) && CV_IS_MAT(points2) && CV_ARE_SIZES_EQ(points1, points2) );
+ CV_Assert( CV_IS_MAT(fmatrix) && fmatrix->cols == 3 &&
+ (fmatrix->rows == 3 || (fmatrix->rows == 9 && method == CV_FM_7POINT)) );
count = MAX(points1->cols, points1->rows);
if( count < 7 )
- EXIT;
+ return 0;
m1 = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( points1, m1 );
if( mask )
{
- CV_ASSERT( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
+ CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
(mask->rows == 1 || mask->cols == 1) &&
mask->rows*mask->cols == count );
- tempMask = mask;
+ tempMask = cvCloneMat(mask);
}
else if( count > 8 )
tempMask = cvCreateMat( 1, count, CV_8U );
- if( tempMask )
+ if( !tempMask.empty() )
cvSet( tempMask, cvScalarAll(1.) );
- {
CvFMEstimator estimator( MIN(count, (method & 3) == CV_FM_7POINT ? 7 : 8) );
if( count == 7 )
result = estimator.run7Point(m1, m2, &_F9x3);
else
result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
if( result <= 0 )
- EXIT;
- icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
+ return 0;
+ /*icvCompressPoints( (CvPoint2D64f*)m1->data.ptr, tempMask->data.ptr, 1, count );
count = icvCompressPoints( (CvPoint2D64f*)m2->data.ptr, tempMask->data.ptr, 1, count );
assert( count >= 8 );
m1->cols = m2->cols = count;
- estimator.run8Point(m1, m2, &_F3x3);
- }
+ estimator.run8Point(m1, m2, &_F3x3);*/
}
if( result )
cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
-
- __END__;
-
- cvReleaseMat( &m1 );
- cvReleaseMat( &m2 );
- if( tempMask != mask )
- cvReleaseMat( &tempMask );
+
+ if( mask && tempMask )
+ cvCopy( tempMask, mask );
return result;
}
-CV_IMPL void
-cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
- const CvMat* fmatrix, CvMat* lines )
+CV_IMPL void cvComputeCorrespondEpilines( const CvMat* points, int pointImageID,
+ const CvMat* fmatrix, CvMat* lines )
{
- CV_FUNCNAME( "cvComputeCorrespondEpilines" );
-
- __BEGIN__;
-
int abc_stride, abc_plane_stride, abc_elem_size;
int plane_stride, stride, elem_size;
int i, dims, count, depth, cn, abc_dims, abc_count, abc_depth, abc_cn;
CvMat F = cvMat( 3, 3, CV_64F, f );
if( !CV_IS_MAT(points) )
- CV_ERROR( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
+ CV_Error( !points ? CV_StsNullPtr : CV_StsBadArg, "points parameter is not a valid matrix" );
depth = CV_MAT_DEPTH(points->type);
cn = CV_MAT_CN(points->type);
- if( depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
- CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
+ if( (depth != CV_32F && depth != CV_64F) || (cn != 1 && cn != 2 && cn != 3) )
+ CV_Error( CV_StsUnsupportedFormat, "The format of point matrix is unsupported" );
if( points->rows > points->cols )
{
}
else
{
- if( points->rows > 1 && cn > 1 || points->rows == 1 && cn == 1 )
- CV_ERROR( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
+ if( (points->rows > 1 && cn > 1) || (points->rows == 1 && cn == 1) )
+ CV_Error( CV_StsBadSize, "The point matrix does not have a proper layout (2xn, 3xn, nx2 or nx3)" );
dims = cn * points->rows;
count = points->cols;
}
if( dims != 2 && dims != 3 )
- CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
+ CV_Error( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
if( !CV_IS_MAT(fmatrix) )
- CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
+ CV_Error( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );
if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
- CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
+ CV_Error( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );
if( fmatrix->cols != 3 || fmatrix->rows != 3 )
- CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3" );
+ CV_Error( CV_StsBadSize, "fundamental matrix must be 3x3" );
if( !CV_IS_MAT(lines) )
- CV_ERROR( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
+ CV_Error( !lines ? CV_StsNullPtr : CV_StsBadArg, "lines parameter is not a valid matrix" );
abc_depth = CV_MAT_DEPTH(lines->type);
abc_cn = CV_MAT_CN(lines->type);
- if( abc_depth != CV_32F && abc_depth != CV_64F || abc_cn != 1 && abc_cn != 3 )
- CV_ERROR( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
+ if( (abc_depth != CV_32F && abc_depth != CV_64F) || (abc_cn != 1 && abc_cn != 3) )
+ CV_Error( CV_StsUnsupportedFormat, "The format of the matrix of lines is unsupported" );
if( lines->rows > lines->cols )
{
}
else
{
- if( lines->rows > 1 && abc_cn > 1 || lines->rows == 1 && abc_cn == 1 )
- CV_ERROR( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
+ if( (lines->rows > 1 && abc_cn > 1) || (lines->rows == 1 && abc_cn == 1) )
+ CV_Error( CV_StsBadSize, "The lines matrix does not have a proper layout (3xn or nx3)" );
abc_dims = abc_cn * lines->rows;
abc_count = lines->cols;
}
if( abc_dims != 3 )
- CV_ERROR( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
+ CV_Error( CV_StsOutOfRange, "The lines matrix does not have a proper layout (3xn or nx3)" );
if( abc_count != count )
- CV_ERROR( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
+ CV_Error( CV_StsUnmatchedSizes, "The numbers of points and lines are different" );
elem_size = CV_ELEM_SIZE(depth);
abc_elem_size = CV_ELEM_SIZE(abc_depth);
abc_stride = lines->rows == 1 ? 3*abc_elem_size : lines->step;
}
- CV_CALL( cvConvert( fmatrix, &F ));
+ cvConvert( fmatrix, &F );
if( pointImageID == 2 )
cvTranspose( &F, &F );
bp += abc_stride;
cp += abc_stride;
}
-
- __END__;
}
-CV_IMPL void
-cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
+CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
{
- CvMat* temp = 0;
- CvMat* denom = 0;
-
- CV_FUNCNAME( "cvConvertPointsHomogeneous" );
-
- __BEGIN__;
+ Ptr<CvMat> temp, denom;
int i, s_count, s_dims, d_count, d_dims;
CvMat _src, _dst, _ones;
CvMat* ones = 0;
if( !CV_IS_MAT(src) )
- CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
+ CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg,
"The input parameter is not a valid matrix" );
if( !CV_IS_MAT(dst) )
- CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
+ CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
"The output parameter is not a valid matrix" );
if( src == dst || src->data.ptr == dst->data.ptr )
{
if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) || !CV_ARE_SIZES_EQ(src,dst)) )
- CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
- EXIT;
+ CV_Error( CV_StsBadArg, "Invalid inplace operation" );
+ return;
}
if( src->rows > src->cols )
{
if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
- CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
+ CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->cols;
s_count = src->rows;
else
{
if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
- CV_ERROR( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
+ CV_Error( CV_StsBadSize, "Either the number of channels or columns or rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->rows;
s_count = src->cols;
if( dst->rows > dst->cols )
{
if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
- CV_ERROR( CV_StsBadSize,
+ CV_Error( CV_StsBadSize,
"Either the number of channels or columns or rows in the input matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->cols;
else
{
if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
- CV_ERROR( CV_StsBadSize,
+ CV_Error( CV_StsBadSize,
"Either the number of channels or columns or rows in the output matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->rows;
dst = cvReshape( dst, &_dst, 1, d_count );
if( s_count != d_count )
- CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
+ CV_Error( CV_StsUnmatchedSizes, "Both matrices must have the same number of points" );
if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
- CV_ERROR( CV_StsUnsupportedFormat,
+ CV_Error( CV_StsUnsupportedFormat,
"Both matrices must be floating-point (single or double precision)" );
if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
- CV_ERROR( CV_StsOutOfRange,
+ CV_Error( CV_StsOutOfRange,
"Both input and output point dimensionality must be 2, 3 or 4" );
if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
- CV_ERROR( CV_StsUnmatchedSizes,
+ CV_Error( CV_StsUnmatchedSizes,
"The dimensionalities of input and output point sets differ too much" );
if( s_dims == d_dims - 1 )
{
if( !CV_ARE_TYPES_EQ( src, dst ))
{
- CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
+ temp = cvCreateMat( src->rows, src->cols, dst->type );
cvConvert( src, temp );
src = temp;
}
if( !CV_ARE_TYPES_EQ( src, dst ))
{
- CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
+ temp = cvCreateMat( src->rows, src->cols, dst->type );
cvConvert( src, temp );
src = temp;
}
else
d_stride = dst->step / elem_size, d_plane_stride = 1;
- CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
+ denom = cvCreateMat( 1, d_count, dst->type );
if( CV_MAT_DEPTH(dst->type) == CV_32F )
{
for( i = 0; i < d_count; i++, ws += s_stride )
{
float t = *ws;
- iw[i] = t ? t : 1.f;
+ iw[i] = fabs((double)t) > FLT_EPSILON ? t : 1.f;
}
cvDiv( 0, denom, denom );
for( i = 0; i < d_count; i++, ws += s_stride )
{
double t = *ws;
- iw[i] = t ? t : 1.;
+ iw[i] = fabs(t) > DBL_EPSILON ? t : 1.;
}
cvDiv( 0, denom, denom );
}
}
}
+}
- __END__;
+namespace cv
+{
- cvReleaseMat( &denom );
- cvReleaseMat( &temp );
+static Mat _findHomography( const Mat& points1, const Mat& points2,
+ int method, double ransacReprojThreshold,
+ vector<uchar>* mask )
+{
+ CV_Assert(points1.isContinuous() && points2.isContinuous() &&
+ points1.type() == points2.type() &&
+ ((points1.rows == 1 && points1.channels() == 2) ||
+ points1.cols*points1.channels() == 2) &&
+ ((points2.rows == 1 && points2.channels() == 2) ||
+ points2.cols*points2.channels() == 2));
+
+ Mat H(3, 3, CV_64F);
+ CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
+ CvMat matH = H, _mask, *pmask = 0;
+ if( mask )
+ {
+ mask->resize(points1.cols*points1.rows*points1.channels()/2);
+ pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
+ }
+ bool ok = cvFindHomography( &_pt1, &_pt2, &matH, method, ransacReprojThreshold, pmask ) > 0;
+ if( !ok )
+ H = Scalar(0);
+ return H;
+}
+
+static Mat _findFundamentalMat( const Mat& points1, const Mat& points2,
+ int method, double param1, double param2,
+ vector<uchar>* mask )
+{
+ CV_Assert(points1.isContinuous() && points2.isContinuous() &&
+ points1.type() == points2.type() &&
+ ((points1.rows == 1 && points1.channels() == 2) ||
+ points1.cols*points1.channels() == 2) &&
+ ((points2.rows == 1 && points2.channels() == 2) ||
+ points2.cols*points2.channels() == 2));
+
+ Mat F(3, 3, CV_64F);
+ CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
+ CvMat matF = F, _mask, *pmask = 0;
+ if( mask )
+ {
+ mask->resize(points1.cols*points1.rows*points1.channels()/2);
+ pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
+ }
+ int n = cvFindFundamentalMat( &_pt1, &_pt2, &matF, method, param1, param2, pmask );
+ if( n <= 0 )
+ F = Scalar(0);
+ return F;
+}
+
+}
+
+
+cv::Mat cv::findHomography( const Mat& srcPoints, const Mat& dstPoints,
+ vector<uchar>& mask, int method,
+ double ransacReprojThreshold )
+{
+ return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, &mask);
+}
+
+cv::Mat cv::findHomography( const Mat& srcPoints, const Mat& dstPoints,
+ int method, double ransacReprojThreshold )
+{
+ return _findHomography(srcPoints, dstPoints, method, ransacReprojThreshold, 0);
+}
+
+
+cv::Mat cv::findFundamentalMat( const Mat& points1, const Mat& points2,
+ vector<uchar>& mask, int method, double param1, double param2 )
+{
+ return _findFundamentalMat( points1, points2, method, param1, param2, &mask );
+}
+
+cv::Mat cv::findFundamentalMat( const Mat& points1, const Mat& points2,
+ int method, double param1, double param2 )
+{
+ return _findFundamentalMat( points1, points2, method, param1, param2, 0 );
+}
+
+void cv::computeCorrespondEpilines( const Mat& points, int whichImage,
+ const Mat& F, vector<Vec3f>& lines )
+{
+ CV_Assert(points.isContinuous() &&
+ (points.depth() == CV_32S || points.depth() == CV_32F) &&
+ ((points.rows == 1 && points.channels() == 2) ||
+ points.cols*points.channels() == 2));
+
+ lines.resize(points.cols*points.rows*points.channels()/2);
+ CvMat _points = points, _lines = Mat(lines), matF = F;
+ cvComputeCorrespondEpilines(&_points, whichImage, &matF, &_lines);
+}
+
+void cv::convertPointsHomogeneous( const Mat& src, vector<Point3f>& dst )
+{
+ CV_Assert(src.isContinuous() &&
+ (src.depth() == CV_32S || src.depth() == CV_32F) &&
+ ((src.rows == 1 && src.channels() == 2) ||
+ src.cols*src.channels() == 2));
+
+ dst.resize(src.cols*src.rows*src.channels()/2);
+ CvMat _src = src, _dst = Mat(dst);
+ cvConvertPointsHomogeneous(&_src, &_dst);
+}
+
+void cv::convertPointsHomogeneous( const Mat& src, vector<Point2f>& dst )
+{
+ CV_Assert(src.isContinuous() &&
+ (src.depth() == CV_32S || src.depth() == CV_32F) &&
+ ((src.rows == 1 && src.channels() == 3) ||
+ src.cols*src.channels() == 3));
+
+ dst.resize(src.cols*src.rows*src.channels()/3);
+ CvMat _src = Mat(src), _dst = Mat(dst);
+ cvConvertPointsHomogeneous(&_src, &_dst);
}
/* End of file. */
projects / opencv.git / blobdiff