Move the sources to trunk
[opencv] / cxcore / src / cxutils.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
42 #include "_cxcore.h"
43
44 CV_IMPL void
45 cvKMeans2( const CvArr* samples_arr, int cluster_count,
46            CvArr* labels_arr, CvTermCriteria termcrit )
47 {
48     CvMat* centers = 0;
49     CvMat* old_centers = 0;
50     CvMat* counters = 0;
51
52     CV_FUNCNAME( "cvKMeans2" );
53
54     __BEGIN__;
55
56     CvMat samples_stub, labels_stub;
57     CvMat* samples = (CvMat*)samples_arr;
58     CvMat* labels = (CvMat*)labels_arr;
59     CvMat* temp = 0;
60     CvRNG rng = CvRNG(-1);
61     int i, j, k, sample_count, dims;
62     int ids_delta, iter;
63     double max_dist;
64
65     if( !CV_IS_MAT( samples ))
66         CV_CALL( samples = cvGetMat( samples, &samples_stub ));
67
68     if( !CV_IS_MAT( labels ))
69         CV_CALL( labels = cvGetMat( labels, &labels_stub ));
70
71     if( cluster_count < 1 )
72         CV_ERROR( CV_StsOutOfRange, "Number of clusters should be positive" );
73
74     if( CV_MAT_DEPTH(samples->type) != CV_32F || CV_MAT_TYPE(labels->type) != CV_32SC1 )
75         CV_ERROR( CV_StsUnsupportedFormat,
76         "samples should be floating-point matrix, cluster_idx - integer vector" );
77
78     if( labels->rows != 1 && (labels->cols != 1 || !CV_IS_MAT_CONT(labels->type)) ||
79         labels->rows + labels->cols - 1 != samples->rows )
80         CV_ERROR( CV_StsUnmatchedSizes,
81         "cluster_idx should be 1D vector of the same number of elements as samples' number of rows" );
82
83     CV_CALL( termcrit = cvCheckTermCriteria( termcrit, 1e-6, 100 ));
84
85     termcrit.epsilon *= termcrit.epsilon;
86     sample_count = samples->rows;
87
88     if( cluster_count > sample_count )
89         cluster_count = sample_count;
90
91     dims = samples->cols*CV_MAT_CN(samples->type);
92     ids_delta = labels->step ? labels->step/(int)sizeof(int) : 1;
93
94     CV_CALL( centers = cvCreateMat( cluster_count, dims, CV_64FC1 ));
95     CV_CALL( old_centers = cvCreateMat( cluster_count, dims, CV_64FC1 ));
96     CV_CALL( counters = cvCreateMat( 1, cluster_count, CV_32SC1 ));
97
98     // init centers
99     for( i = 0; i < sample_count; i++ )
100         labels->data.i[i] = cvRandInt(&rng) % cluster_count;
101
102     counters->cols = cluster_count; // cut down counters
103     max_dist = termcrit.epsilon*2;
104
105     for( iter = 0; iter < termcrit.max_iter; iter++ )
106     {
107         // computer centers
108         cvZero( centers );
109         cvZero( counters );
110
111         for( i = 0; i < sample_count; i++ )
112         {
113             float* s = (float*)(samples->data.ptr + i*samples->step);
114             k = labels->data.i[i*ids_delta];
115             double* c = (double*)(centers->data.ptr + k*centers->step);
116             for( j = 0; j <= dims - 4; j += 4 )
117             {
118                 double t0 = c[j] + s[j];
119                 double t1 = c[j+1] + s[j+1];
120
121                 c[j] = t0;
122                 c[j+1] = t1;
123
124                 t0 = c[j+2] + s[j+2];
125                 t1 = c[j+3] + s[j+3];
126
127                 c[j+2] = t0;
128                 c[j+3] = t1;
129             }
130             for( ; j < dims; j++ )
131                 c[j] += s[j];
132             counters->data.i[k]++;
133         }
134
135         if( iter > 0 )
136             max_dist = 0;
137
138         for( k = 0; k < cluster_count; k++ )
139         {
140             double* c = (double*)(centers->data.ptr + k*centers->step);
141             if( counters->data.i[k] != 0 )
142             {
143                 double scale = 1./counters->data.i[k];
144                 for( j = 0; j < dims; j++ )
145                     c[j] *= scale;
146             }
147             else
148             {
149                 i = cvRandInt( &rng ) % sample_count;
150                 float* s = (float*)(samples->data.ptr + i*samples->step);
151                 for( j = 0; j < dims; j++ )
152                     c[j] = s[j];
153             }
154             
155             if( iter > 0 )
156             {
157                 double dist = 0;
158                 double* c_o = (double*)(old_centers->data.ptr + k*old_centers->step);
159                 for( j = 0; j < dims; j++ )
160                 {
161                     double t = c[j] - c_o[j];
162                     dist += t*t;
163                 }
164                 if( max_dist < dist )
165                     max_dist = dist;
166             }
167         }
168
169         // assign labels
170         for( i = 0; i < sample_count; i++ )
171         {
172             float* s = (float*)(samples->data.ptr + i*samples->step);
173             int k_best = 0;
174             double min_dist = DBL_MAX;
175
176             for( k = 0; k < cluster_count; k++ )
177             {
178                 double* c = (double*)(centers->data.ptr + k*centers->step);
179                 double dist = 0;
180                 
181                 j = 0;
182                 for( ; j <= dims - 4; j += 4 )
183                 {
184                     double t0 = c[j] - s[j];
185                     double t1 = c[j+1] - s[j+1];
186                     dist += t0*t0 + t1*t1;
187                     t0 = c[j+2] - s[j+2];
188                     t1 = c[j+3] - s[j+3];
189                     dist += t0*t0 + t1*t1;
190                 }
191
192                 for( ; j < dims; j++ )
193                 {
194                     double t = c[j] - s[j];
195                     dist += t*t;
196                 }
197
198                 if( min_dist > dist )
199                 {
200                     min_dist = dist;
201                     k_best = k;
202                 }
203             }
204
205             labels->data.i[i*ids_delta] = k_best;
206         }
207
208         if( max_dist < termcrit.epsilon )
209             break;
210
211         CV_SWAP( centers, old_centers, temp );
212     }
213
214     cvZero( counters );
215     for( i = 0; i < sample_count; i++ )
216         counters->data.i[labels->data.i[i]]++;
217
218     // ensure that we do not have empty clusters
219     for( k = 0; k < cluster_count; k++ )
220         if( counters->data.i[k] == 0 )
221             for(;;)
222             {
223                 i = cvRandInt(&rng) % sample_count;
224                 j = labels->data.i[i];
225                 if( counters->data.i[j] > 1 )
226                 {
227                     labels->data.i[i] = k;
228                     counters->data.i[j]--;
229                     counters->data.i[k]++;
230                     break;
231                 }
232             }
233
234     __END__;
235
236     cvReleaseMat( &centers );
237     cvReleaseMat( &old_centers );
238     cvReleaseMat( &counters );
239 }
240
241
242 /*
243   Finds real roots of cubic, quadratic or linear equation.
244   The original code has been taken from Ken Turkowski web page
245   (http://www.worldserver.com/turk/opensource/) and adopted for OpenCV.
246   Here is the copyright notice.
247
248   -----------------------------------------------------------------------
249   Copyright (C) 1978-1999 Ken Turkowski. <turk@computer.org>
250  
251     All rights reserved.
252  
253     Warranty Information
254       Even though I have reviewed this software, I make no warranty
255       or representation, either express or implied, with respect to this
256       software, its quality, accuracy, merchantability, or fitness for a
257       particular purpose.  As a result, this software is provided "as is,"
258       and you, its user, are assuming the entire risk as to its quality
259       and accuracy.
260  
261     This code may be used and freely distributed as long as it includes
262     this copyright notice and the above warranty information.
263   -----------------------------------------------------------------------
264 */
265 CV_IMPL int
266 cvSolveCubic( const CvMat* coeffs, CvMat* roots )
267 {
268     int n = 0;
269     
270     CV_FUNCNAME( "cvSolveCubic" );
271
272     __BEGIN__;
273
274     double a0 = 1., a1, a2, a3;
275     double x0 = 0., x1 = 0., x2 = 0.;
276     int step = 1, coeff_count;
277     
278     if( !CV_IS_MAT(coeffs) )
279         CV_ERROR( !coeffs ? CV_StsNullPtr : CV_StsBadArg, "Input parameter is not a valid matrix" );
280
281     if( !CV_IS_MAT(roots) )
282         CV_ERROR( !roots ? CV_StsNullPtr : CV_StsBadArg, "Output parameter is not a valid matrix" );
283
284     if( CV_MAT_TYPE(coeffs->type) != CV_32FC1 && CV_MAT_TYPE(coeffs->type) != CV_64FC1 ||
285         CV_MAT_TYPE(roots->type) != CV_32FC1 && CV_MAT_TYPE(roots->type) != CV_64FC1 )
286         CV_ERROR( CV_StsUnsupportedFormat,
287         "Both matrices should be floating-point (single or double precision)" );
288
289     coeff_count = coeffs->rows + coeffs->cols - 1;
290
291     if( coeffs->rows != 1 && coeffs->cols != 1 || coeff_count != 3 && coeff_count != 4 )
292         CV_ERROR( CV_StsBadSize,
293         "The matrix of coefficients must be 1-dimensional vector of 3 or 4 elements" );
294
295     if( roots->rows != 1 && roots->cols != 1 ||
296         roots->rows + roots->cols - 1 != 3 )
297         CV_ERROR( CV_StsBadSize,
298         "The matrix of roots must be 1-dimensional vector of 3 elements" );
299
300     if( CV_MAT_TYPE(coeffs->type) == CV_32FC1 )
301     {
302         const float* c = coeffs->data.fl;
303         if( coeffs->rows > 1 )
304             step = coeffs->step/sizeof(c[0]);
305         if( coeff_count == 4 )
306             a0 = c[0], c += step;
307         a1 = c[0];
308         a2 = c[step];
309         a3 = c[step*2];
310     }
311     else
312     {
313         const double* c = coeffs->data.db;
314         if( coeffs->rows > 1 )
315             step = coeffs->step/sizeof(c[0]);
316         if( coeff_count == 4 )
317             a0 = c[0], c += step;
318         a1 = c[0];
319         a2 = c[step];
320         a3 = c[step*2];
321     }
322
323     if( a0 == 0 )
324     {
325         if( a1 == 0 )
326         {
327             if( a2 == 0 )
328                 n = a3 == 0 ? -1 : 0;
329             else
330             {
331                 // linear equation
332                 x0 = a3/a2;
333                 n = 1;
334             }
335         }
336         else
337         {
338             // quadratic equation
339             double d = a2*a2 - 4*a1*a3;
340             if( d >= 0 )
341             {
342                 d = sqrt(d);
343                 double q = (-a2 + (a2 < 0 ? -d : d)) * 0.5;
344                 x0 = q / a1;
345                 x1 = a3 / q;
346                 n = d > 0 ? 2 : 1;
347             }
348         }
349     }
350     else
351     {
352         a0 = 1./a0;
353         a1 *= a0;
354         a2 *= a0;
355         a3 *= a0;
356
357         double Q = (a1 * a1 - 3 * a2) * (1./9);
358         double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) * (1./54);
359         double Qcubed = Q * Q * Q;
360         double d = Qcubed - R * R;
361     
362         if( d >= 0 )
363         {
364             double theta = acos(R / sqrt(Qcubed));
365             double sqrtQ = sqrt(Q);
366             double t0 = -2 * sqrtQ;
367             double t1 = theta * (1./3);
368             double t2 = a1 * (1./3);
369             x0 = t0 * cos(t1) - t2;
370             x1 = t0 * cos(t1 + (2.*CV_PI/3)) - t2;
371             x2 = t0 * cos(t1 + (4.*CV_PI/3)) - t2;
372             n = 3;
373         }
374         else
375         {
376             double e;
377             d = sqrt(-d);
378             e = pow(d + fabs(R), 0.333333333333);
379             if( R > 0 )
380                 e = -e;
381             x0 = (e + Q / e) - a1 * (1./3);
382             n = 1;
383         }
384     }
385
386     step = 1;
387
388     if( CV_MAT_TYPE(roots->type) == CV_32FC1 )
389     {
390         float* r = roots->data.fl;
391         if( roots->rows > 1 )
392             step = roots->step/sizeof(r[0]);
393         r[0] = (float)x0;
394         r[step] = (float)x1;
395         r[step*2] = (float)x2;
396     }
397     else
398     {
399         double* r = roots->data.db;
400         if( roots->rows > 1 )
401             step = roots->step/sizeof(r[0]);
402         r[0] = x0;
403         r[step] = x1;
404         r[step*2] = x2;
405     }
406
407     __END__;
408
409     return n;
410 }
411
412
413 CV_IMPL void cvNormalize( const CvArr* src, CvArr* dst,
414                           double a, double b, int norm_type, const CvArr* mask )
415 {
416     CvMat* tmp = 0;
417
418     CV_FUNCNAME( "cvNormalize" );
419
420     __BEGIN__;
421
422     double scale, shift;
423     
424     if( norm_type == CV_MINMAX )
425     {
426         double smin = 0, smax = 0;
427         double dmin = MIN( a, b ), dmax = MAX( a, b );
428         cvMinMaxLoc( src, &smin, &smax, 0, 0, mask );
429         scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0);
430         shift = dmin - smin*scale;
431     }
432     else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
433     {
434         CvMat *s = (CvMat*)src, *d = (CvMat*)dst;
435         
436         if( CV_IS_MAT(s) && CV_IS_MAT(d) && CV_IS_MAT_CONT(s->type & d->type) &&
437             CV_ARE_TYPES_EQ(s,d) && CV_ARE_SIZES_EQ(s,d) && !mask &&
438             s->cols*s->rows <= CV_MAX_INLINE_MAT_OP_SIZE*CV_MAX_INLINE_MAT_OP_SIZE )
439         {
440             int i, len = s->cols*s->rows;
441             double norm = 0, v;
442
443             if( CV_MAT_TYPE(s->type) == CV_32FC1 )
444             {
445                 const float* sptr = s->data.fl;
446                 float* dptr = d->data.fl;
447                 
448                 if( norm_type == CV_L2 )
449                 {
450                     for( i = 0; i < len; i++ )
451                     {
452                         v = sptr[i];
453                         norm += v*v;
454                     }
455                     norm = sqrt(norm);
456                 }
457                 else if( norm_type == CV_L1 )
458                     for( i = 0; i < len; i++ )
459                     {
460                         v = fabs((double)sptr[i]);
461                         norm += v;
462                     }
463                 else
464                     for( i = 0; i < len; i++ )
465                     {
466                         v = fabs((double)sptr[i]);
467                         norm = MAX(norm,v);
468                     }
469
470                 norm = norm > DBL_EPSILON ? 1./norm : 0.;
471                 for( i = 0; i < len; i++ )
472                     dptr[i] = (float)(sptr[i]*norm);
473                 EXIT;
474             }
475
476             if( CV_MAT_TYPE(s->type) == CV_64FC1 )
477             {
478                 const double* sptr = s->data.db;
479                 double* dptr = d->data.db;
480                 
481                 if( norm_type == CV_L2 )
482                 {
483                     for( i = 0; i < len; i++ )
484                     {
485                         v = sptr[i];
486                         norm += v*v;
487                     }
488                     norm = sqrt(norm);
489                 }
490                 else if( norm_type == CV_L1 )
491                     for( i = 0; i < len; i++ )
492                     {
493                         v = fabs(sptr[i]);
494                         norm += v;
495                     }
496                 else
497                     for( i = 0; i < len; i++ )
498                     {
499                         v = fabs(sptr[i]);
500                         norm = MAX(norm,v);
501                     }
502
503                 norm = norm > DBL_EPSILON ? 1./norm : 0.;
504                 for( i = 0; i < len; i++ )
505                     dptr[i] = sptr[i]*norm;
506                 EXIT;
507             }
508         }
509         
510         scale = cvNorm( src, 0, norm_type, mask );
511         scale = scale > DBL_EPSILON ? 1./scale : 0.;
512         shift = 0;
513     }
514     else
515         CV_ERROR( CV_StsBadArg, "Unknown/unsupported norm type" );
516     
517     if( !mask )
518         cvConvertScale( src, dst, scale, shift );
519     else
520     {
521         CvMat stub, *dmat;
522         CV_CALL( dmat = cvGetMat(dst, &stub));
523         CV_CALL( tmp = cvCreateMat(dmat->rows, dmat->cols, dmat->type) );
524         cvConvertScale( src, tmp, scale, shift );
525         cvCopy( tmp, dst, mask );
526     }
527
528     __END__;
529
530     if( tmp )
531         cvReleaseMat( &tmp );
532 }
533
534
535 CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor )
536 {
537     CV_FUNCNAME( "cvRandShuffle" );
538
539     __BEGIN__;
540
541     const int sizeof_int = (int)sizeof(int);
542     CvMat stub, *mat = (CvMat*)arr;
543     int i, j, k, iters, delta = 0;
544     int cont_flag, arr_size, elem_size, cols, step;
545     const int pair_buf_sz = 100;
546     int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 );
547     CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf );
548     CvRNG _rng = cvRNG(-1);
549     uchar* data = 0;
550     int* idata = 0;
551     
552     if( !CV_IS_MAT(mat) )
553         CV_CALL( mat = cvGetMat( mat, &stub ));
554
555     if( !rng )
556         rng = &_rng;
557
558     cols = mat->cols;
559     step = mat->step;
560     arr_size = cols*mat->rows;
561     iters = cvRound(iter_factor*arr_size)*2;
562     cont_flag = CV_IS_MAT_CONT(mat->type);
563     elem_size = CV_ELEM_SIZE(mat->type);
564     if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) )
565     {
566         idata = mat->data.i;
567         step /= sizeof_int;
568         elem_size /= sizeof_int;
569     }
570     else
571         data = mat->data.ptr;
572
573     for( i = 0; i < iters; i += delta )
574     {
575         delta = MIN( iters - i, pair_buf_sz*2 );
576         _pair_buf.cols = delta;
577         cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) );
578         
579         if( cont_flag )
580         {
581             if( idata )
582                 for( j = 0; j < delta; j += 2 )
583                 {
584                     int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t;
585                     for( k = 0; k < elem_size; k++ )
586                         CV_SWAP( p[k], q[k], t );
587                 }
588             else
589                 for( j = 0; j < delta; j += 2 )
590                 {
591                     uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t;
592                     for( k = 0; k < elem_size; k++ )
593                         CV_SWAP( p[k], q[k], t );
594                 }
595         }
596         else
597         {
598             if( idata )
599                 for( j = 0; j < delta; j += 2 )
600                 {
601                     int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
602                     int* p, *q, t;
603                     row1 = idx1/step; row2 = idx2/step;
604                     p = idata + row1*step + (idx1 - row1*cols)*elem_size;
605                     q = idata + row2*step + (idx2 - row2*cols)*elem_size;
606                     
607                     for( k = 0; k < elem_size; k++ )
608                         CV_SWAP( p[k], q[k], t );
609                 }
610             else
611                 for( j = 0; j < delta; j += 2 )
612                 {
613                     int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
614                     uchar* p, *q, t;
615                     row1 = idx1/step; row2 = idx2/step;
616                     p = data + row1*step + (idx1 - row1*cols)*elem_size;
617                     q = data + row2*step + (idx2 - row2*cols)*elem_size;
618                     
619                     for( k = 0; k < elem_size; k++ )
620                         CV_SWAP( p[k], q[k], t );
621                 }
622         }
623     }
624
625     __END__;
626 }
627
628
629 CV_IMPL CvArr*
630 cvRange( CvArr* arr, double start, double end )
631 {
632     int ok = 0;
633     
634     CV_FUNCNAME( "cvRange" );
635
636     __BEGIN__;
637     
638     CvMat stub, *mat = (CvMat*)arr;
639     double delta;
640     int type, step;
641     double val = start;
642     int i, j;
643     int rows, cols;
644     
645     if( !CV_IS_MAT(mat) )
646         CV_CALL( mat = cvGetMat( mat, &stub) );
647
648     rows = mat->rows;
649     cols = mat->cols;
650     type = CV_MAT_TYPE(mat->type);
651     delta = (end-start)/(rows*cols);
652
653     if( CV_IS_MAT_CONT(mat->type) )
654     {
655         cols *= rows;
656         rows = 1;
657         step = 1;
658     }
659     else
660         step = mat->step / CV_ELEM_SIZE(type);
661
662     if( type == CV_32SC1 )
663     {
664         int* idata = mat->data.i;
665         int ival = cvRound(val), idelta = cvRound(delta);
666
667         if( fabs(val - ival) < DBL_EPSILON &&
668             fabs(delta - idelta) < DBL_EPSILON )
669         {
670             for( i = 0; i < rows; i++, idata += step )
671                 for( j = 0; j < cols; j++, ival += idelta )
672                     idata[j] = ival;
673         }
674         else
675         {
676             for( i = 0; i < rows; i++, idata += step )
677                 for( j = 0; j < cols; j++, val += delta )
678                     idata[j] = cvRound(val);
679         }
680     }
681     else if( type == CV_32FC1 )
682     {
683         float* fdata = mat->data.fl;
684         for( i = 0; i < rows; i++, fdata += step )
685             for( j = 0; j < cols; j++, val += delta )
686                 fdata[j] = (float)val;
687     }
688     else
689         CV_ERROR( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
690
691     ok = 1;
692
693     __END__;
694
695     return ok ? arr : 0;
696 }
697
698 /* End of file. */