git.maemo.org Git - opencv/blob - 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 /*
 414   Finds real and complex roots of polynomials of any degree with real
 415   coefficients. The original code has been taken from Ken Turkowski's web
 416   page (http://www.worldserver.com/turk/opensource/) and adopted for OpenCV.
 417   Here is the copyright notice.
 418
 419   -----------------------------------------------------------------------
 420   Copyright (C) 1981-1999 Ken Turkowski. <turk@computer.org>
 421
 422   All rights reserved.
 423
 424   Warranty Information
 425   Even though I have reviewed this software, I make no warranty
 426   or representation, either express or implied, with respect to this
 427   software, its quality, accuracy, merchantability, or fitness for a
 428   particular purpose.  As a result, this software is provided "as is,"
 429   and you, its user, are assuming the entire risk as to its quality
 430   and accuracy.
 431
 432   This code may be used and freely distributed as long as it includes
 433   this copyright notice and the above warranty information.
 434 */
 435
 436
 437 /*******************************************************************************
 438  * FindPolynomialRoots
 439  *
 440  * The Bairstow and Newton correction formulae are used for a simultaneous
 441  * linear and quadratic iterated synthetic division.  The coefficients of
 442  * a polynomial of degree n are given as a[i] (i=0,i,..., n) where a[0] is
 443  * the constant term.  The coefficients are scaled by dividing them by
 444  * their geometric mean.  The Bairstow or Newton iteration method will
 445  * nearly always converge to the number of figures carried, fig, either to
 446  * root values or to their reciprocals.  If the simultaneous Newton and
 447  * Bairstow iteration fails to converge on root values or their
 448  * reciprocals in maxiter iterations, the convergence requirement will be
 449  * successively reduced by one decimal figure.  This program anticipates
 450  * and protects against loss of significance in the quadratic synthetic
 451  * division.  (Refer to "On Programming the Numerical Solution of
 452  * Polynomial Equations," by K. W. Ellenberger, Commun. ACM 3 (Dec. 1960),
 453  * 644-647.)  The real and imaginary part of each root is stated as u[i]
 454  * and v[i], respectively.
 455  *
 456  * ACM algorithm #30 - Numerical Solution of the Polynomial Equation
 457  * K. W. Ellenberger
 458  * Missle Division, North American Aviation, Downey, California
 459  * Converted to C, modified, optimized, and structured by
 460  * Ken Turkowski
 461  * CADLINC, Inc., Palo Alto, California
 462  *******************************************************************************/
 463
 464 #define MAXN 16
 465
 466 template <class T>
 467 static void icvFindPolynomialRoots(const T *a, T *u, int n, int maxiter, int fig) {
 468   int i;
 469   int j;
 470   T h[MAXN + 3], b[MAXN + 3], c[MAXN + 3], d[MAXN + 3], e[MAXN + 3];
 471   // [-2 : n]
 472   T K, ps, qs, pt, qt, s, rev, r;
 473   int t;
 474   T p, q, qq;
 475
 476   // Zero elements with negative indices
 477   b[2 + -1] = b[2 + -2] =
 478     c[2 + -1] = c[2 + -2] =
 479     d[2 + -1] = d[2 + -2] =
 480     e[2 + -1] = e[2 + -2] =
 481     h[2 + -1] = h[2 + -2] = 0.0;
 482
 483   // Copy polynomial coefficients to working storage
 484   for (j = n; j >= 0; j--)
 485     h[2 + j] = *a++; // Note reversal of coefficients
 486
 487   t = 1;
 488   K = pow(10.0, (double)(fig)); // Relative accuracy
 489
 490   for (; h[2 + n] == 0.0; n--) { // Look for zero high-order coeff.
 491     *u++ = 0.0;
 492     *u++ = 0.0;
 493   }
 494
 495  INIT:
 496   if (n == 0)
 497     return;
 498
 499   ps = qs = pt = qt = s = 0.0;
 500   rev = 1.0;
 501   K = pow(10.0, (double)(fig));
 502
 503   if (n == 1) {
 504     r = -h[2 + 1] / h[2 + 0];
 505     goto LINEAR;
 506   }
 507
 508   for (j = n; j >= 0; j--) // Find geometric mean of coeff's
 509     if (h[2 + j] != 0.0)
 510       s += log(fabs(h[2 + j]));
 511   s = exp(s / (n + 1));
 512
 513   for (j = n; j >= 0; j--) // Normalize coeff's by mean
 514     h[2 + j] /= s;
 515
 516   if (fabs(h[2 + 1] / h[2 + 0]) < fabs(h[2 + n - 1] / h[2 + n])) {
 517   REVERSE:
 518     t = -t;
 519     for (j = (n - 1) / 2; j >= 0; j--) {
 520       s = h[2 + j];
 521       h[2 + j] = h[2 + n - j];
 522       h[2 + n - j] = s;
 523     }
 524   }
 525   if (qs != 0.0) {
 526     p = ps;
 527     q = qs;
 528   } else {
 529     if (h[2 + n - 2] == 0.0) {
 530       q = 1.0;
 531       p = -2.0;
 532     } else {
 533       q = h[2 + n] / h[2 + n - 2];
 534       p = (h[2 + n - 1] - q * h[2 + n - 3]) / h[2 + n - 2];
 535     }
 536     if (n == 2)
 537       goto QADRTIC;
 538     r = 0.0;
 539   }
 540  ITERATE:
 541   for (i = maxiter; i > 0; i--) {
 542
 543     for (j = 0; j <= n; j++) { // BAIRSTOW
 544       b[2 + j] = h[2 + j] - p * b[2 + j - 1] - q * b[2 + j - 2];
 545       c[2 + j] = b[2 + j] - p * c[2 + j - 1] - q * c[2 + j - 2];
 546     }
 547     if ((h[2 + n - 1] != 0.0) && (b[2 + n - 1] != 0.0)) {
 548       if (fabs(h[2 + n - 1] / b[2 + n - 1]) >= K) {
 549         b[2 + n] = h[2 + n] - q * b[2 + n - 2];
 550       }
 551       if (b[2 + n] == 0.0)
 552         goto QADRTIC;
 553       if (K < fabs(h[2 + n] / b[2 + n]))
 554         goto QADRTIC;
 555     }
 556
 557     for (j = 0; j <= n; j++) { // NEWTON
 558       d[2 + j] = h[2 + j] + r * d[2 + j - 1]; // Calculate polynomial at r
 559       e[2 + j] = d[2 + j] + r * e[2 + j - 1]; // Calculate derivative at r
 560     }
 561     if (d[2 + n] == 0.0)
 562       goto LINEAR;
 563     if (K < fabs(h[2 + n] / d[2 + n]))
 564       goto LINEAR;
 565
 566     c[2 + n - 1] = -p * c[2 + n - 2] - q * c[2 + n - 3];
 567     s = c[2 + n - 2] * c[2 + n - 2] - c[2 + n - 1] * c[2 + n - 3];
 568     if (s == 0.0) {
 569       p -= 2.0;
 570       q *= (q + 1.0);
 571     } else {
 572       p += (b[2 + n - 1] * c[2 + n - 2] - b[2 + n] * c[2 + n - 3]) / s;
 573       q += (-b[2 + n - 1] * c[2 + n - 1] + b[2 + n] * c[2 + n - 2]) / s;
 574     }
 575     if (e[2 + n - 1] == 0.0)
 576       r -= 1.0; // Minimum step
 577     else
 578       r -= d[2 + n] / e[2 + n - 1]; // Newton's iteration
 579   }
 580   ps = pt;
 581   qs = qt;
 582   pt = p;
 583   qt = q;
 584   if (rev < 0.0)
 585     K /= 10.0;
 586   rev = -rev;
 587   goto REVERSE;
 588
 589  LINEAR:
 590   if (t < 0)
 591     r = 1.0 / r;
 592   n--;
 593   *u++ = r;
 594   *u++ = 0.0;
 595
 596   for (j = n; j >= 0; j--) { // Polynomial deflation by lin-nomial
 597     if ((d[2 + j] != 0.0) && (fabs(h[2 + j] / d[2 + j]) < K))
 598       h[2 + j] = d[2 + j];
 599     else
 600       h[2 + j] = 0.0;
 601   }
 602
 603   if (n == 0)
 604     return;
 605   goto ITERATE;
 606
 607  QADRTIC:
 608   if (t < 0) {
 609     p /= q;
 610     q = 1.0 / q;
 611   }
 612   n -= 2;
 613
 614   if (0.0 < (q - (p * p / 4.0))) { // Two complex roots
 615     s = sqrt(q - (p * p / 4.0));
 616     *u++ = -p / 2.0;
 617     *u++ = s;
 618     *u++ = -p / 2.0;
 619     *u++ = -s;
 620   } else { // Two real roots
 621     s = sqrt(((p * p / 4.0)) - q);
 622     if (p < 0.0)
 623       *u++ = qq = -p / 2.0 + s;
 624     else
 625       *u++ = qq = -p / 2.0 - s;
 626     *u++ = 0.0;
 627     *u++ = q / qq;
 628     *u++ = 0.0;
 629   }
 630
 631   for (j = n; j >= 0; j--) { // Polynomial deflation by quadratic
 632     if ((b[2 + j] != 0.0) && (fabs(h[2 + j] / b[2 + j]) < K))
 633       h[2 + j] = b[2 + j];
 634     else
 635       h[2 + j] = 0.0;
 636   }
 637   goto INIT;
 638 }
 639
 640 #undef MAXN
 641
 642 void cvSolvePoly(const CvMat* a, CvMat *r, int maxiter, int fig) {
 643   int m = a->rows * a->cols;
 644   int n = r->rows * r->cols;
 645
 646   __BEGIN__;
 647   CV_FUNCNAME("cvSolvePoly");
 648
 649   if (CV_MAT_TYPE(a->type) != CV_32FC1 &&
 650       CV_MAT_TYPE(a->type) != CV_64FC1)
 651     CV_ERROR(CV_StsUnsupportedFormat, "coeffs must be either CV_32FC1 or CV_64FC1");
 652   if (CV_MAT_TYPE(r->type) != CV_32FC2 &&
 653       CV_MAT_TYPE(r->type) != CV_64FC2)
 654     CV_ERROR(CV_StsUnsupportedFormat, "roots must be either CV_32FC2 or CV_64FC2");
 655   if (CV_MAT_DEPTH(a->type) != CV_MAT_DEPTH(r->type))
 656     CV_ERROR(CV_StsUnmatchedFormats, "coeffs and roots must have same depth");
 657
 658   if (m - 1 != n)
 659     CV_ERROR(CV_StsUnmatchedFormats, "must have n + 1 coefficients");
 660
 661   switch (CV_MAT_DEPTH(a->type)) {
 662   case CV_32F:
 663     icvFindPolynomialRoots(a->data.fl, r->data.fl, n, maxiter, fig);
 664     break;
 665   case CV_64F:
 666     icvFindPolynomialRoots(a->data.db, r->data.db, n, maxiter, fig);
 667     break;
 668   }
 669
 670   __END__;
 671 }
 672
 673
 674 CV_IMPL void cvNormalize( const CvArr* src, CvArr* dst,
 675                           double a, double b, int norm_type, const CvArr* mask )
 676 {
 677     CvMat* tmp = 0;
 678
 679     CV_FUNCNAME( "cvNormalize" );
 680
 681     __BEGIN__;
 682
 683     double scale, shift;
 684
 685     if( norm_type == CV_MINMAX )
 686     {
 687         double smin = 0, smax = 0;
 688         double dmin = MIN( a, b ), dmax = MAX( a, b );
 689         cvMinMaxLoc( src, &smin, &smax, 0, 0, mask );
 690         scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0);
 691         shift = dmin - smin*scale;
 692     }
 693     else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
 694     {
 695         CvMat *s = (CvMat*)src, *d = (CvMat*)dst;
 696
 697         if( CV_IS_MAT(s) && CV_IS_MAT(d) && CV_IS_MAT_CONT(s->type & d->type) &&
 698             CV_ARE_TYPES_EQ(s,d) && CV_ARE_SIZES_EQ(s,d) && !mask &&
 699             s->cols*s->rows <= CV_MAX_INLINE_MAT_OP_SIZE*CV_MAX_INLINE_MAT_OP_SIZE )
 700         {
 701             int i, len = s->cols*s->rows;
 702             double norm = 0, v;
 703
 704             if( CV_MAT_TYPE(s->type) == CV_32FC1 )
 705             {
 706                 const float* sptr = s->data.fl;
 707                 float* dptr = d->data.fl;
 708
 709                 if( norm_type == CV_L2 )
 710                 {
 711                     for( i = 0; i < len; i++ )
 712                     {
 713                         v = sptr[i];
 714                         norm += v*v;
 715                     }
 716                     norm = sqrt(norm);
 717                 }
 718                 else if( norm_type == CV_L1 )
 719                     for( i = 0; i < len; i++ )
 720                     {
 721                         v = fabs((double)sptr[i]);
 722                         norm += v;
 723                     }
 724                 else
 725                     for( i = 0; i < len; i++ )
 726                     {
 727                         v = fabs((double)sptr[i]);
 728                         norm = MAX(norm,v);
 729                     }
 730
 731                 norm = norm > DBL_EPSILON ? 1./norm : 0.;
 732                 for( i = 0; i < len; i++ )
 733                     dptr[i] = (float)(sptr[i]*norm);
 734                 EXIT;
 735             }
 736
 737             if( CV_MAT_TYPE(s->type) == CV_64FC1 )
 738             {
 739                 const double* sptr = s->data.db;
 740                 double* dptr = d->data.db;
 741
 742                 if( norm_type == CV_L2 )
 743                 {
 744                     for( i = 0; i < len; i++ )
 745                     {
 746                         v = sptr[i];
 747                         norm += v*v;
 748                     }
 749                     norm = sqrt(norm);
 750                 }
 751                 else if( norm_type == CV_L1 )
 752                     for( i = 0; i < len; i++ )
 753                     {
 754                         v = fabs(sptr[i]);
 755                         norm += v;
 756                     }
 757                 else
 758                     for( i = 0; i < len; i++ )
 759                     {
 760                         v = fabs(sptr[i]);
 761                         norm = MAX(norm,v);
 762                     }
 763
 764                 norm = norm > DBL_EPSILON ? 1./norm : 0.;
 765                 for( i = 0; i < len; i++ )
 766                     dptr[i] = sptr[i]*norm;
 767                 EXIT;
 768             }
 769         }
 770
 771         scale = cvNorm( src, 0, norm_type, mask );
 772         scale = scale > DBL_EPSILON ? 1./scale : 0.;
 773         shift = 0;
 774     }
 775     else
 776         CV_ERROR( CV_StsBadArg, "Unknown/unsupported norm type" );
 777
 778     if( !mask )
 779         cvConvertScale( src, dst, scale, shift );
 780     else
 781     {
 782         CvMat stub, *dmat;
 783         CV_CALL( dmat = cvGetMat(dst, &stub));
 784         CV_CALL( tmp = cvCreateMat(dmat->rows, dmat->cols, dmat->type) );
 785         cvConvertScale( src, tmp, scale, shift );
 786         cvCopy( tmp, dst, mask );
 787     }
 788
 789     __END__;
 790
 791     if( tmp )
 792         cvReleaseMat( &tmp );
 793 }
 794
 795
 796 CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor )
 797 {
 798     CV_FUNCNAME( "cvRandShuffle" );
 799
 800     __BEGIN__;
 801
 802     const int sizeof_int = (int)sizeof(int);
 803     CvMat stub, *mat = (CvMat*)arr;
 804     int i, j, k, iters, delta = 0;
 805     int cont_flag, arr_size, elem_size, cols, step;
 806     const int pair_buf_sz = 100;
 807     int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 );
 808     CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf );
 809     CvRNG _rng = cvRNG(-1);
 810     uchar* data = 0;
 811     int* idata = 0;
 812
 813     if( !CV_IS_MAT(mat) )
 814         CV_CALL( mat = cvGetMat( mat, &stub ));
 815
 816     if( !rng )
 817         rng = &_rng;
 818
 819     cols = mat->cols;
 820     step = mat->step;
 821     arr_size = cols*mat->rows;
 822     iters = cvRound(iter_factor*arr_size)*2;
 823     cont_flag = CV_IS_MAT_CONT(mat->type);
 824     elem_size = CV_ELEM_SIZE(mat->type);
 825     if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) )
 826     {
 827         idata = mat->data.i;
 828         step /= sizeof_int;
 829         elem_size /= sizeof_int;
 830     }
 831     else
 832         data = mat->data.ptr;
 833
 834     for( i = 0; i < iters; i += delta )
 835     {
 836         delta = MIN( iters - i, pair_buf_sz*2 );
 837         _pair_buf.cols = delta;
 838         cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) );
 839
 840         if( cont_flag )
 841         {
 842             if( idata )
 843                 for( j = 0; j < delta; j += 2 )
 844                 {
 845                     int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t;
 846                     for( k = 0; k < elem_size; k++ )
 847                         CV_SWAP( p[k], q[k], t );
 848                 }
 849             else
 850                 for( j = 0; j < delta; j += 2 )
 851                 {
 852                     uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t;
 853                     for( k = 0; k < elem_size; k++ )
 854                         CV_SWAP( p[k], q[k], t );
 855                 }
 856         }
 857         else
 858         {
 859             if( idata )
 860                 for( j = 0; j < delta; j += 2 )
 861                 {
 862                     int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
 863                     int* p, *q, t;
 864                     row1 = idx1/step; row2 = idx2/step;
 865                     p = idata + row1*step + (idx1 - row1*cols)*elem_size;
 866                     q = idata + row2*step + (idx2 - row2*cols)*elem_size;
 867
 868                     for( k = 0; k < elem_size; k++ )
 869                         CV_SWAP( p[k], q[k], t );
 870                 }
 871             else
 872                 for( j = 0; j < delta; j += 2 )
 873                 {
 874                     int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
 875                     uchar* p, *q, t;
 876                     row1 = idx1/step; row2 = idx2/step;
 877                     p = data + row1*step + (idx1 - row1*cols)*elem_size;
 878                     q = data + row2*step + (idx2 - row2*cols)*elem_size;
 879
 880                     for( k = 0; k < elem_size; k++ )
 881                         CV_SWAP( p[k], q[k], t );
 882                 }
 883         }
 884     }
 885
 886     __END__;
 887 }
 888
 889
 890 CV_IMPL CvArr*
 891 cvRange( CvArr* arr, double start, double end )
 892 {
 893     int ok = 0;
 894
 895     CV_FUNCNAME( "cvRange" );
 896
 897     __BEGIN__;
 898
 899     CvMat stub, *mat = (CvMat*)arr;
 900     double delta;
 901     int type, step;
 902     double val = start;
 903     int i, j;
 904     int rows, cols;
 905
 906     if( !CV_IS_MAT(mat) )
 907         CV_CALL( mat = cvGetMat( mat, &stub) );
 908
 909     rows = mat->rows;
 910     cols = mat->cols;
 911     type = CV_MAT_TYPE(mat->type);
 912     delta = (end-start)/(rows*cols);
 913
 914     if( CV_IS_MAT_CONT(mat->type) )
 915     {
 916         cols *= rows;
 917         rows = 1;
 918         step = 1;
 919     }
 920     else
 921         step = mat->step / CV_ELEM_SIZE(type);
 922
 923     if( type == CV_32SC1 )
 924     {
 925         int* idata = mat->data.i;
 926         int ival = cvRound(val), idelta = cvRound(delta);
 927
 928         if( fabs(val - ival) < DBL_EPSILON &&
 929             fabs(delta - idelta) < DBL_EPSILON )
 930         {
 931             for( i = 0; i < rows; i++, idata += step )
 932                 for( j = 0; j < cols; j++, ival += idelta )
 933                     idata[j] = ival;
 934         }
 935         else
 936         {
 937             for( i = 0; i < rows; i++, idata += step )
 938                 for( j = 0; j < cols; j++, val += delta )
 939                     idata[j] = cvRound(val);
 940         }
 941     }
 942     else if( type == CV_32FC1 )
 943     {
 944         float* fdata = mat->data.fl;
 945         for( i = 0; i < rows; i++, fdata += step )
 946             for( j = 0; j < cols; j++, val += delta )
 947                 fdata[j] = (float)val;
 948     }
 949     else
 950         CV_ERROR( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
 951
 952     ok = 1;
 953
 954     __END__;
 955
 956     return ok ? arr : 0;
 957 }
 958
 959 /* End of file. */