From: Vadim Pisarevsky Date: Sun, 20 Jan 2013 21:07:47 +0000 (+0400) Subject: converted fitline to C++, fixed Mat::checkVector(). X-Git-Tag: submit/tizen_ivi/20141117.190038~2^2~1199^2~2 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=c2241dccc5d969717ec539c30bb9957b66adc15f;p=profile%2Fivi%2Fopencv.git converted fitline to C++, fixed Mat::checkVector(). --- diff --git a/modules/core/src/matrix.cpp b/modules/core/src/matrix.cpp index 4be71d4..ebee0de 100644 --- a/modules/core/src/matrix.cpp +++ b/modules/core/src/matrix.cpp @@ -841,7 +841,7 @@ int Mat::checkVector(int _elemChannels, int _depth, bool _requireContinuous) con { return (depth() == _depth || _depth <= 0) && (isContinuous() || !_requireContinuous) && - ((dims == 2 && (((rows == 1 || cols == 1) && channels() == _elemChannels) || (cols == _elemChannels))) || + ((dims == 2 && (((rows == 1 || cols == 1) && channels() == _elemChannels) || (cols == _elemChannels && channels() == 1))) || (dims == 3 && channels() == 1 && size.p[2] == _elemChannels && (size.p[0] == 1 || size.p[1] == 1) && (isContinuous() || step.p[1] == step.p[2]*size.p[2]))) ? (int)(total()*channels()/_elemChannels) : -1; diff --git a/modules/imgproc/src/contours.cpp b/modules/imgproc/src/contours.cpp index 9078d70..8c5dcb5 100644 --- a/modules/imgproc/src/contours.cpp +++ b/modules/imgproc/src/contours.cpp @@ -1824,28 +1824,6 @@ cv::RotatedRect cv::fitEllipse( InputArray _points ) } -void cv::fitLine( InputArray _points, OutputArray _line, int distType, - double param, double reps, double aeps ) -{ - Mat points = _points.getMat(); - - bool is3d = points.checkVector(3) >= 0; - bool is2d = points.checkVector(2) >= 0; - - CV_Assert( (is2d || is3d) && (points.depth() == CV_32F || points.depth() == CV_32S) ); - CvMat _cpoints = points.reshape(2 + (int)is3d); - float line[6]; - cvFitLine(&_cpoints, distType, param, reps, aeps, &line[0]); - - int out_size = (is2d)?( (is3d)? (points.channels() * points.rows * 2) : 4 ): 6; - - _line.create(out_size, 1, CV_32F, -1, true); - Mat l = _line.getMat(); - CV_Assert( l.isContinuous() ); - memcpy( l.data, line, out_size * sizeof(line[0]) ); -} - - double cv::pointPolygonTest( InputArray _contour, Point2f pt, bool measureDist ) { diff --git a/modules/imgproc/src/convhull.cpp b/modules/imgproc/src/convhull.cpp index 7b2c3d0..8b4a855 100644 --- a/modules/imgproc/src/convhull.cpp +++ b/modules/imgproc/src/convhull.cpp @@ -584,7 +584,7 @@ void convexHull( InputArray _points, OutputArray _hull, bool clockwise, bool ret void convexityDefects( InputArray _points, InputArray _hull, OutputArray _defects ) { Mat points = _points.getMat(); - int i, j = 0, index, npoints = points.checkVector(2, CV_32S); + int i, j = 0, npoints = points.checkVector(2, CV_32S); CV_Assert( npoints >= 0 ); if( npoints <= 3 ) diff --git a/modules/imgproc/src/linefit.cpp b/modules/imgproc/src/linefit.cpp index c513565..57034d7 100644 --- a/modules/imgproc/src/linefit.cpp +++ b/modules/imgproc/src/linefit.cpp @@ -40,19 +40,19 @@ //M*/ #include "precomp.hpp" +namespace cv +{ + static const double eps = 1e-6; -static CvStatus -icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line ) +static void fitLine2D_wods( const Point2f* points, int count, float *weights, float *line ) { double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0; double dx2, dy2, dxy; int i; - int count = _count; float t; - /* Calculating the average of x and y... */ - + // Calculating the average of x and y... if( weights == 0 ) { for( i = 0; i < count; i += 1 ) @@ -94,12 +94,9 @@ icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *lin line[2] = (float) x; line[3] = (float) y; - - return CV_NO_ERR; } -static CvStatus -icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line ) +static void fitLine3D_wods( const Point3f * points, int count, float *weights, float *line ) { int i; float w0 = 0; @@ -184,25 +181,13 @@ icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line det[7] = det[5]; det[8] = dy2 + dx2; - /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */ -#if 1 - { - CvMat _det = cvMat( 3, 3, CV_32F, det ); - CvMat _evc = cvMat( 3, 3, CV_32F, evc ); - CvMat _evl = cvMat( 3, 1, CV_32F, evl ); - cvEigenVV( &_det, &_evc, &_evl, 0 ); + // Searching for a eigenvector of det corresponding to the minimal eigenvalue + Mat _det( 3, 3, CV_32F, det ); + Mat _evc( 3, 3, CV_32F, evc ); + Mat _evl( 3, 1, CV_32F, evl ); + eigen( _det, _evl, _evc ); i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2); - } -#else - { - CvMat _det = cvMat( 3, 3, CV_32F, det ); - CvMat _evc = cvMat( 3, 3, CV_32F, evc ); - CvMat _evl = cvMat( 1, 3, CV_32F, evl ); - cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T ); - } - i = 2; -#endif v = &evc[i * 3]; n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] ); n = (float)MAX(n, eps); @@ -212,12 +197,9 @@ icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line line[3] = x0; line[4] = y0; line[5] = z0; - - return CV_NO_ERR; } -static double -icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist ) +static double calcDist2D( const Point2f* points, int count, float *_line, float *dist ) { int j; float px = _line[2], py = _line[3]; @@ -238,8 +220,7 @@ icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist ) return sum_dist; } -static double -icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist ) +static double calcDist3D( const Point3f* points, int count, float *_line, float *dist ) { int j; float px = _line[3], py = _line[4], pz = _line[5]; @@ -266,8 +247,7 @@ icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist ) return sum_dist; } -static void -icvWeightL1( float *d, int count, float *w ) +static void weightL1( float *d, int count, float *w ) { int i; @@ -278,8 +258,7 @@ icvWeightL1( float *d, int count, float *w ) } } -static void -icvWeightL12( float *d, int count, float *w ) +static void weightL12( float *d, int count, float *w ) { int i; @@ -290,8 +269,7 @@ icvWeightL12( float *d, int count, float *w ) } -static void -icvWeightHuber( float *d, int count, float *w, float _c ) +static void weightHuber( float *d, int count, float *w, float _c ) { int i; const float c = _c <= 0 ? 1.345f : _c; @@ -306,8 +284,7 @@ icvWeightHuber( float *d, int count, float *w, float _c ) } -static void -icvWeightFair( float *d, int count, float *w, float _c ) +static void weightFair( float *d, int count, float *w, float _c ) { int i; const float c = _c == 0 ? 1 / 1.3998f : 1 / _c; @@ -318,76 +295,72 @@ icvWeightFair( float *d, int count, float *w, float _c ) } } -static void -icvWeightWelsch( float *d, int count, float *w, float _c ) +static void weightWelsch( float *d, int count, float *w, float _c ) { int i; const float c = _c == 0 ? 1 / 2.9846f : 1 / _c; for( i = 0; i < count; i++ ) { - w[i] = (float) exp( -d[i] * d[i] * c * c ); + w[i] = (float) std::exp( -d[i] * d[i] * c * c ); } } /* Takes an array of 2D points, type of distance (including user-defined -distance specified by callbacks, fills the array of four floats with line -parameters A, B, C, D, where (A, B) is the normalized direction vector, -(C, D) is the point that belongs to the line. */ + distance specified by callbacks, fills the array of four floats with line + parameters A, B, C, D, where (A, B) is the normalized direction vector, + (C, D) is the point that belongs to the line. */ -static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, - float _param, float reps, float aeps, float *line ) +static void fitLine2D( const Point2f * points, int count, int dist, + float _param, float reps, float aeps, float *line ) { double EPS = count*FLT_EPSILON; void (*calc_weights) (float *, int, float *) = 0; void (*calc_weights_param) (float *, int, float *, float) = 0; - float *w; /* weights */ - float *r; /* square distances */ int i, j, k; float _line[6], _lineprev[6]; float rdelta = reps != 0 ? reps : 1.0f; float adelta = aeps != 0 ? aeps : 0.01f; double min_err = DBL_MAX, err = 0; - CvRNG rng = cvRNG(-1); + RNG rng(-1); memset( line, 0, 4*sizeof(line[0]) ); switch (dist) { case CV_DIST_L2: - return icvFitLine2D_wods( points, count, 0, line ); + return fitLine2D_wods( points, count, 0, line ); case CV_DIST_L1: - calc_weights = icvWeightL1; + calc_weights = weightL1; break; case CV_DIST_L12: - calc_weights = icvWeightL12; + calc_weights = weightL12; break; case CV_DIST_FAIR: - calc_weights_param = icvWeightFair; + calc_weights_param = weightFair; break; case CV_DIST_WELSCH: - calc_weights_param = icvWeightWelsch; + calc_weights_param = weightWelsch; break; case CV_DIST_HUBER: - calc_weights_param = icvWeightHuber; + calc_weights_param = weightHuber; break; - /*case CV_DIST_USER: - calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; - break;*/ - + /*case DIST_USER: + calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; + break;*/ default: - return CV_BADFACTOR_ERR; + CV_Error(CV_StsBadArg, "Unknown distance type"); } - w = (float *) cvAlloc( count * sizeof( float )); - r = (float *) cvAlloc( count * sizeof( float )); + AutoBuffer wr(count*2); + float *w = wr, *r = w + count; for( k = 0; k < 20; k++ ) { @@ -397,7 +370,7 @@ static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, for( i = 0; i < MIN(count,10); ) { - j = cvRandInt(&rng) % count; + j = rng.uniform(0, count); if( w[j] < FLT_EPSILON ) { w[j] = 1.f; @@ -405,7 +378,7 @@ static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, } } - icvFitLine2D_wods( points, count, w, _line ); + fitLine2D_wods( points, count, w, _line ); for( i = 0; i < 30; i++ ) { double sum_w = 0; @@ -432,7 +405,7 @@ static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, } } /* calculate distances */ - err = icvCalcDist2D( points, count, _line, r ); + err = calcDist2D( points, count, _line, r ); if( err < EPS ) break; @@ -461,7 +434,7 @@ static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, memcpy( _lineprev, _line, 4 * sizeof( float )); /* Run again... */ - icvFitLine2D_wods( points, count, w, _line ); + fitLine2D_wods( points, count, w, _line ); } if( err < min_err ) @@ -472,70 +445,57 @@ static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, break; } } - - cvFree( &w ); - cvFree( &r ); - return CV_OK; } /* Takes an array of 3D points, type of distance (including user-defined -distance specified by callbacks, fills the array of four floats with line -parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector, -(D, E, F) is the point that belongs to the line. */ - -static CvStatus -icvFitLine3D( CvPoint3D32f * points, int count, int dist, - float _param, float reps, float aeps, float *line ) + distance specified by callbacks, fills the array of four floats with line + parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector, + (D, E, F) is the point that belongs to the line. */ +static void fitLine3D( Point3f * points, int count, int dist, + float _param, float reps, float aeps, float *line ) { double EPS = count*FLT_EPSILON; void (*calc_weights) (float *, int, float *) = 0; void (*calc_weights_param) (float *, int, float *, float) = 0; - float *w; /* weights */ - float *r; /* square distances */ int i, j, k; float _line[6]={0,0,0,0,0,0}, _lineprev[6]={0,0,0,0,0,0}; float rdelta = reps != 0 ? reps : 1.0f; float adelta = aeps != 0 ? aeps : 0.01f; double min_err = DBL_MAX, err = 0; - CvRNG rng = cvRNG(-1); + RNG rng(-1); switch (dist) { case CV_DIST_L2: - return icvFitLine3D_wods( points, count, 0, line ); + return fitLine3D_wods( points, count, 0, line ); case CV_DIST_L1: - calc_weights = icvWeightL1; + calc_weights = weightL1; break; case CV_DIST_L12: - calc_weights = icvWeightL12; + calc_weights = weightL12; break; case CV_DIST_FAIR: - calc_weights_param = icvWeightFair; + calc_weights_param = weightFair; break; case CV_DIST_WELSCH: - calc_weights_param = icvWeightWelsch; + calc_weights_param = weightWelsch; break; case CV_DIST_HUBER: - calc_weights_param = icvWeightHuber; + calc_weights_param = weightHuber; break; - /*case CV_DIST_USER: - _PFP.p = param; - calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; - break;*/ - default: - return CV_BADFACTOR_ERR; + CV_Error(CV_StsBadArg, "Unknown distance"); } - w = (float *) cvAlloc( count * sizeof( float )); - r = (float *) cvAlloc( count * sizeof( float )); + AutoBuffer buf(count*2); + float *w = buf, *r = w + count; for( k = 0; k < 20; k++ ) { @@ -545,7 +505,7 @@ icvFitLine3D( CvPoint3D32f * points, int count, int dist, for( i = 0; i < MIN(count,10); ) { - j = cvRandInt(&rng) % count; + j = rng.uniform(0, count); if( w[j] < FLT_EPSILON ) { w[j] = 1.f; @@ -553,7 +513,7 @@ icvFitLine3D( CvPoint3D32f * points, int count, int dist, } } - icvFitLine3D_wods( points, count, w, _line ); + fitLine3D_wods( points, count, w, _line ); for( i = 0; i < 30; i++ ) { double sum_w = 0; @@ -587,8 +547,9 @@ icvFitLine3D( CvPoint3D32f * points, int count, int dist, } } /* calculate distances */ - if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count ) - break; + err = calcDist3D( points, count, _line, r ); + //if( err < FLT_EPSILON*count ) + // break; /* calculate weights */ if( calc_weights ) @@ -610,14 +571,14 @@ icvFitLine3D( CvPoint3D32f * points, int count, int dist, for( j = 0; j < count; j++ ) w[j] = 1.f; } - + /* save the line parameters */ memcpy( _lineprev, _line, 6 * sizeof( float )); - + /* Run again... */ - icvFitLine3D_wods( points, count, w, _line ); + fitLine3D_wods( points, count, w, _line ); } - + if( err < min_err ) { min_err = err; @@ -626,94 +587,50 @@ icvFitLine3D( CvPoint3D32f * points, int count, int dist, break; } } - - // Return... - cvFree( &w ); - cvFree( &r ); - return CV_OK; } +} -CV_IMPL void -cvFitLine( const CvArr* array, int dist, double param, - double reps, double aeps, float *line ) +void cv::fitLine( InputArray _points, OutputArray _line, int distType, + double param, double reps, double aeps ) { - cv::AutoBuffer buffer; - - schar* points = 0; - union { CvContour contour; CvSeq seq; } header; - CvSeqBlock block; - CvSeq* ptseq = (CvSeq*)array; - int type; - - if( !line ) - CV_Error( CV_StsNullPtr, "NULL pointer to line parameters" ); - - if( CV_IS_SEQ(ptseq) ) - { - type = CV_SEQ_ELTYPE(ptseq); - if( ptseq->total == 0 ) - CV_Error( CV_StsBadSize, "The sequence has no points" ); - if( (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) || - CV_ELEM_SIZE(type) != ptseq->elem_size ) - CV_Error( CV_StsUnsupportedFormat, - "Input sequence must consist of 2d points or 3d points" ); - } - else - { - CvMat* mat = (CvMat*)array; - type = CV_MAT_TYPE(mat->type); - if( !CV_IS_MAT(mat)) - CV_Error( CV_StsBadArg, "Input array is not a sequence nor matrix" ); - - if( !CV_IS_MAT_CONT(mat->type) || - (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) || - (mat->width != 1 && mat->height != 1)) - CV_Error( CV_StsBadArg, - "Input array must be 1d continuous array of 2d or 3d points" ); - - ptseq = cvMakeSeqHeaderForArray( - CV_SEQ_KIND_GENERIC|type, sizeof(CvContour), CV_ELEM_SIZE(type), mat->data.ptr, - mat->width + mat->height - 1, &header.seq, &block ); - } - - if( reps < 0 || aeps < 0 ) - CV_Error( CV_StsOutOfRange, "Both reps and aeps must be non-negative" ); - - if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first ) + Mat points = _points.getMat(); + + float linebuf[6]={0.f}; + int npoints2 = points.checkVector(2, -1, false); + int npoints3 = points.checkVector(3, -1, false); + + CV_Assert( npoints2 >= 0 || npoints3 >= 0 ); + + if( points.depth() != CV_32F || !points.isContinuous() ) { - /* no need to copy data in this case */ - points = ptseq->first->data; + Mat temp; + points.convertTo(temp, CV_32F); + points = temp; } + + if( npoints2 >= 0 ) + fitLine2D( points.ptr(), npoints2, distType, + (float)param, (float)reps, (float)aeps, linebuf); else - { - buffer.allocate(ptseq->total*CV_ELEM_SIZE(type)); - points = buffer; - cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ ); + fitLine3D( points.ptr(), npoints3, distType, + (float)param, (float)reps, (float)aeps, linebuf); + + Mat(npoints2 >= 0 ? 4 : 6, 1, CV_32F, linebuf).copyTo(_line); +} - if( CV_MAT_DEPTH(type) != CV_32F ) - { - int i, total = ptseq->total*CV_MAT_CN(type); - assert( CV_MAT_DEPTH(type) == CV_32S ); - for( i = 0; i < total; i++ ) - ((float*)points)[i] = (float)((int*)points)[i]; - } - } +CV_IMPL void +cvFitLine( const CvArr* array, int dist, double param, + double reps, double aeps, float *line ) +{ + CV_Assert(line != 0); - if( dist == CV_DIST_USER ) - CV_Error( CV_StsBadArg, "User-defined distance is not allowed" ); + cv::AutoBuffer buf; + cv::Mat points = cv::cvarrToMat(array, false, false, 0, &buf); + cv::Mat linemat(points.checkVector(2) >= 0 ? 4 : 6, 1, CV_32F, line); - if( CV_MAT_CN(type) == 2 ) - { - IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total, - dist, (float)param, (float)reps, (float)aeps, line )); - } - else - { - IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total, - dist, (float)param, (float)reps, (float)aeps, line )); - } + cv::fitLine(points, linemat, dist, param, reps, aeps); } /* End of file. */