--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// Intel License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of Intel Corporation may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+#include "_cv.h"
+
+static const double eps = 1e-6;
+
+static CvStatus
+icvFitLine2D_wods( CvPoint2D32f * 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... */
+
+ if( weights == 0 )
+ {
+ for( i = 0; i < count; i += 1 )
+ {
+ x += points[i].x;
+ y += points[i].y;
+ x2 += points[i].x * points[i].x;
+ y2 += points[i].y * points[i].y;
+ xy += points[i].x * points[i].y;
+ }
+ w = (float) count;
+ }
+ else
+ {
+ for( i = 0; i < count; i += 1 )
+ {
+ x += weights[i] * points[i].x;
+ y += weights[i] * points[i].y;
+ x2 += weights[i] * points[i].x * points[i].x;
+ y2 += weights[i] * points[i].y * points[i].y;
+ xy += weights[i] * points[i].x * points[i].y;
+ w += weights[i];
+ }
+ }
+
+ x /= w;
+ y /= w;
+ x2 /= w;
+ y2 /= w;
+ xy /= w;
+
+ dx2 = x2 - x * x;
+ dy2 = y2 - y * y;
+ dxy = xy - x * y;
+
+ t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
+ line[0] = (float) cos( t );
+ line[1] = (float) sin( t );
+
+ line[2] = (float) x;
+ line[3] = (float) y;
+
+ return CV_NO_ERR;
+}
+
+static CvStatus
+icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line )
+{
+ int i;
+ float w0 = 0;
+ float x0 = 0, y0 = 0, z0 = 0;
+ float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
+ float dx2, dy2, dz2, dxy, dxz, dyz;
+ float *v;
+ float n;
+ float det[9], evc[9], evl[3];
+
+ memset( evl, 0, 3*sizeof(evl[0]));
+ memset( evc, 0, 9*sizeof(evl[0]));
+
+ if( weights )
+ {
+ for( i = 0; i < count; i++ )
+ {
+ float x = points[i].x;
+ float y = points[i].y;
+ float z = points[i].z;
+ float w = weights[i];
+
+
+ x2 += x * x * w;
+ xy += x * y * w;
+ xz += x * z * w;
+ y2 += y * y * w;
+ yz += y * z * w;
+ z2 += z * z * w;
+ x0 += x * w;
+ y0 += y * w;
+ z0 += z * w;
+ w0 += w;
+ }
+ }
+ else
+ {
+ for( i = 0; i < count; i++ )
+ {
+ float x = points[i].x;
+ float y = points[i].y;
+ float z = points[i].z;
+
+ x2 += x * x;
+ xy += x * y;
+ xz += x * z;
+ y2 += y * y;
+ yz += y * z;
+ z2 += z * z;
+ x0 += x;
+ y0 += y;
+ z0 += z;
+ }
+ w0 = (float) count;
+ }
+
+ x2 /= w0;
+ xy /= w0;
+ xz /= w0;
+ y2 /= w0;
+ yz /= w0;
+ z2 /= w0;
+
+ x0 /= w0;
+ y0 /= w0;
+ z0 /= w0;
+
+ dx2 = x2 - x0 * x0;
+ dxy = xy - x0 * y0;
+ dxz = xz - x0 * z0;
+ dy2 = y2 - y0 * y0;
+ dyz = yz - y0 * z0;
+ dz2 = z2 - z0 * z0;
+
+ det[0] = dz2 + dy2;
+ det[1] = -dxy;
+ det[2] = -dxz;
+ det[3] = det[1];
+ det[4] = dx2 + dz2;
+ det[5] = -dyz;
+ det[6] = det[2];
+ 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 );
+ 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);
+ line[0] = v[0] / n;
+ line[1] = v[1] / n;
+ line[2] = v[2] / n;
+ line[3] = x0;
+ line[4] = y0;
+ line[5] = z0;
+
+ return CV_NO_ERR;
+}
+
+static double
+icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist )
+{
+ int j;
+ float px = _line[2], py = _line[3];
+ float nx = _line[1], ny = -_line[0];
+ double sum_dist = 0.;
+
+ for( j = 0; j < count; j++ )
+ {
+ float x, y;
+
+ x = points[j].x - px;
+ y = points[j].y - py;
+
+ dist[j] = (float) fabs( nx * x + ny * y );
+ sum_dist += dist[j];
+ }
+
+ return sum_dist;
+}
+
+static double
+icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist )
+{
+ int j;
+ float px = _line[3], py = _line[4], pz = _line[5];
+ float vx = _line[0], vy = _line[1], vz = _line[2];
+ double sum_dist = 0.;
+
+ for( j = 0; j < count; j++ )
+ {
+ float x, y, z;
+ double p1, p2, p3;
+
+ x = points[j].x - px;
+ y = points[j].y - py;
+ z = points[j].z - pz;
+
+ p1 = vy * z - vz * y;
+ p2 = vz * x - vx * z;
+ p3 = vx * y - vy * x;
+
+ dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 );
+ sum_dist += dist[j];
+ }
+
+ return sum_dist;
+}
+
+static void
+icvWeightL1( float *d, int count, float *w )
+{
+ int i;
+
+ for( i = 0; i < count; i++ )
+ {
+ double t = fabs( (double) d[i] );
+ w[i] = (float)(1. / MAX(t, eps));
+ }
+}
+
+static void
+icvWeightL12( float *d, int count, float *w )
+{
+ int i;
+
+ for( i = 0; i < count; i++ )
+ {
+ w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
+ }
+}
+
+
+static void
+icvWeightHuber( float *d, int count, float *w, float _c )
+{
+ int i;
+ const float c = _c <= 0 ? 1.345f : _c;
+
+ for( i = 0; i < count; i++ )
+ {
+ if( d[i] < c )
+ w[i] = 1.0f;
+ else
+ w[i] = c/d[i];
+ }
+}
+
+
+static void
+icvWeightFair( float *d, int count, float *w, float _c )
+{
+ int i;
+ const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;
+
+ for( i = 0; i < count; i++ )
+ {
+ w[i] = 1 / (1 + d[i] * c);
+ }
+}
+
+static void
+icvWeightWelsch( 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 );
+ }
+}
+
+
+/* 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. */
+
+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, 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);
+
+ memset( line, 0, 4*sizeof(line[0]) );
+
+ switch (dist)
+ {
+ case CV_DIST_L2:
+ return icvFitLine2D_wods( points, count, 0, line );
+
+ case CV_DIST_L1:
+ calc_weights = icvWeightL1;
+ break;
+
+ case CV_DIST_L12:
+ calc_weights = icvWeightL12;
+ break;
+
+ case CV_DIST_FAIR:
+ calc_weights_param = icvWeightFair;
+ break;
+
+ case CV_DIST_WELSCH:
+ calc_weights_param = icvWeightWelsch;
+ break;
+
+ case CV_DIST_HUBER:
+ calc_weights_param = icvWeightHuber;
+ break;
+
+ /*case CV_DIST_USER:
+ calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
+ break;*/
+
+ default:
+ return CV_BADFACTOR_ERR;
+ }
+
+ w = (float *) cvAlloc( count * sizeof( float ));
+ r = (float *) cvAlloc( count * sizeof( float ));
+
+ for( k = 0; k < 20; k++ )
+ {
+ int first = 1;
+ for( i = 0; i < count; i++ )
+ w[i] = 0.f;
+
+ for( i = 0; i < MIN(count,10); )
+ {
+ j = cvRandInt(&rng) % count;
+ if( w[j] < FLT_EPSILON )
+ {
+ w[j] = 1.f;
+ i++;
+ }
+ }
+
+ icvFitLine2D_wods( points, count, w, _line );
+ for( i = 0; i < 30; i++ )
+ {
+ double sum_w = 0;
+
+ 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 );
+
+ for( j = 0; j < count; j++ )
+ 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 );
+ }
+
+ if( err < min_err )
+ {
+ min_err = err;
+ memcpy( line, _line, 4 * sizeof(line[0]));
+ if( err < EPS )
+ 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 )
+{
+ 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);
+
+ memset( line, 0, 6*sizeof(line[0]) );
+
+ switch (dist)
+ {
+ case CV_DIST_L2:
+ return icvFitLine3D_wods( points, count, 0, line );
+
+ case CV_DIST_L1:
+ calc_weights = icvWeightL1;
+ break;
+
+ case CV_DIST_L12:
+ calc_weights = icvWeightL12;
+ break;
+
+ case CV_DIST_FAIR:
+ calc_weights_param = icvWeightFair;
+ break;
+
+ case CV_DIST_WELSCH:
+ calc_weights_param = icvWeightWelsch;
+ break;
+
+ case CV_DIST_HUBER:
+ calc_weights_param = icvWeightHuber;
+ break;
+
+ /*case CV_DIST_USER:
+ _PFP.p = param;
+ calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
+ break;*/
+
+ default:
+ return CV_BADFACTOR_ERR;
+ }
+
+ w = (float *) cvAlloc( count * sizeof( float ));
+ r = (float *) cvAlloc( count * sizeof( float ));
+
+ for( k = 0; k < 20; k++ )
+ {
+ int first = 1;
+ for( i = 0; i < count; i++ )
+ w[i] = 0.f;
+
+ for( i = 0; i < MIN(count,10); )
+ {
+ j = cvRandInt(&rng) % count;
+ if( w[j] < FLT_EPSILON )
+ {
+ w[j] = 1.f;
+ i++;
+ }
+ }
+
+ icvFitLine3D_wods( points, count, w, _line );
+ for( i = 0; i < 30; i++ )
+ {
+ double sum_w = 0;
+
+ 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 );
+
+ for( j = 0; j < count; j++ )
+ 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 );
+ }
+
+ if( err < min_err )
+ {
+ min_err = err;
+ memcpy( line, _line, 6 * sizeof(line[0]));
+ if( err < EPS )
+ 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 )
+{
+ schar* buffer = 0;
+ CV_FUNCNAME( "cvFitLine" );
+
+ __BEGIN__;
+
+ 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" );
+
+ CV_CALL( 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 )
+ {
+ /* no need to copy data in this case */
+ points = ptseq->first->data;
+ }
+ else
+ {
+ 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 )
+ {
+ 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];
+ }
+ }
+
+ if( dist == CV_DIST_USER )
+ CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" );
+
+ 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 ));
+ }
+
+ __END__;
+
+ cvFree( &buffer );
+}
+
+/* End of file. */
projects / opencv / blobdiff