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( ¢ers ); 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 (at) 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 (at) 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 static void icvFindPolynomialRoots(const double *a, double *u, int n, int maxiter, int fig) 467 { 468 int i; 469 int j; 470 double h[MAXN + 3], b[MAXN + 3], c[MAXN + 3], d[MAXN + 3], e[MAXN + 3]; 471 // [-2 : n] 472 double K, ps, qs, pt, qt, s, rev, r = 0; 473 int t; 474 double p = 0, q = 0, 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 { 644 __BEGIN__; 645 646 int m, n; 647 double *ad = 0, *rd = 0; 648 649 CV_FUNCNAME("cvSolvePoly"); 650 651 if (CV_MAT_TYPE(a->type) != CV_32FC1 && 652 CV_MAT_TYPE(a->type) != CV_64FC1) 653 CV_ERROR(CV_StsUnsupportedFormat, "coeffs must be either CV_32FC1 or CV_64FC1"); 654 if (CV_MAT_TYPE(r->type) != CV_32FC2 && 655 CV_MAT_TYPE(r->type) != CV_64FC2) 656 CV_ERROR(CV_StsUnsupportedFormat, "roots must be either CV_32FC2 or CV_64FC2"); 657 m = a->rows * a->cols; 658 n = r->rows * r->cols; 659 660 if (m - 1 != n) 661 CV_ERROR(CV_StsUnmatchedFormats, "must have n + 1 coefficients"); 662 663 if( CV_MAT_TYPE(a->type) == CV_32F || !CV_IS_MAT_CONT(a->type)) 664 { 665 ad = (double*)cvStackAlloc(m*sizeof(ad[0])); 666 CvMat _a = cvMat( a->rows, a->cols, CV_64F, ad ); 667 cvConvert( a, &_a ); 668 } 669 else 670 ad = a->data.db; 671 672 if( CV_MAT_TYPE(r->type) == CV_32F || !CV_IS_MAT_CONT(r->type)) 673 rd = (double*)cvStackAlloc(n*sizeof(ad[0])); 674 else 675 rd = r->data.db; 676 677 icvFindPolynomialRoots( ad, rd, n, maxiter, fig); 678 if( rd != r->data.db ) 679 { 680 CvMat _r = cvMat( r->rows, r->cols, CV_64F, rd ); 681 cvConvert( &_r, r ); 682 } 683 684 __END__; 685 } 686 687 688 CV_IMPL void cvNormalize( const CvArr* src, CvArr* dst, 689 double a, double b, int norm_type, const CvArr* mask ) 690 { 691 CvMat* tmp = 0; 692 693 CV_FUNCNAME( "cvNormalize" ); 694 695 __BEGIN__; 696 697 double scale, shift; 698 699 if( norm_type == CV_MINMAX ) 700 { 701 double smin = 0, smax = 0; 702 double dmin = MIN( a, b ), dmax = MAX( a, b ); 703 cvMinMaxLoc( src, &smin, &smax, 0, 0, mask ); 704 scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0); 705 shift = dmin - smin*scale; 706 } 707 else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C ) 708 { 709 CvMat *s = (CvMat*)src, *d = (CvMat*)dst; 710 711 if( CV_IS_MAT(s) && CV_IS_MAT(d) && CV_IS_MAT_CONT(s->type & d->type) && 712 CV_ARE_TYPES_EQ(s,d) && CV_ARE_SIZES_EQ(s,d) && !mask && 713 s->cols*s->rows <= CV_MAX_INLINE_MAT_OP_SIZE*CV_MAX_INLINE_MAT_OP_SIZE ) 714 { 715 int i, len = s->cols*s->rows; 716 double norm = 0, v; 717 718 if( CV_MAT_TYPE(s->type) == CV_32FC1 ) 719 { 720 const float* sptr = s->data.fl; 721 float* dptr = d->data.fl; 722 723 if( norm_type == CV_L2 ) 724 { 725 for( i = 0; i < len; i++ ) 726 { 727 v = sptr[i]; 728 norm += v*v; 729 } 730 norm = sqrt(norm); 731 } 732 else if( norm_type == CV_L1 ) 733 for( i = 0; i < len; i++ ) 734 { 735 v = fabs((double)sptr[i]); 736 norm += v; 737 } 738 else 739 for( i = 0; i < len; i++ ) 740 { 741 v = fabs((double)sptr[i]); 742 norm = MAX(norm,v); 743 } 744 745 norm = norm > DBL_EPSILON ? 1./norm : 0.; 746 for( i = 0; i < len; i++ ) 747 dptr[i] = (float)(sptr[i]*norm); 748 EXIT; 749 } 750 751 if( CV_MAT_TYPE(s->type) == CV_64FC1 ) 752 { 753 const double* sptr = s->data.db; 754 double* dptr = d->data.db; 755 756 if( norm_type == CV_L2 ) 757 { 758 for( i = 0; i < len; i++ ) 759 { 760 v = sptr[i]; 761 norm += v*v; 762 } 763 norm = sqrt(norm); 764 } 765 else if( norm_type == CV_L1 ) 766 for( i = 0; i < len; i++ ) 767 { 768 v = fabs(sptr[i]); 769 norm += v; 770 } 771 else 772 for( i = 0; i < len; i++ ) 773 { 774 v = fabs(sptr[i]); 775 norm = MAX(norm,v); 776 } 777 778 norm = norm > DBL_EPSILON ? 1./norm : 0.; 779 for( i = 0; i < len; i++ ) 780 dptr[i] = sptr[i]*norm; 781 EXIT; 782 } 783 } 784 785 scale = cvNorm( src, 0, norm_type, mask ); 786 scale = scale > DBL_EPSILON ? 1./scale : 0.; 787 shift = 0; 788 } 789 else 790 CV_ERROR( CV_StsBadArg, "Unknown/unsupported norm type" ); 791 792 if( !mask ) 793 cvConvertScale( src, dst, scale, shift ); 794 else 795 { 796 CvMat stub, *dmat; 797 CV_CALL( dmat = cvGetMat(dst, &stub)); 798 CV_CALL( tmp = cvCreateMat(dmat->rows, dmat->cols, dmat->type) ); 799 cvConvertScale( src, tmp, scale, shift ); 800 cvCopy( tmp, dst, mask ); 801 } 802 803 __END__; 804 805 if( tmp ) 806 cvReleaseMat( &tmp ); 807 } 808 809 810 CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor ) 811 { 812 CV_FUNCNAME( "cvRandShuffle" ); 813 814 __BEGIN__; 815 816 const int sizeof_int = (int)sizeof(int); 817 CvMat stub, *mat = (CvMat*)arr; 818 int i, j, k, iters, delta = 0; 819 int cont_flag, arr_size, elem_size, cols, step; 820 const int pair_buf_sz = 100; 821 int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 ); 822 CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf ); 823 CvRNG _rng = cvRNG(-1); 824 uchar* data = 0; 825 int* idata = 0; 826 827 if( !CV_IS_MAT(mat) ) 828 CV_CALL( mat = cvGetMat( mat, &stub )); 829 830 if( !rng ) 831 rng = &_rng; 832 833 cols = mat->cols; 834 step = mat->step; 835 arr_size = cols*mat->rows; 836 iters = cvRound(iter_factor*arr_size)*2; 837 cont_flag = CV_IS_MAT_CONT(mat->type); 838 elem_size = CV_ELEM_SIZE(mat->type); 839 if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) ) 840 { 841 idata = mat->data.i; 842 step /= sizeof_int; 843 elem_size /= sizeof_int; 844 } 845 else 846 data = mat->data.ptr; 847 848 for( i = 0; i < iters; i += delta ) 849 { 850 delta = MIN( iters - i, pair_buf_sz*2 ); 851 _pair_buf.cols = delta; 852 cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) ); 853 854 if( cont_flag ) 855 { 856 if( idata ) 857 for( j = 0; j < delta; j += 2 ) 858 { 859 int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t; 860 for( k = 0; k < elem_size; k++ ) 861 CV_SWAP( p[k], q[k], t ); 862 } 863 else 864 for( j = 0; j < delta; j += 2 ) 865 { 866 uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t; 867 for( k = 0; k < elem_size; k++ ) 868 CV_SWAP( p[k], q[k], t ); 869 } 870 } 871 else 872 { 873 if( idata ) 874 for( j = 0; j < delta; j += 2 ) 875 { 876 int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2; 877 int* p, *q, t; 878 row1 = idx1/step; row2 = idx2/step; 879 p = idata + row1*step + (idx1 - row1*cols)*elem_size; 880 q = idata + row2*step + (idx2 - row2*cols)*elem_size; 881 882 for( k = 0; k < elem_size; k++ ) 883 CV_SWAP( p[k], q[k], t ); 884 } 885 else 886 for( j = 0; j < delta; j += 2 ) 887 { 888 int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2; 889 uchar* p, *q, t; 890 row1 = idx1/step; row2 = idx2/step; 891 p = data + row1*step + (idx1 - row1*cols)*elem_size; 892 q = data + row2*step + (idx2 - row2*cols)*elem_size; 893 894 for( k = 0; k < elem_size; k++ ) 895 CV_SWAP( p[k], q[k], t ); 896 } 897 } 898 } 899 900 __END__; 901 } 902 903 904 CV_IMPL CvArr* 905 cvRange( CvArr* arr, double start, double end ) 906 { 907 int ok = 0; 908 909 CV_FUNCNAME( "cvRange" ); 910 911 __BEGIN__; 912 913 CvMat stub, *mat = (CvMat*)arr; 914 double delta; 915 int type, step; 916 double val = start; 917 int i, j; 918 int rows, cols; 919 920 if( !CV_IS_MAT(mat) ) 921 CV_CALL( mat = cvGetMat( mat, &stub) ); 922 923 rows = mat->rows; 924 cols = mat->cols; 925 type = CV_MAT_TYPE(mat->type); 926 delta = (end-start)/(rows*cols); 927 928 if( CV_IS_MAT_CONT(mat->type) ) 929 { 930 cols *= rows; 931 rows = 1; 932 step = 1; 933 } 934 else 935 step = mat->step / CV_ELEM_SIZE(type); 936 937 if( type == CV_32SC1 ) 938 { 939 int* idata = mat->data.i; 940 int ival = cvRound(val), idelta = cvRound(delta); 941 942 if( fabs(val - ival) < DBL_EPSILON && 943 fabs(delta - idelta) < DBL_EPSILON ) 944 { 945 for( i = 0; i < rows; i++, idata += step ) 946 for( j = 0; j < cols; j++, ival += idelta ) 947 idata[j] = ival; 948 } 949 else 950 { 951 for( i = 0; i < rows; i++, idata += step ) 952 for( j = 0; j < cols; j++, val += delta ) 953 idata[j] = cvRound(val); 954 } 955 } 956 else if( type == CV_32FC1 ) 957 { 958 float* fdata = mat->data.fl; 959 for( i = 0; i < rows; i++, fdata += step ) 960 for( j = 0; j < cols; j++, val += delta ) 961 fdata[j] = (float)val; 962 } 963 else 964 CV_ERROR( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" ); 965 966 ok = 1; 967 968 __END__; 969 970 return ok ? arr : 0; 971 } 972 973 974 #define ICV_LT_BY_IDX(a, b) (aux[a] < aux[b]) 975 976 static CV_IMPLEMENT_QSORT_EX( icvSortIdx64f, int, ICV_LT_BY_IDX, const double* ) 977 static CV_IMPLEMENT_QSORT_EX( icvSortIdx32f, int, ICV_LT_BY_IDX, const float* ) 978 static CV_IMPLEMENT_QSORT_EX( icvSortIdx32s, int, ICV_LT_BY_IDX, const int* ) 979 static CV_IMPLEMENT_QSORT_EX( icvSortIdx16u, int, ICV_LT_BY_IDX, const ushort* ) 980 static CV_IMPLEMENT_QSORT_EX( icvSortIdx16s, int, ICV_LT_BY_IDX, const short* ) 981 static CV_IMPLEMENT_QSORT_EX( icvSortIdx8u, int, ICV_LT_BY_IDX, const uchar* ) 982 static CV_IMPLEMENT_QSORT_EX( icvSortIdx8s, int, ICV_LT_BY_IDX, const schar* ) 983 984 static CV_IMPLEMENT_QSORT_EX( icvSort64f, double, CV_LT, int ) 985 static CV_IMPLEMENT_QSORT_EX( icvSort32f, float, CV_LT, int ) 986 static CV_IMPLEMENT_QSORT_EX( icvSort32s, int, CV_LT, int ) 987 static CV_IMPLEMENT_QSORT_EX( icvSort16u, ushort, CV_LT, int ) 988 static CV_IMPLEMENT_QSORT_EX( icvSort16s, short, CV_LT, int ) 989 static CV_IMPLEMENT_QSORT_EX( icvSort8u, uchar, CV_LT, int ) 990 static CV_IMPLEMENT_QSORT_EX( icvSort8s, schar, CV_LT, int ) 991 992 typedef void (*CvSortFunc)( void* arr, size_t n, int ); 993 typedef void (*CvSortIdxFunc)( int* arr, size_t n, const void* ); 994 995 static inline void 996 icvCopy1D( const void* src, int s, void* dst, int d, int n, int elemSize ) 997 { 998 int i; 999 switch( elemSize ) 1000 { 1001 case 1: 1002 for( i = 0; i < n; i++ ) 1003 ((uchar*)dst)[i*d] = ((uchar*)src)[i*s]; 1004 break; 1005 case 2: 1006 for( i = 0; i < n; i++ ) 1007 ((ushort*)dst)[i*d] = ((ushort*)src)[i*s]; 1008 break; 1009 case 4: 1010 for( i = 0; i < n; i++ ) 1011 ((int*)dst)[i*d] = ((int*)src)[i*s]; 1012 break; 1013 case 8: 1014 for( i = 0; i < n; i++ ) 1015 ((int64*)dst)[i*d] = ((int64*)src)[i*s]; 1016 break; 1017 default: 1018 assert(0); 1019 } 1020 } 1021 1022 static void 1023 icvShuffle1D( const uchar* src, const int* idx, uchar* dst, int d, int n, int elemSize ) 1024 { 1025 int i; 1026 switch( elemSize ) 1027 { 1028 case 1: 1029 for( i = 0; i < n; i++ ) 1030 dst[i*d] = src[idx[i]]; 1031 break; 1032 case 2: 1033 for( i = 0; i < n; i++ ) 1034 ((ushort*)dst)[i*d] = ((ushort*)src)[idx[i]]; 1035 break; 1036 case 4: 1037 for( i = 0; i < n; i++ ) 1038 ((int*)dst)[i*d] = ((int*)src)[idx[i]]; 1039 break; 1040 case 8: 1041 for( i = 0; i < n; i++ ) 1042 ((int64*)dst)[i*d] = ((int64*)src)[idx[i]]; 1043 break; 1044 default: 1045 assert(0); 1046 } 1047 } 1048 1049 1050 CV_IMPL void 1051 cvSort( const CvArr* _src, CvArr* _dst, CvArr* _idx, int flags ) 1052 { 1053 uchar *tsrc = 0; 1054 int* tidx = 0; 1055 1056 CV_FUNCNAME("cvSort"); 1057 1058 __BEGIN__; 1059 1060 CvMat sstub, *src = cvGetMat(_src, &sstub); 1061 CvMat dstub, *dst = _dst ? cvGetMat(_dst, &dstub) : 0; 1062 CvMat istub, *idx = _idx ? cvGetMat(_idx, &istub) : 0; 1063 int type = CV_MAT_TYPE(src->type), elemSize = CV_ELEM_SIZE(type); 1064 int sstep = src->step, dstep = dst ? dst->step : 0; 1065 int istep = idx ? idx->step/sizeof(int) : 0; 1066 int i, len = src->cols; 1067 CvSortFunc sortFunc = 0; 1068 CvSortIdxFunc sortIdxFunc = 0; 1069 1070 if( CV_MAT_CN( src->type ) != 1 ) 1071 CV_ERROR( CV_StsUnsupportedFormat, "The input matrix should be a one-channel matrix." ); 1072 if( idx ) 1073 { 1074 if( CV_MAT_TYPE( idx->type ) != CV_32SC1) 1075 CV_ERROR( CV_StsUnsupportedFormat, "The index matrix must be CV_32SC1." ); 1076 1077 if( !CV_ARE_SIZES_EQ( idx, src )) 1078 CV_ERROR( CV_StsUnmatchedSizes, "The input matrix and index matrix must be of the same size" ); 1079 } 1080 1081 if( dst ) 1082 { 1083 if( !CV_ARE_TYPES_EQ(src, dst) ) 1084 CV_ERROR( CV_StsUnmatchedFormats, "The input and output matrix must be of the same type" ); 1085 1086 if( !CV_ARE_SIZES_EQ(src, dst) ) 1087 CV_ERROR( CV_StsUnmatchedSizes, "The input and output matrix must be of the same size" ); 1088 } 1089 1090 if( !idx && !dst ) 1091 CV_ERROR( CV_StsNullPtr, "At least one of index array or destination array must be non-NULL" ); 1092 1093 if( type == CV_8U ) 1094 sortIdxFunc = (CvSortIdxFunc)icvSortIdx8u, sortFunc = (CvSortFunc)icvSort8u; 1095 else if( type == CV_8S ) 1096 sortIdxFunc = (CvSortIdxFunc)icvSortIdx8s, sortFunc = (CvSortFunc)icvSort8s; 1097 else if( type == CV_16U ) 1098 sortIdxFunc = (CvSortIdxFunc)icvSortIdx16u, sortFunc = (CvSortFunc)icvSort16u; 1099 else if( type == CV_16S ) 1100 sortIdxFunc = (CvSortIdxFunc)icvSortIdx16s, sortFunc = (CvSortFunc)icvSort16s; 1101 else if( type == CV_32S ) 1102 sortIdxFunc = (CvSortIdxFunc)icvSortIdx32s, sortFunc = (CvSortFunc)icvSort32s; 1103 else if( type == CV_32F ) 1104 sortIdxFunc = (CvSortIdxFunc)icvSortIdx32f, sortFunc = (CvSortFunc)icvSort32f; 1105 else if( type == CV_64F ) 1106 sortIdxFunc = (CvSortIdxFunc)icvSortIdx64f, sortFunc = (CvSortFunc)icvSort64f; 1107 else 1108 CV_ERROR( CV_StsUnsupportedFormat, "Unsupported format of the input array" ); 1109 1110 // single-column case, where all of src, idx & dst arrays are continuous, is 1111 // equivalent to "sort every row" mode. 1112 if( (flags & CV_SORT_EVERY_COLUMN) && 1113 (src->cols > 1 || !CV_IS_MAT_CONT(src->type & 1114 (dst ? dst->type : -1) & (idx ? idx->type : -1))) ) 1115 { 1116 uchar* dptr = dst ? dst->data.ptr : 0; 1117 int* idxptr = idx ? idx->data.i : 0; 1118 1119 len = src->rows; 1120 tsrc = (uchar*)cvAlloc(len*elemSize); 1121 if( idx ) 1122 { 1123 tidx = (int*)cvAlloc(len*sizeof(tidx[0])); 1124 for( i = 0; i < len; i++ ) 1125 tidx[i] = i; 1126 } 1127 1128 if( flags & CV_SORT_DESCENDING ) 1129 { 1130 dptr += dstep*(src->rows - 1); 1131 dstep = -dstep; 1132 idxptr += istep*(src->rows - 1); 1133 istep = -istep; 1134 } 1135 1136 sstep /= elemSize; 1137 dstep /= elemSize; 1138 1139 for( i = 0; i < src->cols; i++ ) 1140 { 1141 icvCopy1D( src->data.ptr + i*elemSize, sstep, tsrc, 1, len, elemSize ); 1142 if( tidx ) 1143 { 1144 sortIdxFunc( tidx, len, tsrc ); 1145 if( dst ) 1146 icvShuffle1D( tsrc, tidx, dptr + i*elemSize, dstep, len, elemSize ); 1147 icvCopy1D( tidx, 1, idxptr + i, istep, len, sizeof(int) ); 1148 } 1149 else 1150 { 1151 sortFunc( tsrc, len, 0 ); 1152 icvCopy1D( tsrc, 1, dptr + i*elemSize, dstep, len, elemSize ); 1153 } 1154 } 1155 } 1156 else 1157 { 1158 int j, t, count = src->rows; 1159 if( flags & CV_SORT_EVERY_COLUMN ) 1160 CV_SWAP( len, count, t ); 1161 1162 if( (flags & CV_SORT_DESCENDING) || (idx && dst && dst->data.ptr == src->data.ptr) ) 1163 tsrc = (uchar*)cvAlloc(len*elemSize); 1164 1165 for( i = 0; i < count; i++ ) 1166 { 1167 if( !idx ) 1168 { 1169 const uchar* sptr = src->data.ptr + i*sstep; 1170 uchar* dptr = dst->data.ptr + i*dstep; 1171 uchar* ptr = flags & CV_SORT_DESCENDING ? tsrc : dptr; 1172 if( ptr != sptr ) 1173 icvCopy1D( sptr, 1, ptr, 1, len, elemSize ); 1174 sortFunc( ptr, len, 0 ); 1175 if( flags & CV_SORT_DESCENDING ) 1176 icvCopy1D( ptr + (len-1)*elemSize, -1, dptr, 1, len, elemSize ); 1177 } 1178 else 1179 { 1180 int* idx_ = idx->data.i + istep*i; 1181 const uchar* sptr = src->data.ptr + sstep*i; 1182 uchar* dptr = dst ? dst->data.ptr + dstep*i : 0; 1183 for( j = 0; j < len; j++ ) 1184 idx_[j] = j; 1185 if( dptr && dptr == sptr ) 1186 { 1187 icvCopy1D( sptr, 1, tsrc, 1, len, elemSize ); 1188 sptr = tsrc; 1189 } 1190 sortIdxFunc( idx_, len, sptr ); 1191 if( flags & CV_SORT_DESCENDING ) 1192 for( j = 0; j < len/2; j++ ) 1193 CV_SWAP(idx_[j], idx_[len-j-1], t); 1194 if( dptr ) 1195 icvShuffle1D( sptr, idx_, dptr, 1, len, elemSize ); 1196 } 1197 } 1198 } 1199 1200 __END__; 1201 1202 cvFree( &tsrc ); 1203 cvFree( &tidx ); 1204 } 1205 1206 /* End of file. */ 1207
