Update the trunk to the OpenCV's CVS (2008-07-14)
[opencv] / cv / src / cvlinefit.cpp
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
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
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
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
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
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
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
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
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
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
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
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
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. */