X-Git-Url: http://vcs.maemo.org/git/?a=blobdiff_plain;f=cv%2Fsrc%2Fcvlinefit.cpp;fp=cv%2Fsrc%2Fcvlinefit.cpp;h=4095c807d24c23757a6d6b73b60076d3435eb7b8;hb=80cd7b93506cc1926882d5fd08a2c74ee9359e29;hp=aaf22db028d6bcdd5c7429775faff6554139d2f4;hpb=467a270adf12425827305759c0c4ea8f5b2b3854;p=opencv diff --git a/cv/src/cvlinefit.cpp b/cv/src/cvlinefit.cpp index aaf22db..4095c80 100644 --- a/cv/src/cvlinefit.cpp +++ b/cv/src/cvlinefit.cpp @@ -190,7 +190,7 @@ icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line 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 ); + cvEigenVV( &_det, &_evc, &_evl, 0 ); i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2); } #else @@ -331,7 +331,7 @@ icvWeightWelsch( float *d, int count, float *w, float _c ) } -/* Takes an array of 2D points, type of distance (including user-defined +/* 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. */ @@ -339,15 +339,17 @@ parameters A, B, C, D, where (A, B) is the normalized direction vector, static CvStatus icvFitLine2D( CvPoint2D32f * 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; + int i, j, k; float _line[6], _lineprev[6]; - int first = 1; 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); memset( line, 0, 4*sizeof(line[0]) ); @@ -387,99 +389,116 @@ static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, w = (float *) cvAlloc( count * sizeof( float )); r = (float *) cvAlloc( count * sizeof( float )); - for( i = 0; i < count; i++ ) - w[i] = 1.0f; - - icvFitLine2D_wods( points, count, 0, _line ); - for( i = 0; i < 100; i++ ) + for( k = 0; k < 20; k++ ) { - double sum_w = 0; - - if( first ) - { - first = 0; - } - else + int first = 1; + for( i = 0; i < count; i++ ) + w[i] = 0.f; + + for( i = 0; i < MIN(count,10); ) { - double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1]; - t = MAX(t,-1.); - t = MIN(t,1.); - if( fabs(acos(t)) < adelta ) + j = cvRandInt(&rng) % count; + if( w[j] < FLT_EPSILON ) { - float x, y, d; - - x = (float) fabs( _line[2] - _lineprev[2] ); - y = (float) fabs( _line[3] - _lineprev[3] ); - - d = x > y ? x : y; - if( d < rdelta ) - goto _exit_; + w[j] = 1.f; + i++; } } - /* calculate distances */ - if( icvCalcDist2D( points, count, _line, r ) < FLT_EPSILON*count ) - break; - /* calculate weights */ - if( calc_weights ) - { - calc_weights( r, count, w ); - } - else if( calc_weights_param ) + icvFitLine2D_wods( points, count, w, _line ); + for( i = 0; i < 30; i++ ) { - calc_weights_param( r, count, w, _param ); - } - else - goto _exit_; + double sum_w = 0; - for( j = 0; j < count; j++ ) - sum_w += w[j]; + if( first ) + { + first = 0; + } + else + { + double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1]; + t = MAX(t,-1.); + t = MIN(t,1.); + if( fabs(acos(t)) < adelta ) + { + float x, y, d; + + x = (float) fabs( _line[2] - _lineprev[2] ); + y = (float) fabs( _line[3] - _lineprev[3] ); + + d = x > y ? x : y; + if( d < rdelta ) + break; + } + } + /* calculate distances */ + err = icvCalcDist2D( points, count, _line, r ); + if( err < EPS ) + break; + + /* calculate weights */ + if( calc_weights ) + calc_weights( r, count, w ); + else + calc_weights_param( r, count, w, _param ); - if( fabs(sum_w) > FLT_EPSILON ) - { - sum_w = 1./sum_w; for( j = 0; j < count; j++ ) - w[j] = (float)(w[j]*sum_w); + sum_w += w[j]; + + if( fabs(sum_w) > FLT_EPSILON ) + { + sum_w = 1./sum_w; + for( j = 0; j < count; j++ ) + w[j] = (float)(w[j]*sum_w); + } + else + { + for( j = 0; j < count; j++ ) + w[j] = 1.f; + } + + /* save the line parameters */ + memcpy( _lineprev, _line, 4 * sizeof( float )); + + /* Run again... */ + icvFitLine2D_wods( points, count, w, _line ); } - else + + if( err < min_err ) { - for( j = 0; j < count; j++ ) - w[j] = 1.f; + min_err = err; + memcpy( line, _line, 4 * sizeof(line[0])); + if( err < EPS ) + break; } - - /* save the line parameters */ - memcpy( _lineprev, _line, 4 * sizeof( float )); - - /* Run again... */ - icvFitLine2D_wods( points, count, w, _line ); } -_exit_: - memcpy( line, _line, 4 * sizeof(line[0])); 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, +/* 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 ) { + 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; + int i, j, k; float _line[6], _lineprev[6]; - int first = 1; 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); memset( line, 0, 6*sizeof(line[0]) ); @@ -520,82 +539,97 @@ icvFitLine3D( CvPoint3D32f * points, int count, int dist, w = (float *) cvAlloc( count * sizeof( float )); r = (float *) cvAlloc( count * sizeof( float )); - for( i = 0; i < count; i++ ) - w[i] = 1.0f; - - icvFitLine3D_wods( points, count, 0, _line ); - for( i = 0; i < 100; i++ ) + for( k = 0; k < 20; k++ ) { - double sum_w = 0; + int first = 1; + for( i = 0; i < count; i++ ) + w[i] = 0.f; - if( first ) + for( i = 0; i < MIN(count,10); ) { - first = 0; - } - else - { - double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2]; - t = MAX(t,-1.); - t = MIN(t,1.); - if( fabs(acos(t)) < adelta ) + j = cvRandInt(&rng) % count; + if( w[j] < FLT_EPSILON ) { - float x, y, z, ax, ay, az, dx, dy, dz, d; - - x = _line[3] - _lineprev[3]; - y = _line[4] - _lineprev[4]; - z = _line[5] - _lineprev[5]; - ax = _line[0] - _lineprev[0]; - ay = _line[1] - _lineprev[1]; - az = _line[2] - _lineprev[2]; - dx = (float) fabs( y * az - z * ay ); - dy = (float) fabs( z * ax - x * az ); - dz = (float) fabs( x * ay - y * ax ); - - d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz); - if( d < rdelta ) - goto _exit_; + w[j] = 1.f; + i++; } } - /* calculate distances */ - if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count ) - break; - /* calculate weights */ - if( calc_weights ) - { - calc_weights( r, count, w ); - } - else if( calc_weights_param ) + icvFitLine3D_wods( points, count, w, _line ); + for( i = 0; i < 30; i++ ) { - calc_weights_param( r, count, w, _param ); - } - else - goto _exit_; + double sum_w = 0; - for( j = 0; j < count; j++ ) - sum_w += w[j]; + if( first ) + { + first = 0; + } + else + { + double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2]; + t = MAX(t,-1.); + t = MIN(t,1.); + if( fabs(acos(t)) < adelta ) + { + float x, y, z, ax, ay, az, dx, dy, dz, d; + + x = _line[3] - _lineprev[3]; + y = _line[4] - _lineprev[4]; + z = _line[5] - _lineprev[5]; + ax = _line[0] - _lineprev[0]; + ay = _line[1] - _lineprev[1]; + az = _line[2] - _lineprev[2]; + dx = (float) fabs( y * az - z * ay ); + dy = (float) fabs( z * ax - x * az ); + dz = (float) fabs( x * ay - y * ax ); + + d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz); + if( d < rdelta ) + break; + } + } + /* calculate distances */ + if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count ) + break; + + /* calculate weights */ + if( calc_weights ) + calc_weights( r, count, w ); + else + calc_weights_param( r, count, w, _param ); - if( fabs(sum_w) > FLT_EPSILON ) - { - sum_w = 1./sum_w; for( j = 0; j < count; j++ ) - w[j] = (float)(w[j]*sum_w); + sum_w += w[j]; + + if( fabs(sum_w) > FLT_EPSILON ) + { + sum_w = 1./sum_w; + for( j = 0; j < count; j++ ) + w[j] = (float)(w[j]*sum_w); + } + else + { + 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 ); } - else + + if( err < min_err ) { - for( j = 0; j < count; j++ ) - w[j] = 1.f; + min_err = err; + memcpy( line, _line, 6 * sizeof(line[0])); + if( err < EPS ) + break; } - - /* save the line parameters */ - memcpy( _lineprev, _line, 6 * sizeof( float )); - - /* Run again... */ - icvFitLine3D_wods( points, count, w, _line ); } -_exit_: + // Return... - memcpy( line, _line, 6 * sizeof(line[0])); cvFree( &w ); cvFree( &r ); return CV_OK; @@ -606,12 +640,12 @@ CV_IMPL void cvFitLine( const CvArr* array, int dist, double param, double reps, double aeps, float *line ) { - char* buffer = 0; + schar* buffer = 0; CV_FUNCNAME( "cvFitLine" ); __BEGIN__; - char* points = 0; + schar* points = 0; union { CvContour contour; CvSeq seq; } header; CvSeqBlock block; CvSeq* ptseq = (CvSeq*)array; @@ -658,7 +692,7 @@ cvFitLine( const CvArr* array, int dist, double param, } else { - CV_CALL( buffer = points = (char*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) )); + CV_CALL( buffer = points = (schar*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) )); CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ )); if( CV_MAT_DEPTH(type) != CV_32F )