1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
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.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
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.
25 //
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.
28 //
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.
39 //
40 //M*/
41 #include "_cv.h"
42
43 static const double eps = 1e-6;
44
45 static CvStatus
icvFitLine2D_wods(CvPoint2D32f * points,int _count,float * weights,float * line)46 icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line )
47 {
48 double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0;
49 double dx2, dy2, dxy;
50 int i;
51 int count = _count;
52 float t;
53
54 /* Calculating the average of x and y... */
55
56 if( weights == 0 )
57 {
58 for( i = 0; i < count; i += 1 )
59 {
60 x += points[i].x;
61 y += points[i].y;
62 x2 += points[i].x * points[i].x;
63 y2 += points[i].y * points[i].y;
64 xy += points[i].x * points[i].y;
65 }
66 w = (float) count;
67 }
68 else
69 {
70 for( i = 0; i < count; i += 1 )
71 {
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;
77 w += weights[i];
78 }
79 }
80
81 x /= w;
82 y /= w;
83 x2 /= w;
84 y2 /= w;
85 xy /= w;
86
87 dx2 = x2 - x * x;
88 dy2 = y2 - y * y;
89 dxy = xy - x * y;
90
91 t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
92 line[0] = (float) cos( t );
93 line[1] = (float) sin( t );
94
95 line[2] = (float) x;
96 line[3] = (float) y;
97
98 return CV_NO_ERR;
99 }
100
101 static CvStatus
icvFitLine3D_wods(CvPoint3D32f * points,int count,float * weights,float * line)102 icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line )
103 {
104 int i;
105 float w0 = 0;
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;
109 float *v;
110 float n;
111 float det[9], evc[9], evl[3];
112
113 memset( evl, 0, 3*sizeof(evl[0]));
114 memset( evc, 0, 9*sizeof(evl[0]));
115
116 if( weights )
117 {
118 for( i = 0; i < count; i++ )
119 {
120 float x = points[i].x;
121 float y = points[i].y;
122 float z = points[i].z;
123 float w = weights[i];
124
125
126 x2 += x * x * w;
127 xy += x * y * w;
128 xz += x * z * w;
129 y2 += y * y * w;
130 yz += y * z * w;
131 z2 += z * z * w;
132 x0 += x * w;
133 y0 += y * w;
134 z0 += z * w;
135 w0 += w;
136 }
137 }
138 else
139 {
140 for( i = 0; i < count; i++ )
141 {
142 float x = points[i].x;
143 float y = points[i].y;
144 float z = points[i].z;
145
146 x2 += x * x;
147 xy += x * y;
148 xz += x * z;
149 y2 += y * y;
150 yz += y * z;
151 z2 += z * z;
152 x0 += x;
153 y0 += y;
154 z0 += z;
155 }
156 w0 = (float) count;
157 }
158
159 x2 /= w0;
160 xy /= w0;
161 xz /= w0;
162 y2 /= w0;
163 yz /= w0;
164 z2 /= w0;
165
166 x0 /= w0;
167 y0 /= w0;
168 z0 /= w0;
169
170 dx2 = x2 - x0 * x0;
171 dxy = xy - x0 * y0;
172 dxz = xz - x0 * z0;
173 dy2 = y2 - y0 * y0;
174 dyz = yz - y0 * z0;
175 dz2 = z2 - z0 * z0;
176
177 det[0] = dz2 + dy2;
178 det[1] = -dxy;
179 det[2] = -dxz;
180 det[3] = det[1];
181 det[4] = dx2 + dz2;
182 det[5] = -dyz;
183 det[6] = det[2];
184 det[7] = det[5];
185 det[8] = dy2 + dx2;
186
187 /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */
188 #if 1
189 {
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);
195 }
196 #else
197 {
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 );
201
202 cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T );
203 }
204 i = 2;
205 #endif
206 v = &evc[i * 3];
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);
209 line[0] = v[0] / n;
210 line[1] = v[1] / n;
211 line[2] = v[2] / n;
212 line[3] = x0;
213 line[4] = y0;
214 line[5] = z0;
215
216 return CV_NO_ERR;
217 }
218
219 static double
icvCalcDist2D(CvPoint2D32f * points,int count,float * _line,float * dist)220 icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist )
221 {
222 int j;
223 float px = _line[2], py = _line[3];
224 float nx = _line[1], ny = -_line[0];
225 double sum_dist = 0.;
226
227 for( j = 0; j < count; j++ )
228 {
229 float x, y;
230
231 x = points[j].x - px;
232 y = points[j].y - py;
233
234 dist[j] = (float) fabs( nx * x + ny * y );
235 sum_dist += dist[j];
236 }
237
238 return sum_dist;
239 }
240
241 static double
icvCalcDist3D(CvPoint3D32f * points,int count,float * _line,float * dist)242 icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist )
243 {
244 int j;
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.;
248
249 for( j = 0; j < count; j++ )
250 {
251 float x, y, z;
252 double p1, p2, p3;
253
254 x = points[j].x - px;
255 y = points[j].y - py;
256 z = points[j].z - pz;
257
258 p1 = vy * z - vz * y;
259 p2 = vz * x - vx * z;
260 p3 = vx * y - vy * x;
261
262 dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 );
263 sum_dist += dist[j];
264 }
265
266 return sum_dist;
267 }
268
269 static void
icvWeightL1(float * d,int count,float * w)270 icvWeightL1( float *d, int count, float *w )
271 {
272 int i;
273
274 for( i = 0; i < count; i++ )
275 {
276 double t = fabs( (double) d[i] );
277 w[i] = (float)(1. / MAX(t, eps));
278 }
279 }
280
281 static void
icvWeightL12(float * d,int count,float * w)282 icvWeightL12( float *d, int count, float *w )
283 {
284 int i;
285
286 for( i = 0; i < count; i++ )
287 {
288 w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
289 }
290 }
291
292
293 static void
icvWeightHuber(float * d,int count,float * w,float _c)294 icvWeightHuber( float *d, int count, float *w, float _c )
295 {
296 int i;
297 const float c = _c <= 0 ? 1.345f : _c;
298
299 for( i = 0; i < count; i++ )
300 {
301 if( d[i] < c )
302 w[i] = 1.0f;
303 else
304 w[i] = c/d[i];
305 }
306 }
307
308
309 static void
icvWeightFair(float * d,int count,float * w,float _c)310 icvWeightFair( float *d, int count, float *w, float _c )
311 {
312 int i;
313 const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;
314
315 for( i = 0; i < count; i++ )
316 {
317 w[i] = 1 / (1 + d[i] * c);
318 }
319 }
320
321 static void
icvWeightWelsch(float * d,int count,float * w,float _c)322 icvWeightWelsch( float *d, int count, float *w, float _c )
323 {
324 int i;
325 const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;
326
327 for( i = 0; i < count; i++ )
328 {
329 w[i] = (float) exp( -d[i] * d[i] * c * c );
330 }
331 }
332
333
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. */
338
icvFitLine2D(CvPoint2D32f * points,int count,int dist,float _param,float reps,float aeps,float * line)339 static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist,
340 float _param, float reps, float aeps, float *line )
341 {
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 */
347 int i, j, k;
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);
353
354 memset( line, 0, 4*sizeof(line[0]) );
355
356 switch (dist)
357 {
358 case CV_DIST_L2:
359 return icvFitLine2D_wods( points, count, 0, line );
360
361 case CV_DIST_L1:
362 calc_weights = icvWeightL1;
363 break;
364
365 case CV_DIST_L12:
366 calc_weights = icvWeightL12;
367 break;
368
369 case CV_DIST_FAIR:
370 calc_weights_param = icvWeightFair;
371 break;
372
373 case CV_DIST_WELSCH:
374 calc_weights_param = icvWeightWelsch;
375 break;
376
377 case CV_DIST_HUBER:
378 calc_weights_param = icvWeightHuber;
379 break;
380
381 /*case CV_DIST_USER:
382 calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
383 break;*/
384
385 default:
386 return CV_BADFACTOR_ERR;
387 }
388
389 w = (float *) cvAlloc( count * sizeof( float ));
390 r = (float *) cvAlloc( count * sizeof( float ));
391
392 for( k = 0; k < 20; k++ )
393 {
394 int first = 1;
395 for( i = 0; i < count; i++ )
396 w[i] = 0.f;
397
398 for( i = 0; i < MIN(count,10); )
399 {
400 j = cvRandInt(&rng) % count;
401 if( w[j] < FLT_EPSILON )
402 {
403 w[j] = 1.f;
404 i++;
405 }
406 }
407
408 icvFitLine2D_wods( points, count, w, _line );
409 for( i = 0; i < 30; i++ )
410 {
411 double sum_w = 0;
412
413 if( first )
414 {
415 first = 0;
416 }
417 else
418 {
419 double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1];
420 t = MAX(t,-1.);
421 t = MIN(t,1.);
422 if( fabs(acos(t)) < adelta )
423 {
424 float x, y, d;
425
426 x = (float) fabs( _line[2] - _lineprev[2] );
427 y = (float) fabs( _line[3] - _lineprev[3] );
428
429 d = x > y ? x : y;
430 if( d < rdelta )
431 break;
432 }
433 }
434 /* calculate distances */
435 err = icvCalcDist2D( points, count, _line, r );
436 if( err < EPS )
437 break;
438
439 /* calculate weights */
440 if( calc_weights )
441 calc_weights( r, count, w );
442 else
443 calc_weights_param( r, count, w, _param );
444
445 for( j = 0; j < count; j++ )
446 sum_w += w[j];
447
448 if( fabs(sum_w) > FLT_EPSILON )
449 {
450 sum_w = 1./sum_w;
451 for( j = 0; j < count; j++ )
452 w[j] = (float)(w[j]*sum_w);
453 }
454 else
455 {
456 for( j = 0; j < count; j++ )
457 w[j] = 1.f;
458 }
459
460 /* save the line parameters */
461 memcpy( _lineprev, _line, 4 * sizeof( float ));
462
463 /* Run again... */
464 icvFitLine2D_wods( points, count, w, _line );
465 }
466
467 if( err < min_err )
468 {
469 min_err = err;
470 memcpy( line, _line, 4 * sizeof(line[0]));
471 if( err < EPS )
472 break;
473 }
474 }
475
476 cvFree( &w );
477 cvFree( &r );
478 return CV_OK;
479 }
480
481
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. */
486
487 static CvStatus
icvFitLine3D(CvPoint3D32f * points,int count,int dist,float _param,float reps,float aeps,float * line)488 icvFitLine3D( CvPoint3D32f * points, int count, int dist,
489 float _param, float reps, float aeps, float *line )
490 {
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 */
496 int i, j, k;
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);
502
503 memset( line, 0, 6*sizeof(line[0]) );
504
505 switch (dist)
506 {
507 case CV_DIST_L2:
508 return icvFitLine3D_wods( points, count, 0, line );
509
510 case CV_DIST_L1:
511 calc_weights = icvWeightL1;
512 break;
513
514 case CV_DIST_L12:
515 calc_weights = icvWeightL12;
516 break;
517
518 case CV_DIST_FAIR:
519 calc_weights_param = icvWeightFair;
520 break;
521
522 case CV_DIST_WELSCH:
523 calc_weights_param = icvWeightWelsch;
524 break;
525
526 case CV_DIST_HUBER:
527 calc_weights_param = icvWeightHuber;
528 break;
529
530 /*case CV_DIST_USER:
531 _PFP.p = param;
532 calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
533 break;*/
534
535 default:
536 return CV_BADFACTOR_ERR;
537 }
538
539 w = (float *) cvAlloc( count * sizeof( float ));
540 r = (float *) cvAlloc( count * sizeof( float ));
541
542 for( k = 0; k < 20; k++ )
543 {
544 int first = 1;
545 for( i = 0; i < count; i++ )
546 w[i] = 0.f;
547
548 for( i = 0; i < MIN(count,10); )
549 {
550 j = cvRandInt(&rng) % count;
551 if( w[j] < FLT_EPSILON )
552 {
553 w[j] = 1.f;
554 i++;
555 }
556 }
557
558 icvFitLine3D_wods( points, count, w, _line );
559 for( i = 0; i < 30; i++ )
560 {
561 double sum_w = 0;
562
563 if( first )
564 {
565 first = 0;
566 }
567 else
568 {
569 double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2];
570 t = MAX(t,-1.);
571 t = MIN(t,1.);
572 if( fabs(acos(t)) < adelta )
573 {
574 float x, y, z, ax, ay, az, dx, dy, dz, d;
575
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 );
585
586 d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz);
587 if( d < rdelta )
588 break;
589 }
590 }
591 /* calculate distances */
592 if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count )
593 break;
594
595 /* calculate weights */
596 if( calc_weights )
597 calc_weights( r, count, w );
598 else
599 calc_weights_param( r, count, w, _param );
600
601 for( j = 0; j < count; j++ )
602 sum_w += w[j];
603
604 if( fabs(sum_w) > FLT_EPSILON )
605 {
606 sum_w = 1./sum_w;
607 for( j = 0; j < count; j++ )
608 w[j] = (float)(w[j]*sum_w);
609 }
610 else
611 {
612 for( j = 0; j < count; j++ )
613 w[j] = 1.f;
614 }
615
616 /* save the line parameters */
617 memcpy( _lineprev, _line, 6 * sizeof( float ));
618
619 /* Run again... */
620 icvFitLine3D_wods( points, count, w, _line );
621 }
622
623 if( err < min_err )
624 {
625 min_err = err;
626 memcpy( line, _line, 6 * sizeof(line[0]));
627 if( err < EPS )
628 break;
629 }
630 }
631
632 // Return...
633 cvFree( &w );
634 cvFree( &r );
635 return CV_OK;
636 }
637
638
639 CV_IMPL void
cvFitLine(const CvArr * array,int dist,double param,double reps,double aeps,float * line)640 cvFitLine( const CvArr* array, int dist, double param,
641 double reps, double aeps, float *line )
642 {
643 schar* buffer = 0;
644 CV_FUNCNAME( "cvFitLine" );
645
646 __BEGIN__;
647
648 schar* points = 0;
649 union { CvContour contour; CvSeq seq; } header;
650 CvSeqBlock block;
651 CvSeq* ptseq = (CvSeq*)array;
652 int type;
653
654 if( !line )
655 CV_ERROR( CV_StsNullPtr, "NULL pointer to line parameters" );
656
657 if( CV_IS_SEQ(ptseq) )
658 {
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" );
666 }
667 else
668 {
669 CvMat* mat = (CvMat*)array;
670 type = CV_MAT_TYPE(mat->type);
671 if( !CV_IS_MAT(mat))
672 CV_ERROR( CV_StsBadArg, "Input array is not a sequence nor matrix" );
673
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" );
679
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 ));
683 }
684
685 if( reps < 0 || aeps < 0 )
686 CV_ERROR( CV_StsOutOfRange, "Both reps and aeps must be non-negative" );
687
688 if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first )
689 {
690 /* no need to copy data in this case */
691 points = ptseq->first->data;
692 }
693 else
694 {
695 CV_CALL( buffer = points = (schar*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) ));
696 CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ ));
697
698 if( CV_MAT_DEPTH(type) != CV_32F )
699 {
700 int i, total = ptseq->total*CV_MAT_CN(type);
701 assert( CV_MAT_DEPTH(type) == CV_32S );
702
703 for( i = 0; i < total; i++ )
704 ((float*)points)[i] = (float)((int*)points)[i];
705 }
706 }
707
708 if( dist == CV_DIST_USER )
709 CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" );
710
711 if( CV_MAT_CN(type) == 2 )
712 {
713 IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total,
714 dist, (float)param, (float)reps, (float)aeps, line ));
715 }
716 else
717 {
718 IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total,
719 dist, (float)param, (float)reps, (float)aeps, line ));
720 }
721
722 __END__;
723
724 cvFree( &buffer );
725 }
726
727 /* End of file. */
728