1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
43 static const double eps = 1e-6;
46 icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line )
48 double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0;
54 /* Calculating the average of x and y... */
58 for( i = 0; i < count; i += 1 )
62 x2 += points[i].x * points[i].x;
63 y2 += points[i].y * points[i].y;
64 xy += points[i].x * points[i].y;
70 for( i = 0; i < count; i += 1 )
72 x += weights[i] * points[i].x;
73 y += weights[i] * points[i].y;
74 x2 += weights[i] * points[i].x * points[i].x;
75 y2 += weights[i] * points[i].y * points[i].y;
76 xy += weights[i] * points[i].x * points[i].y;
91 t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
92 line[0] = (float) cos( t );
93 line[1] = (float) sin( t );
102 icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line )
106 float x0 = 0, y0 = 0, z0 = 0;
107 float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
108 float dx2, dy2, dz2, dxy, dxz, dyz;
111 float det[9], evc[9], evl[3];
113 memset( evl, 0, 3*sizeof(evl[0]));
114 memset( evc, 0, 9*sizeof(evl[0]));
118 for( i = 0; i < count; i++ )
120 float x = points[i].x;
121 float y = points[i].y;
122 float z = points[i].z;
123 float w = weights[i];
140 for( i = 0; i < count; i++ )
142 float x = points[i].x;
143 float y = points[i].y;
144 float z = points[i].z;
187 /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */
190 CvMat _det = cvMat( 3, 3, CV_32F, det );
191 CvMat _evc = cvMat( 3, 3, CV_32F, evc );
192 CvMat _evl = cvMat( 3, 1, CV_32F, evl );
193 cvEigenVV( &_det, &_evc, &_evl, 0 );
194 i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2);
198 CvMat _det = cvMat( 3, 3, CV_32F, det );
199 CvMat _evc = cvMat( 3, 3, CV_32F, evc );
200 CvMat _evl = cvMat( 1, 3, CV_32F, evl );
202 cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T );
207 n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] );
208 n = (float)MAX(n, eps);
220 icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist )
223 float px = _line[2], py = _line[3];
224 float nx = _line[1], ny = -_line[0];
225 double sum_dist = 0.;
227 for( j = 0; j < count; j++ )
231 x = points[j].x - px;
232 y = points[j].y - py;
234 dist[j] = (float) fabs( nx * x + ny * y );
242 icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist )
245 float px = _line[3], py = _line[4], pz = _line[5];
246 float vx = _line[0], vy = _line[1], vz = _line[2];
247 double sum_dist = 0.;
249 for( j = 0; j < count; j++ )
254 x = points[j].x - px;
255 y = points[j].y - py;
256 z = points[j].z - pz;
258 p1 = vy * z - vz * y;
259 p2 = vz * x - vx * z;
260 p3 = vx * y - vy * x;
262 dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 );
270 icvWeightL1( float *d, int count, float *w )
274 for( i = 0; i < count; i++ )
276 double t = fabs( (double) d[i] );
277 w[i] = (float)(1. / MAX(t, eps));
282 icvWeightL12( float *d, int count, float *w )
286 for( i = 0; i < count; i++ )
288 w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
294 icvWeightHuber( float *d, int count, float *w, float _c )
297 const float c = _c <= 0 ? 1.345f : _c;
299 for( i = 0; i < count; i++ )
310 icvWeightFair( float *d, int count, float *w, float _c )
313 const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;
315 for( i = 0; i < count; i++ )
317 w[i] = 1 / (1 + d[i] * c);
322 icvWeightWelsch( float *d, int count, float *w, float _c )
325 const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;
327 for( i = 0; i < count; i++ )
329 w[i] = (float) exp( -d[i] * d[i] * c * c );
334 /* Takes an array of 2D points, type of distance (including user-defined
335 distance specified by callbacks, fills the array of four floats with line
336 parameters A, B, C, D, where (A, B) is the normalized direction vector,
337 (C, D) is the point that belongs to the line. */
339 static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist,
340 float _param, float reps, float aeps, float *line )
342 double EPS = count*FLT_EPSILON;
343 void (*calc_weights) (float *, int, float *) = 0;
344 void (*calc_weights_param) (float *, int, float *, float) = 0;
345 float *w; /* weights */
346 float *r; /* square distances */
348 float _line[6], _lineprev[6];
349 float rdelta = reps != 0 ? reps : 1.0f;
350 float adelta = aeps != 0 ? aeps : 0.01f;
351 double min_err = DBL_MAX, err = 0;
352 CvRNG rng = cvRNG(-1);
354 memset( line, 0, 4*sizeof(line[0]) );
359 return icvFitLine2D_wods( points, count, 0, line );
362 calc_weights = icvWeightL1;
366 calc_weights = icvWeightL12;
370 calc_weights_param = icvWeightFair;
374 calc_weights_param = icvWeightWelsch;
378 calc_weights_param = icvWeightHuber;
382 calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
386 return CV_BADFACTOR_ERR;
389 w = (float *) cvAlloc( count * sizeof( float ));
390 r = (float *) cvAlloc( count * sizeof( float ));
392 for( k = 0; k < 20; k++ )
395 for( i = 0; i < count; i++ )
398 for( i = 0; i < MIN(count,10); )
400 j = cvRandInt(&rng) % count;
401 if( w[j] < FLT_EPSILON )
408 icvFitLine2D_wods( points, count, w, _line );
409 for( i = 0; i < 30; i++ )
419 double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1];
422 if( fabs(acos(t)) < adelta )
426 x = (float) fabs( _line[2] - _lineprev[2] );
427 y = (float) fabs( _line[3] - _lineprev[3] );
434 /* calculate distances */
435 err = icvCalcDist2D( points, count, _line, r );
439 /* calculate weights */
441 calc_weights( r, count, w );
443 calc_weights_param( r, count, w, _param );
445 for( j = 0; j < count; j++ )
448 if( fabs(sum_w) > FLT_EPSILON )
451 for( j = 0; j < count; j++ )
452 w[j] = (float)(w[j]*sum_w);
456 for( j = 0; j < count; j++ )
460 /* save the line parameters */
461 memcpy( _lineprev, _line, 4 * sizeof( float ));
464 icvFitLine2D_wods( points, count, w, _line );
470 memcpy( line, _line, 4 * sizeof(line[0]));
482 /* Takes an array of 3D points, type of distance (including user-defined
483 distance specified by callbacks, fills the array of four floats with line
484 parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector,
485 (D, E, F) is the point that belongs to the line. */
488 icvFitLine3D( CvPoint3D32f * points, int count, int dist,
489 float _param, float reps, float aeps, float *line )
491 double EPS = count*FLT_EPSILON;
492 void (*calc_weights) (float *, int, float *) = 0;
493 void (*calc_weights_param) (float *, int, float *, float) = 0;
494 float *w; /* weights */
495 float *r; /* square distances */
497 float _line[6], _lineprev[6];
498 float rdelta = reps != 0 ? reps : 1.0f;
499 float adelta = aeps != 0 ? aeps : 0.01f;
500 double min_err = DBL_MAX, err = 0;
501 CvRNG rng = cvRNG(-1);
503 memset( line, 0, 6*sizeof(line[0]) );
508 return icvFitLine3D_wods( points, count, 0, line );
511 calc_weights = icvWeightL1;
515 calc_weights = icvWeightL12;
519 calc_weights_param = icvWeightFair;
523 calc_weights_param = icvWeightWelsch;
527 calc_weights_param = icvWeightHuber;
532 calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
536 return CV_BADFACTOR_ERR;
539 w = (float *) cvAlloc( count * sizeof( float ));
540 r = (float *) cvAlloc( count * sizeof( float ));
542 for( k = 0; k < 20; k++ )
545 for( i = 0; i < count; i++ )
548 for( i = 0; i < MIN(count,10); )
550 j = cvRandInt(&rng) % count;
551 if( w[j] < FLT_EPSILON )
558 icvFitLine3D_wods( points, count, w, _line );
559 for( i = 0; i < 30; i++ )
569 double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2];
572 if( fabs(acos(t)) < adelta )
574 float x, y, z, ax, ay, az, dx, dy, dz, d;
576 x = _line[3] - _lineprev[3];
577 y = _line[4] - _lineprev[4];
578 z = _line[5] - _lineprev[5];
579 ax = _line[0] - _lineprev[0];
580 ay = _line[1] - _lineprev[1];
581 az = _line[2] - _lineprev[2];
582 dx = (float) fabs( y * az - z * ay );
583 dy = (float) fabs( z * ax - x * az );
584 dz = (float) fabs( x * ay - y * ax );
586 d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz);
591 /* calculate distances */
592 if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count )
595 /* calculate weights */
597 calc_weights( r, count, w );
599 calc_weights_param( r, count, w, _param );
601 for( j = 0; j < count; j++ )
604 if( fabs(sum_w) > FLT_EPSILON )
607 for( j = 0; j < count; j++ )
608 w[j] = (float)(w[j]*sum_w);
612 for( j = 0; j < count; j++ )
616 /* save the line parameters */
617 memcpy( _lineprev, _line, 6 * sizeof( float ));
620 icvFitLine3D_wods( points, count, w, _line );
626 memcpy( line, _line, 6 * sizeof(line[0]));
640 cvFitLine( const CvArr* array, int dist, double param,
641 double reps, double aeps, float *line )
644 CV_FUNCNAME( "cvFitLine" );
649 union { CvContour contour; CvSeq seq; } header;
651 CvSeq* ptseq = (CvSeq*)array;
655 CV_ERROR( CV_StsNullPtr, "NULL pointer to line parameters" );
657 if( CV_IS_SEQ(ptseq) )
659 type = CV_SEQ_ELTYPE(ptseq);
660 if( ptseq->total == 0 )
661 CV_ERROR( CV_StsBadSize, "The sequence has no points" );
662 if( (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) ||
663 CV_ELEM_SIZE(type) != ptseq->elem_size )
664 CV_ERROR( CV_StsUnsupportedFormat,
665 "Input sequence must consist of 2d points or 3d points" );
669 CvMat* mat = (CvMat*)array;
670 type = CV_MAT_TYPE(mat->type);
672 CV_ERROR( CV_StsBadArg, "Input array is not a sequence nor matrix" );
674 if( !CV_IS_MAT_CONT(mat->type) ||
675 (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) ||
676 (mat->width != 1 && mat->height != 1))
677 CV_ERROR( CV_StsBadArg,
678 "Input array must be 1d continuous array of 2d or 3d points" );
680 CV_CALL( ptseq = cvMakeSeqHeaderForArray(
681 CV_SEQ_KIND_GENERIC|type, sizeof(CvContour), CV_ELEM_SIZE(type), mat->data.ptr,
682 mat->width + mat->height - 1, &header.seq, &block ));
685 if( reps < 0 || aeps < 0 )
686 CV_ERROR( CV_StsOutOfRange, "Both reps and aeps must be non-negative" );
688 if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first )
690 /* no need to copy data in this case */
691 points = ptseq->first->data;
695 CV_CALL( buffer = points = (schar*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) ));
696 CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ ));
698 if( CV_MAT_DEPTH(type) != CV_32F )
700 int i, total = ptseq->total*CV_MAT_CN(type);
701 assert( CV_MAT_DEPTH(type) == CV_32S );
703 for( i = 0; i < total; i++ )
704 ((float*)points)[i] = (float)((int*)points)[i];
708 if( dist == CV_DIST_USER )
709 CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" );
711 if( CV_MAT_CN(type) == 2 )
713 IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total,
714 dist, (float)param, (float)reps, (float)aeps, line ));
718 IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total,
719 dist, (float)param, (float)reps, (float)aeps, line ));