//M*/
#include "_cv.h"
+#include "_cvmodelest.h"
+
+using namespace cv;
+
+template<typename T> int icvCompressPoints( T* ptr, const uchar* mask, int mstep, int count )
+{
+ int i, j;
+ for( i = j = 0; i < count; i++ )
+ if( mask[i*mstep] )
+ {
+ if( i > j )
+ ptr[j] = ptr[i];
+ j++;
+ }
+ return j;
+}
+
+class CvHomographyEstimator : public CvModelEstimator2
+{
+public:
+ CvHomographyEstimator( int modelPoints );
+
+ virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
+ virtual bool refine( const CvMat* m1, const CvMat* m2,
+ CvMat* model, int maxIters );
+protected:
+ virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
+ const CvMat* model, CvMat* error );
+};
+
+
+CvHomographyEstimator::CvHomographyEstimator(int _modelPoints)
+ : CvModelEstimator2(_modelPoints, cvSize(3,3), 1)
+{
+ assert( _modelPoints == 4 || _modelPoints == 5 );
+ checkPartialSubsets = false;
+}
+
+int CvHomographyEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* H )
+{
+ int i, count = m1->rows*m1->cols;
+ const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
+ const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
+
+ double LtL[9][9], W[9][9], V[9][9];
+ CvMat _LtL = cvMat( 9, 9, CV_64F, LtL );
+ 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};
+
+ for( i = 0; i < count; i++ )
+ {
+ cm.x += m[i].x; cm.y += m[i].y;
+ cM.x += M[i].x; cM.y += M[i].y;
+ }
+
+ cm.x /= count; cm.y /= count;
+ cM.x /= count; cM.y /= count;
+
+ for( i = 0; i < count; i++ )
+ {
+ sm.x += fabs(m[i].x - cm.x);
+ sm.y += fabs(m[i].y - cm.y);
+ sM.x += fabs(M[i].x - cM.x);
+ 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;
+
+ double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
+ double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
+ CvMat _invHnorm = cvMat( 3, 3, CV_64FC1, invHnorm );
+ CvMat _Hnorm2 = cvMat( 3, 3, CV_64FC1, Hnorm2 );
+
+ cvZero( &_LtL );
+ for( i = 0; i < count; i++ )
+ {
+ double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
+ double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
+ double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
+ double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
+ int j, k;
+ for( j = 0; j < 9; j++ )
+ for( k = j; k < 9; k++ )
+ LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
+ }
+ cvCompleteSymm( &_LtL );
+
+ //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] );
+
+ return 1;
+}
+
+
+void CvHomographyEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
+ const CvMat* model, CvMat* _err )
+{
+ int i, count = m1->rows*m1->cols;
+ const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
+ const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
+ const double* H = model->data.db;
+ float* err = _err->data.fl;
+
+ for( i = 0; i < count; i++ )
+ {
+ double ww = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
+ double dx = (H[0]*M[i].x + H[1]*M[i].y + H[2])*ww - m[i].x;
+ double dy = (H[3]*M[i].x + H[4]*M[i].y + H[5])*ww - m[i].y;
+ err[i] = (float)(dx*dx + dy*dy);
+ }
+}
+
+bool CvHomographyEstimator::refine( const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters )
+{
+ CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
+ int i, j, k, count = m1->rows*m1->cols;
+ const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
+ const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
+ CvMat modelPart = cvMat( solver.param->rows, solver.param->cols, model->type, model->data.ptr );
+ cvCopy( &modelPart, solver.param );
+
+ for(;;)
+ {
+ const CvMat* _param = 0;
+ CvMat *_JtJ = 0, *_JtErr = 0;
+ double* _errNorm = 0;
+
+ if( !solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ))
+ break;
+
+ for( i = 0; i < count; i++ )
+ {
+ const double* h = _param->data.db;
+ double Mx = M[i].x, My = M[i].y;
+ double ww = 1./(h[6]*Mx + h[7]*My + 1.);
+ double _xi = (h[0]*Mx + h[1]*My + h[2])*ww;
+ double _yi = (h[3]*Mx + h[4]*My + h[5])*ww;
+ double err[] = { _xi - m[i].x, _yi - m[i].y };
+ if( _JtJ || _JtErr )
+ {
+ double J[][8] =
+ {
+ { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
+ { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
+ };
+
+ for( j = 0; j < 8; j++ )
+ {
+ for( k = j; k < 8; k++ )
+ _JtJ->data.db[j*8+k] += J[0][j]*J[0][k] + J[1][j]*J[1][k];
+ _JtErr->data.db[j] += J[0][j]*err[0] + J[1][j]*err[1];
+ }
+ }
+ if( _errNorm )
+ *_errNorm += err[0]*err[0] + err[1]*err[1];
+ }
+ }
+
+ cvCopy( solver.param, &modelPart );
+ return true;
+}
+
+
+CV_IMPL int
+cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
+ CvMat* __H, int method, double ransacReprojThreshold,
+ CvMat* mask )
+{
+ const double confidence = 0.995;
+ const int maxIters = 2000;
+ bool result = false;
+ Ptr<CvMat> m, M, tempMask;
+
+ double H[9];
+ CvMat matH = cvMat( 3, 3, CV_64FC1, H );
+ int count;
+
+ CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
+
+ count = MAX(imagePoints->cols, imagePoints->rows);
+ CV_Assert( count >= 4 );
+
+ m = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( imagePoints, m );
+
+ M = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( objectPoints, M );
+
+ if( mask )
+ {
+ 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 = cvCloneMat(mask);
+ }
+ else if( count > 4 )
+ tempMask = cvCreateMat( 1, count, CV_8U );
+ if( !tempMask.empty() )
+ cvSet( tempMask, cvScalarAll(1.) );
+
+ CvHomographyEstimator estimator( MIN(count, 4) );
+ if( count == 4 )
+ method = 0;
+ if( method == CV_LMEDS )
+ result = estimator.runLMeDS( M, m, &matH, tempMask, confidence, maxIters );
+ else if( method == CV_RANSAC )
+ result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
+ else
+ 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, &matH, 10 );
+ }
+
+ if( result )
+ cvConvert( &matH, __H );
+
+ if( mask && tempMask )
+ cvCopy( tempMask, mask );
+
+ return (int)result;
+}
+
/* Evaluation of Fundamental Matrix from point correspondences.
The original code has been written by Valery Mosyagin */
that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */
/************************************** 7-point algorithm *******************************/
-static int
-icvFMatrix_7Point( const CvPoint2D64f* m0, const CvPoint2D64f* m1, double* fmatrix )
+class CvFMEstimator : public CvModelEstimator2
+{
+public:
+ CvFMEstimator( int _modelPoints );
+
+ virtual int runKernel( const CvMat* m1, const CvMat* m2, CvMat* model );
+ virtual int run7Point( const CvMat* m1, const CvMat* m2, CvMat* model );
+ virtual int run8Point( const CvMat* m1, const CvMat* m2, CvMat* model );
+protected:
+ virtual void computeReprojError( const CvMat* m1, const CvMat* m2,
+ const CvMat* model, CvMat* error );
+};
+
+CvFMEstimator::CvFMEstimator( int _modelPoints )
+: CvModelEstimator2( _modelPoints, cvSize(3,3), _modelPoints == 7 ? 3 : 1 )
+{
+ assert( _modelPoints == 7 || _modelPoints == 8 );
+}
+
+
+int CvFMEstimator::runKernel( const CvMat* m1, const CvMat* m2, CvMat* model )
+{
+ return modelPoints == 7 ? run7Point( m1, m2, model ) : run8Point( m1, m2, model );
+}
+
+int CvFMEstimator::run7Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
{
double a[7*9], w[7], v[9*9], c[4], r[3];
double* f1, *f2;
CvMat W = cvMat( 7, 1, CV_64F, w );
CvMat coeffs = cvMat( 1, 4, CV_64F, c );
CvMat roots = cvMat( 1, 3, CV_64F, r );
+ const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
+ const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
+ double* fmatrix = _fmatrix->data.db;
int i, k, n;
- assert( m0 && m1 && fmatrix );
-
// form a linear system: i-th row of A(=a) represents
- // the equation: (m1[i], 1)'*F*(m0[i], 1) = 0
+ // the equation: (m2[i], 1)'*F*(m1[i], 1) = 0
for( i = 0; i < 7; i++ )
{
- double x0 = m0[i].x, y0 = m0[i].y;
- double x1 = m1[i].x, y1 = m1[i].y;
+ double x0 = m1[i].x, y0 = m1[i].y;
+ double x1 = m2[i].x, y1 = m2[i].y;
a[i*9+0] = x1*x0;
a[i*9+1] = x1*y0;
}
-/*************************************** 8-point algorithm ******************************/
-static int
-icvFMatrix_8Point( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
- const uchar* mask, int count, double* fmatrix )
+int CvFMEstimator::run8Point( const CvMat* _m1, const CvMat* _m2, CvMat* _fmatrix )
{
- int result = 0;
- CvMat* A = 0;
-
- double w[9], v[9*9];
- CvMat W = cvMat( 1, 9, CV_64F, w);
- CvMat V = cvMat( 9, 9, CV_64F, v);
+ double a[9*9], w[9], v[9*9];
+ CvMat W = cvMat( 1, 9, CV_64F, w );
+ CvMat V = cvMat( 9, 9, CV_64F, v );
+ CvMat A = cvMat( 9, 9, CV_64F, a );
CvMat U, F0, TF;
- int i, good_count = 0;
CvPoint2D64f m0c = {0,0}, m1c = {0,0};
double t, scale0 = 0, scale1 = 0;
- double* a;
- int a_step;
-
- CV_FUNCNAME( "icvFMatrix_8Point" );
-
- __BEGIN__;
- assert( m0 && m1 && fmatrix );
+ const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
+ const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
+ double* fmatrix = _fmatrix->data.db;
+ int i, j, k, count = _m1->cols*_m1->rows;
// compute centers and average distances for each of the two point sets
for( i = 0; i < count; i++ )
- if( !mask || mask[i] )
- {
- double x = m0[i].x, y = m0[i].y;
- m0c.x += x; m0c.y += y;
-
- x = m1[i].x, y = m1[i].y;
- m1c.x += x; m1c.y += y;
- good_count++;
- }
+ {
+ double x = m1[i].x, y = m1[i].y;
+ m0c.x += x; m0c.y += y;
- if( good_count < 8 )
- EXIT;
+ x = m2[i].x, y = m2[i].y;
+ m1c.x += x; m1c.y += y;
+ }
// calculate the normalizing transformations for each of the point sets:
// after the transformation each set will have the mass center at the coordinate origin
// and the average distance from the origin will be ~sqrt(2).
- t = 1./good_count;
+ t = 1./count;
m0c.x *= t; m0c.y *= t;
m1c.x *= t; m1c.y *= t;
for( i = 0; i < count; i++ )
- if( !mask || mask[i] )
- {
- double x = m0[i].x - m0c.x, y = m0[i].y - m0c.y;
- scale0 += sqrt(x*x + y*y);
+ {
+ double x = m1[i].x - m0c.x, y = m1[i].y - m0c.y;
+ scale0 += sqrt(x*x + y*y);
- x = fabs(m1[i].x - m1c.x), y = fabs(m1[i].y - m1c.y);
- scale1 += sqrt(x*x + y*y);
- }
+ x = fabs(m2[i].x - m1c.x), y = fabs(m2[i].y - m1c.y);
+ scale1 += sqrt(x*x + y*y);
+ }
scale0 *= t;
scale1 *= t;
if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
- EXIT;
+ return 0;
scale0 = sqrt(2.)/scale0;
scale1 = sqrt(2.)/scale1;
+
+ cvZero( &A );
- CV_CALL( A = cvCreateMat( good_count, 9, CV_64F ));
- a = A->data.db;
- a_step = A->step / sizeof(a[0]);
-
- // form a linear system: for each selected pair of points m0 & m1,
- // the row of A(=a) represents the equation: (m1, 1)'*F*(m0, 1) = 0
+ // form a linear system Ax=0: for each selected pair of points m1 & m2,
+ // the row of A(=a) represents the coefficients of equation: (m2, 1)'*F*(m1, 1) = 0
+ // to save computation time, we compute (At*A) instead of A and then solve (At*A)x=0.
for( i = 0; i < count; i++ )
{
- if( !mask || mask[i] )
- {
- double x0 = (m0[i].x - m0c.x)*scale0;
- double y0 = (m0[i].y - m0c.y)*scale0;
- double x1 = (m1[i].x - m1c.x)*scale1;
- double y1 = (m1[i].y - m1c.y)*scale1;
-
- a[0] = x1*x0;
- a[1] = x1*y0;
- a[2] = x1;
- a[3] = y1*x0;
- a[4] = y1*y0;
- a[5] = y1;
- a[6] = x0;
- a[7] = y0;
- a[8] = 1;
- a += a_step;
- }
+ double x0 = (m1[i].x - m0c.x)*scale0;
+ double y0 = (m1[i].y - m0c.y)*scale0;
+ double x1 = (m2[i].x - m1c.x)*scale1;
+ double y1 = (m2[i].y - m1c.y)*scale1;
+ double r[9] = { x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1 };
+ for( j = 0; j < 9; j++ )
+ for( k = 0; k < 9; k++ )
+ a[j*9+k] += r[j]*r[k];
}
- cvSVD( A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
+ cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
for( i = 0; i < 8; i++ )
{
- if( fabs(w[i]) < FLT_EPSILON )
+ if( fabs(w[i]) < DBL_EPSILON )
break;
}
if( i < 7 )
- EXIT;
+ return 0;
F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0
cvScale( &F0, &F0, 1./F0.data.db[8] );
}
- result = 1;
-
- __END__;
-
- cvReleaseMat( &A );
- return result;
+ return 1;
}
-CV_IMPL int
-cvRANSACUpdateNumIters( double p, double ep,
- int model_points, int max_iters )
+void CvFMEstimator::computeReprojError( const CvMat* _m1, const CvMat* _m2,
+ const CvMat* model, CvMat* _err )
{
- 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);
+ int i, count = _m1->rows*_m1->cols;
+ const CvPoint2D64f* m1 = (const CvPoint2D64f*)_m1->data.ptr;
+ const CvPoint2D64f* m2 = (const CvPoint2D64f*)_m2->data.ptr;
+ const double* F = model->data.db;
+ float* err = _err->data.fl;
- result = denom >= 0 || -num >= max_iters*(-denom) ?
- max_iters : cvRound(num/denom);
-
- __END__;
-
- return result;
-}
-
-
-/************************************ RANSAC algorithm **********************************/
-static int
-icvFMatrix_RANSAC( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
- uchar* mask, int count, double* fmatrix,
- double threshold, double p,
- unsigned rng_seed, int use_8point )
-{
- int result = 0;
-
- const int max_random_iters = 1000;
- const int sample_size = 7;
- uchar* curr_mask = 0;
- uchar* temp_mask = 0;
-
- CV_FUNCNAME( "icvFMatrix_RANSAC" );
-
- __BEGIN__;
-
- double ff[9*3];
- CvRNG rng = cvRNG(rng_seed);
- int i, j, k, sample_count, max_samples = 500;
- int best_good_count = 0;
-
- assert( m0 && m1 && fmatrix && 0 < p && p < 1 && threshold > 0 );
-
- threshold *= threshold;
-
- CV_CALL( curr_mask = (uchar*)cvAlloc( count ));
- if( !mask && use_8point )
- {
- CV_CALL( temp_mask = (uchar*)cvAlloc( count ));
- mask = temp_mask;
- }
-
- // find the best fundamental matrix (giving the least backprojection error)
- // by picking at most <max_samples> 7-tuples of corresponding points
- // <max_samples> may be updated (decreased) within the loop based on statistics of outliers
- for( sample_count = 0; sample_count < max_samples; sample_count++ )
- {
- int idx[sample_size], n;
- CvPoint2D64f ms0[sample_size], ms1[sample_size];
-
- // choose random <sample_size> (=7) points
- for( i = 0; i < sample_size; i++ )
- {
- for( k = 0; k < max_random_iters; k++ )
- {
- idx[i] = cvRandInt(&rng) % count;
- for( j = 0; j < i; j++ )
- if( idx[j] == idx[i] )
- break;
- if( j == i )
- {
- ms0[i] = m0[idx[i]];
- ms1[i] = m1[idx[i]];
- break;
- }
- }
- if( k >= max_random_iters )
- break;
- }
-
- if( i < sample_size )
- continue;
-
- // find 1 or 3 fundamental matrices out of the 7 point correspondences
- n = icvFMatrix_7Point( ms0, ms1, ff );
-
- if( n < 1 || n > 3 )
- continue;
-
- // for each matrix calculate the backprojection error
- // (distance to the corresponding epipolar lines) for each point and thus find
- // the number of in-liers.
- for( k = 0; k < n; k++ )
- {
- const double* f = ff + k*9;
- int good_count = 0;
-
- for( i = 0; i < count; i++ )
- {
- double d0, d1, s0, s1;
-
- double a = f[0]*m0[i].x + f[1]*m0[i].y + f[2];
- double b = f[3]*m0[i].x + f[4]*m0[i].y + f[5];
- double c = f[6]*m0[i].x + f[7]*m0[i].y + f[8];
-
- s1 = a*a + b*b;
- d1 = m1[i].x*a + m1[i].y*b + c;
-
- a = f[0]*m1[i].x + f[3]*m1[i].y + f[6];
- b = f[1]*m1[i].x + f[4]*m1[i].y + f[7];
- c = f[2]*m1[i].x + f[5]*m1[i].y + f[8];
-
- s0 = a*a + b*b;
- d0 = m0[i].x*a + m0[i].y*b + c;
-
- curr_mask[i] = d1*d1 < threshold*s1 && d0*d0 < threshold*s0;
- good_count += curr_mask[i];
- }
-
- if( good_count > MAX( best_good_count, 6 ) )
- {
- // update the current best fundamental matrix and "goodness" flags
- if( mask )
- memcpy( mask, curr_mask, count );
- memcpy( fmatrix, f, 9*sizeof(f[0]));
- best_good_count = good_count;
-
- max_samples = cvRANSACUpdateNumIters( p,
- (double)(count - good_count)/count, 7, max_samples );
- if( max_samples == 0 )
- break;
- }
- }
- }
-
- if( best_good_count < 7 )
- EXIT;
-
- result = 1;
-
- // optionally, use 8-point algorithm to compute fundamental matrix using only the in-liers
- if( best_good_count >= 8 && use_8point )
- result = icvFMatrix_8Point( m0, m1, mask, count, fmatrix );
-
- __END__;
-
- cvFree( &temp_mask );
- cvFree( &curr_mask );
-
- return result;
-}
-
-
-/***************************** Least Median of Squares algorithm ************************/
-
-static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )
-
-/* the algorithm is quite similar to RANSAC, but here we choose the matrix that gives
- the least median of d(m0[i], F'*m1[i])^2 + d(m1[i], F*m0[i])^2 (0<=i<count),
- instead of choosing the matrix that gives the least number of outliers (as it is done in RANSAC) */
-static int
-icvFMatrix_LMedS( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
- uchar* mask, int count, double* fmatrix,
- double threshold, double p,
- unsigned rng_seed, int use_8point )
-{
- int result = 0;
-
- const int max_random_iters = 1000;
- const int sample_size = 7;
-
- float* dist = 0;
- uchar* curr_mask = 0;
- uchar* temp_mask = 0;
-
- CV_FUNCNAME( "icvFMatrix_LMedS" );
-
- __BEGIN__;
-
- double ff[9*3];
- CvRNG rng = cvRNG(rng_seed);
- int i, j, k, sample_count, max_samples = 500;
- double least_median = DBL_MAX, median;
- int best_good_count = 0;
-
- assert( m0 && m1 && fmatrix && 0 < p && p < 1 && threshold > 0 );
-
- threshold *= threshold;
-
- CV_CALL( curr_mask = (uchar*)cvAlloc( count ));
- CV_CALL( dist = (float*)cvAlloc( count*sizeof(dist[0]) ));
-
- if( !mask && use_8point )
- {
- CV_CALL( temp_mask = (uchar*)cvAlloc( count ));
- mask = temp_mask;
- }
-
- // find the best fundamental matrix (giving the least backprojection error)
- // by picking at most <max_samples> 7-tuples of corresponding points
- // <max_samples> may be updated (decreased) within the loop based on statistics of outliers
- for( sample_count = 0; sample_count < max_samples; sample_count++ )
+ for( i = 0; i < count; i++ )
{
- int idx[sample_size], n;
- CvPoint2D64f ms0[sample_size], ms1[sample_size];
-
- // choose random <sample_size> (=7) points
- for( i = 0; i < sample_size; i++ )
- {
- for( k = 0; k < max_random_iters; k++ )
- {
- idx[i] = cvRandInt(&rng) % count;
- for( j = 0; j < i; j++ )
- if( idx[j] == idx[i] )
- break;
- if( j == i )
- {
- ms0[i] = m0[idx[i]];
- ms1[i] = m1[idx[i]];
- break;
- }
- }
- if( k >= max_random_iters )
- break;
- }
-
- if( i < sample_size )
- continue;
+ double a, b, c, d1, d2, s1, s2;
- // find 1 or 3 fundamental matrix out of the 7 point correspondences
- n = icvFMatrix_7Point( ms0, ms1, ff );
+ a = F[0]*m1[i].x + F[1]*m1[i].y + F[2];
+ b = F[3]*m1[i].x + F[4]*m1[i].y + F[5];
+ c = F[6]*m1[i].x + F[7]*m1[i].y + F[8];
- if( n < 1 || n > 3 )
- continue;
+ s2 = 1./(a*a + b*b);
+ d2 = m2[i].x*a + m2[i].y*b + c;
- // for each matrix calculate the backprojection error
- // (distance to the corresponding epipolar lines) for each point and thus find
- // the number of in-liers.
- for( k = 0; k < n; k++ )
- {
- const double* f = ff + k*9;
- int good_count = 0;
-
- for( i = 0; i < count; i++ )
- {
- double d0, d1, s;
-
- double a = f[0]*m0[i].x + f[1]*m0[i].y + f[2];
- double b = f[3]*m0[i].x + f[4]*m0[i].y + f[5];
- double c = f[6]*m0[i].x + f[7]*m0[i].y + f[8];
-
- s = 1./(a*a + b*b);
- d1 = m1[i].x*a + m1[i].y*b + c;
- d1 = s*d1*d1;
-
- a = f[0]*m1[i].x + f[3]*m1[i].y + f[6];
- b = f[1]*m1[i].x + f[4]*m1[i].y + f[7];
- c = f[2]*m1[i].x + f[5]*m1[i].y + f[8];
-
- s = 1./(a*a + b*b);
- d0 = m0[i].x*a + m0[i].y*b + c;
- d0 = s*d0*d0;
-
- curr_mask[i] = d1 < threshold && d0 < threshold;
- good_count += curr_mask[i];
-
- dist[i] = (float)(d0 + d1);
- }
+ a = F[0]*m2[i].x + F[3]*m2[i].y + F[6];
+ b = F[1]*m2[i].x + F[4]*m2[i].y + F[7];
+ c = F[2]*m2[i].x + F[5]*m2[i].y + F[8];
- icvSortDistances( (int*)dist, count, 0 );
- median = (double)dist[count/2];
+ s1 = 1./(a*a + b*b);
+ d1 = m1[i].x*a + m1[i].y*b + c;
- if( median < least_median )
- {
- double ep, lp, lep;
- int new_max_samples;
-
- // update the current best fundamental matrix and "goodness" flags
- if( mask )
- memcpy( mask, curr_mask, count );
- memcpy( fmatrix, f, 9*sizeof(f[0]));
- least_median = median;
- best_good_count = good_count;
-
- // try to update (decrease) <max_samples>
- ep = (double)(count - good_count)/count;
- lp = log(1. - p);
- lep = log(1. - pow(ep,7.));
- if( lp < lep || lep >= 0 )
- break;
- else
- {
- new_max_samples = cvRound(lp/lep);
- max_samples = MIN( new_max_samples, max_samples );
- }
- }
- }
+ err[i] = (float)std::max(d1*d1*s1, d2*d2*s2);
}
-
- if( best_good_count < 7 )
- EXIT;
-
- result = 1;
-
- // optionally, use 8-point algorithm to compute fundamental matrix using only the in-liers
- if( best_good_count >= 8 && use_8point )
- result = icvFMatrix_8Point( m0, m1, mask, count, fmatrix );
-
- __END__;
-
- cvFree( &temp_mask );
- cvFree( &curr_mask );
- cvFree( &dist );
-
- return result;
}
-CV_IMPL int
-cvFindFundamentalMat( const CvMat* points0, const CvMat* points1,
- CvMat* fmatrix, int method,
- double param1, double param2, CvMat* status )
+CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
+ CvMat* fmatrix, int method,
+ double param1, double param2, CvMat* mask )
{
- const unsigned rng_seed = 0xffffffff;
int result = 0;
- int pt_alloc_flag[2] = { 0, 0 };
- int i, k;
- CvPoint2D64f* pt[2] = { 0, 0 };
- CvMat* _status = 0;
-
- CV_FUNCNAME( "cvFindFundamentalMat" );
-
- __BEGIN__;
-
- int count, dims;
- int depth, cn;
- uchar* status_data = 0;
- double fmatrix_data0[9*3];
- double* fmatrix_data = 0;
+ Ptr<CvMat> m1, m2, tempMask;
- if( !CV_IS_MAT(points0) )
- CV_ERROR( !points0 ? CV_StsNullPtr : CV_StsBadArg, "points0 is not a valid matrix" );
+ double F[3*9];
+ CvMat _F3x3 = cvMat( 3, 3, CV_64FC1, F ), _F9x3 = cvMat( 9, 3, CV_64FC1, F );
+ int count;
- if( !CV_IS_MAT(points1) )
- CV_ERROR( !points1 ? CV_StsNullPtr : CV_StsBadArg, "points1 is not a valid matrix" );
+ 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)) );
- if( !CV_ARE_TYPES_EQ(points0, points1) )
- CV_ERROR( CV_StsUnmatchedFormats, "The matrices of points should have the same data type" );
+ count = MAX(points1->cols, points1->rows);
+ if( count < 7 )
+ return 0;
- if( !CV_ARE_SIZES_EQ(points0, points1) )
- CV_ERROR( CV_StsUnmatchedSizes, "The matrices of points should have the same size" );
+ m1 = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( points1, m1 );
- depth = CV_MAT_DEPTH(points0->type);
- cn = CV_MAT_CN(points0->type);
- if( depth != CV_32S && depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
- CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrices is unsupported" );
+ m2 = cvCreateMat( 1, count, CV_64FC2 );
+ cvConvertPointsHomogeneous( points2, m2 );
- if( points0->rows > points0->cols )
+ if( mask )
{
- dims = cn*points0->cols;
- count = points0->rows;
+ 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 = cvCloneMat(mask);
}
- else
+ else if( count > 8 )
+ tempMask = cvCreateMat( 1, count, CV_8U );
+ 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 if( count == 8 || method == CV_FM_8POINT )
+ result = estimator.run8Point(m1, m2, &_F3x3);
+ else if( count > 8 )
{
- if( points0->rows > 1 && cn > 1 || points0->rows == 1 && cn == 1 )
- CV_ERROR( CV_StsBadSize, "The point matrices do not have a proper layout (2xn, 3xn, nx2 or nx3)" );
- dims = cn * points0->rows;
- count = points0->cols;
- }
-
- if( dims != 2 && dims != 3 )
- CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );
-
- if( method == CV_FM_7POINT && count != 7 ||
- method != CV_FM_7POINT && count < 7 + (method == CV_FM_8POINT) )
- CV_ERROR( CV_StsOutOfRange,
- "The number of points must be 7 for 7-point algorithm, "
- ">=8 for 8-point algorithm and >=7 for other algorithms" );
-
- if( !CV_IS_MAT(fmatrix) )
- 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" );
-
- if( fmatrix->cols != 3 || (fmatrix->rows != 3 && (method != CV_FM_7POINT || fmatrix->rows != 9)))
- CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3 or 3x9 (for 7-point method only)" );
-
- fmatrix_data = fmatrix->data.db;
- if( !CV_IS_MAT_CONT(fmatrix->type) || CV_MAT_TYPE(fmatrix->type) != CV_64FC1 ||
- method == CV_FM_7POINT && fmatrix->rows != 9 )
- fmatrix_data = fmatrix_data0;
-
- if( status )
- {
- if( !CV_IS_MAT(status) )
- CV_ERROR( CV_StsBadArg, "The output status is not a valid matrix" );
-
- if( status->cols != 1 && status->rows != 1 || status->cols + status->rows - 1 != count )
- CV_ERROR( CV_StsUnmatchedSizes,
- "The status matrix must have the same size as the point matrices" );
-
- if( method == CV_FM_7POINT || method == CV_FM_8POINT )
- cvSet( status, cvScalarAll(1.) );
- else
- {
- status_data = status->data.ptr;
- if( !CV_IS_MAT_CONT(status->type) || !CV_IS_MASK_ARR(status) )
- {
- CV_CALL( _status = cvCreateMat( status->rows, status->cols, CV_8UC1 ));
- status_data = _status->data.ptr;
- }
- }
- }
-
- for( k = 0; k < 2; k++ )
- {
- const CvMat* spt = k == 0 ? points0 : points1;
- CvPoint2D64f* dpt = pt[k] = (CvPoint2D64f*)spt->data.db;
- int plane_stride, stride, elem_size;
-
- if( CV_IS_MAT_CONT(spt->type) && CV_MAT_DEPTH(spt->type) == CV_64F &&
- dims == 2 && (spt->rows == 1 || spt->rows == count) )
- continue;
-
- elem_size = CV_ELEM_SIZE(depth);
-
- if( spt->rows == dims )
- {
- plane_stride = spt->step / elem_size;
- stride = 1;
- }
- else
- {
- plane_stride = 1;
- stride = spt->rows == 1 ? dims : spt->step / elem_size;
- }
-
- CV_CALL( dpt = pt[k] = (CvPoint2D64f*)cvAlloc( count*sizeof(dpt[0]) ));
- pt_alloc_flag[k] = 1;
-
- if( depth == CV_32F )
- {
- const float* xp = spt->data.fl;
- const float* yp = xp + plane_stride;
- const float* zp = dims == 3 ? yp + plane_stride : 0;
-
- for( i = 0; i < count; i++ )
- {
- double x = *xp, y = *yp;
- xp += stride;
- yp += stride;
- if( dims == 3 )
- {
- double z = *zp;
- zp += stride;
- z = z ? 1./z : 1.;
- x *= z;
- y *= z;
- }
- dpt[i].x = x;
- dpt[i].y = y;
- }
- }
- else
- {
- const double* xp = spt->data.db;
- const double* yp = xp + plane_stride;
- const double* zp = dims == 3 ? yp + plane_stride : 0;
-
- for( i = 0; i < count; i++ )
- {
- double x = *xp, y = *yp;
- xp += stride;
- yp += stride;
- if( dims == 3 )
- {
- double z = *zp;
- zp += stride;
- z = z ? 1./z : 1.;
- x *= z;
- y *= z;
- }
- dpt[i].x = x;
- dpt[i].y = y;
- }
- }
- }
-
- if( method == CV_FM_7POINT )
- result = icvFMatrix_7Point( pt[0], pt[1], fmatrix_data );
- else if( method == CV_FM_8POINT )
- result = icvFMatrix_8Point( pt[0], pt[1], 0, count, fmatrix_data );
- else
- {
- if( param1 < 0 )
- CV_ERROR( CV_StsOutOfRange, "param1 (threshold) must be > 0" );
-
- if( param2 < 0 || param2 > 1 )
- CV_ERROR( CV_StsOutOfRange, "param2 (confidence level) must be between 0 and 1" );
-
+ if( param1 <= 0 )
+ param1 = 3;
if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
param2 = 0.99;
-
- if( method < CV_FM_RANSAC_ONLY )
- result = icvFMatrix_LMedS( pt[0], pt[1], status_data, count, fmatrix_data,
- param1, param2, rng_seed, method & CV_FM_8POINT );
+
+ if( (method & ~3) == CV_RANSAC )
+ result = estimator.runRANSAC(m1, m2, &_F3x3, tempMask, param1, param2 );
else
- result = icvFMatrix_RANSAC( pt[0], pt[1], status_data, count, fmatrix_data,
- param1, param2, rng_seed, method & CV_FM_8POINT );
+ result = estimator.runLMeDS(m1, m2, &_F3x3, tempMask, param2 );
+ if( result <= 0 )
+ 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);*/
}
- if( result && fmatrix->data.db != fmatrix_data )
- {
- CvMat hdr;
- cvZero( fmatrix );
- hdr = cvMat( MIN(fmatrix->rows, result*3), fmatrix->cols, CV_64F, fmatrix_data );
- cvConvert( &hdr, fmatrix );
- }
-
- if( status && status_data && status->data.ptr != status_data )
- cvConvert( _status, status );
-
- __END__;
-
- cvReleaseMat( &_status );
- for( k = 0; k < 2; k++ )
- if( pt_alloc_flag[k] )
- cvFree( &pt[k] );
+ if( result )
+ cvConvert( fmatrix->rows == 3 ? &_F3x3 : &_F9x3, fmatrix );
+
+ 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
-cvConvertPointsHomogenious( const CvMat* src, CvMat* dst )
+CV_IMPL void cvConvertPointsHomogeneous( const CvMat* src, CvMat* dst )
{
- CvMat* temp = 0;
- CvMat* denom = 0;
-
- CV_FUNCNAME( "cvConvertPointsHomogenious" );
-
- __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 );
}
}
}
+}
+
+namespace cv
+{
+
+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);
+}
- __END__;
+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);
+}
- cvReleaseMat( &denom );
- cvReleaseMat( &temp );
+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