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. */ 728
