git.maemo.org Git - opencv/blob - cv/src/cvlinefit.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41 #include "_cv.h"
  42
  43 static const double eps = 1e-6;
  44
  45 static CvStatus
  46 icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line )
  47 {
  48     double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0;
  49     double dx2, dy2, dxy;
  50     int i;
  51     int count = _count;
  52     float t;
  53
  54     /* Calculating the average of x and y... */
  55
  56     if( weights == 0 )
  57     {
  58         for( i = 0; i < count; i += 1 )
  59         {
  60             x += points[i].x;
  61             y += points[i].y;
  62             x2 += points[i].x * points[i].x;
  63             y2 += points[i].y * points[i].y;
  64             xy += points[i].x * points[i].y;
  65         }
  66         w = (float) count;
  67     }
  68     else
  69     {
  70         for( i = 0; i < count; i += 1 )
  71         {
  72             x += weights[i] * points[i].x;
  73             y += weights[i] * points[i].y;
  74             x2 += weights[i] * points[i].x * points[i].x;
  75             y2 += weights[i] * points[i].y * points[i].y;
  76             xy += weights[i] * points[i].x * points[i].y;
  77             w += weights[i];
  78         }
  79     }
  80
  81     x /= w;
  82     y /= w;
  83     x2 /= w;
  84     y2 /= w;
  85     xy /= w;
  86
  87     dx2 = x2 - x * x;
  88     dy2 = y2 - y * y;
  89     dxy = xy - x * y;
  90
  91     t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
  92     line[0] = (float) cos( t );
  93     line[1] = (float) sin( t );
  94
  95     line[2] = (float) x;
  96     line[3] = (float) y;
  97
  98     return CV_NO_ERR;
  99 }
 100
 101 static CvStatus
 102 icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line )
 103 {
 104     int i;
 105     float w0 = 0;
 106     float x0 = 0, y0 = 0, z0 = 0;
 107     float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
 108     float dx2, dy2, dz2, dxy, dxz, dyz;
 109     float *v;
 110     float n;
 111     float det[9], evc[9], evl[3];
 112
 113     memset( evl, 0, 3*sizeof(evl[0]));
 114     memset( evc, 0, 9*sizeof(evl[0]));
 115
 116     if( weights )
 117     {
 118         for( i = 0; i < count; i++ )
 119         {
 120             float x = points[i].x;
 121             float y = points[i].y;
 122             float z = points[i].z;
 123             float w = weights[i];
 124
 125
 126             x2 += x * x * w;
 127             xy += x * y * w;
 128             xz += x * z * w;
 129             y2 += y * y * w;
 130             yz += y * z * w;
 131             z2 += z * z * w;
 132             x0 += x * w;
 133             y0 += y * w;
 134             z0 += z * w;
 135             w0 += w;
 136         }
 137     }
 138     else
 139     {
 140         for( i = 0; i < count; i++ )
 141         {
 142             float x = points[i].x;
 143             float y = points[i].y;
 144             float z = points[i].z;
 145
 146             x2 += x * x;
 147             xy += x * y;
 148             xz += x * z;
 149             y2 += y * y;
 150             yz += y * z;
 151             z2 += z * z;
 152             x0 += x;
 153             y0 += y;
 154             z0 += z;
 155         }
 156         w0 = (float) count;
 157     }
 158
 159     x2 /= w0;
 160     xy /= w0;
 161     xz /= w0;
 162     y2 /= w0;
 163     yz /= w0;
 164     z2 /= w0;
 165
 166     x0 /= w0;
 167     y0 /= w0;
 168     z0 /= w0;
 169
 170     dx2 = x2 - x0 * x0;
 171     dxy = xy - x0 * y0;
 172     dxz = xz - x0 * z0;
 173     dy2 = y2 - y0 * y0;
 174     dyz = yz - y0 * z0;
 175     dz2 = z2 - z0 * z0;
 176
 177     det[0] = dz2 + dy2;
 178     det[1] = -dxy;
 179     det[2] = -dxz;
 180     det[3] = det[1];
 181     det[4] = dx2 + dz2;
 182     det[5] = -dyz;
 183     det[6] = det[2];
 184     det[7] = det[5];
 185     det[8] = dy2 + dx2;
 186
 187     /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */
 188 #if 1
 189     {
 190     CvMat _det = cvMat( 3, 3, CV_32F, det );
 191     CvMat _evc = cvMat( 3, 3, CV_32F, evc );
 192     CvMat _evl = cvMat( 3, 1, CV_32F, evl );
 193     cvEigenVV( &_det, &_evc, &_evl, 0 );
 194     i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2);
 195     }
 196 #else
 197     {
 198         CvMat _det = cvMat( 3, 3, CV_32F, det );
 199         CvMat _evc = cvMat( 3, 3, CV_32F, evc );
 200         CvMat _evl = cvMat( 1, 3, CV_32F, evl );
 201
 202         cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T );
 203     }
 204     i = 2;
 205 #endif
 206     v = &evc[i * 3];
 207     n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] );
 208     n = (float)MAX(n, eps);
 209     line[0] = v[0] / n;
 210     line[1] = v[1] / n;
 211     line[2] = v[2] / n;
 212     line[3] = x0;
 213     line[4] = y0;
 214     line[5] = z0;
 215
 216     return CV_NO_ERR;
 217 }
 218
 219 static double
 220 icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist )
 221 {
 222     int j;
 223     float px = _line[2], py = _line[3];
 224     float nx = _line[1], ny = -_line[0];
 225     double sum_dist = 0.;
 226
 227     for( j = 0; j < count; j++ )
 228     {
 229         float x, y;
 230
 231         x = points[j].x - px;
 232         y = points[j].y - py;
 233
 234         dist[j] = (float) fabs( nx * x + ny * y );
 235         sum_dist += dist[j];
 236     }
 237
 238     return sum_dist;
 239 }
 240
 241 static double
 242 icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist )
 243 {
 244     int j;
 245     float px = _line[3], py = _line[4], pz = _line[5];
 246     float vx = _line[0], vy = _line[1], vz = _line[2];
 247     double sum_dist = 0.;
 248
 249     for( j = 0; j < count; j++ )
 250     {
 251         float x, y, z;
 252         double p1, p2, p3;
 253
 254         x = points[j].x - px;
 255         y = points[j].y - py;
 256         z = points[j].z - pz;
 257
 258         p1 = vy * z - vz * y;
 259         p2 = vz * x - vx * z;
 260         p3 = vx * y - vy * x;
 261
 262         dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 );
 263         sum_dist += dist[j];
 264     }
 265
 266     return sum_dist;
 267 }
 268
 269 static void
 270 icvWeightL1( float *d, int count, float *w )
 271 {
 272     int i;
 273
 274     for( i = 0; i < count; i++ )
 275     {
 276         double t = fabs( (double) d[i] );
 277         w[i] = (float)(1. / MAX(t, eps));
 278     }
 279 }
 280
 281 static void
 282 icvWeightL12( float *d, int count, float *w )
 283 {
 284     int i;
 285
 286     for( i = 0; i < count; i++ )
 287     {
 288         w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
 289     }
 290 }
 291
 292
 293 static void
 294 icvWeightHuber( float *d, int count, float *w, float _c )
 295 {
 296     int i;
 297     const float c = _c <= 0 ? 1.345f : _c;
 298
 299     for( i = 0; i < count; i++ )
 300     {
 301         if( d[i] < c )
 302             w[i] = 1.0f;
 303         else
 304             w[i] = c/d[i];
 305     }
 306 }
 307
 308
 309 static void
 310 icvWeightFair( float *d, int count, float *w, float _c )
 311 {
 312     int i;
 313     const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;
 314
 315     for( i = 0; i < count; i++ )
 316     {
 317         w[i] = 1 / (1 + d[i] * c);
 318     }
 319 }
 320
 321 static void
 322 icvWeightWelsch( float *d, int count, float *w, float _c )
 323 {
 324     int i;
 325     const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;
 326
 327     for( i = 0; i < count; i++ )
 328     {
 329         w[i] = (float) exp( -d[i] * d[i] * c * c );
 330     }
 331 }
 332
 333
 334 /* Takes an array of 2D points, type of distance (including user-defined
 335 distance specified by callbacks, fills the array of four floats with line
 336 parameters A, B, C, D, where (A, B) is the normalized direction vector,
 337 (C, D) is the point that belongs to the line. */
 338
 339 static CvStatus  icvFitLine2D( CvPoint2D32f * points, int count, int dist,
 340                                float _param, float reps, float aeps, float *line )
 341 {
 342     void (*calc_weights) (float *, int, float *) = 0;
 343     void (*calc_weights_param) (float *, int, float *, float) = 0;
 344     float *w;                   /* weights */
 345     float *r;                   /* square distances */
 346     int i, j;
 347     float _line[6], _lineprev[6];
 348     int first = 1;
 349     float rdelta = reps != 0 ? reps : 1.0f;
 350     float adelta = aeps != 0 ? aeps : 0.01f;
 351
 352     memset( line, 0, 4*sizeof(line[0]) );
 353
 354     switch (dist)
 355     {
 356     case CV_DIST_L2:
 357         return icvFitLine2D_wods( points, count, 0, line );
 358
 359     case CV_DIST_L1:
 360         calc_weights = icvWeightL1;
 361         break;
 362
 363     case CV_DIST_L12:
 364         calc_weights = icvWeightL12;
 365         break;
 366
 367     case CV_DIST_FAIR:
 368         calc_weights_param = icvWeightFair;
 369         break;
 370
 371     case CV_DIST_WELSCH:
 372         calc_weights_param = icvWeightWelsch;
 373         break;
 374
 375     case CV_DIST_HUBER:
 376         calc_weights_param = icvWeightHuber;
 377         break;
 378
 379     /*case CV_DIST_USER:
 380         calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
 381         break;*/
 382
 383     default:
 384         return CV_BADFACTOR_ERR;
 385     }
 386
 387     w = (float *) cvAlloc( count * sizeof( float ));
 388     r = (float *) cvAlloc( count * sizeof( float ));
 389
 390     for( i = 0; i < count; i++ )
 391         w[i] = 1.0f;
 392
 393     icvFitLine2D_wods( points, count, 0, _line );
 394     for( i = 0; i < 100; i++ )
 395     {
 396         double sum_w = 0;
 397
 398         if( first )
 399         {
 400             first = 0;
 401         }
 402         else
 403         {
 404             double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1];
 405             t = MAX(t,-1.);
 406             t = MIN(t,1.);
 407             if( fabs(acos(t)) < adelta )
 408             {
 409                 float x, y, d;
 410
 411                 x = (float) fabs( _line[2] - _lineprev[2] );
 412                 y = (float) fabs( _line[3] - _lineprev[3] );
 413
 414                 d = x > y ? x : y;
 415                 if( d < rdelta )
 416                     goto _exit_;
 417             }
 418         }
 419         /* calculate distances */
 420         if( icvCalcDist2D( points, count, _line, r ) < FLT_EPSILON*count )
 421             break;
 422
 423         /* calculate weights */
 424         if( calc_weights )
 425         {
 426             calc_weights( r, count, w );
 427         }
 428         else if( calc_weights_param )
 429         {
 430             calc_weights_param( r, count, w, _param );
 431         }
 432         else
 433             goto _exit_;
 434
 435         for( j = 0; j < count; j++ )
 436             sum_w += w[j];
 437
 438         if( fabs(sum_w) > FLT_EPSILON )
 439         {
 440             sum_w = 1./sum_w;
 441             for( j = 0; j < count; j++ )
 442                 w[j] = (float)(w[j]*sum_w);
 443         }
 444         else
 445         {
 446             for( j = 0; j < count; j++ )
 447                 w[j] = 1.f;
 448         }
 449
 450         /* save the line parameters */
 451         memcpy( _lineprev, _line, 4 * sizeof( float ));
 452
 453         /* Run again... */
 454         icvFitLine2D_wods( points, count, w, _line );
 455     }
 456
 457 _exit_:
 458     memcpy( line, _line, 4 * sizeof(line[0]));
 459     cvFree( &w );
 460     cvFree( &r );
 461     return CV_OK;
 462 }
 463
 464
 465 /* Takes an array of 3D points, type of distance (including user-defined
 466 distance specified by callbacks, fills the array of four floats with line
 467 parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector,
 468 (D, E, F) is the point that belongs to the line. */
 469
 470 static CvStatus
 471 icvFitLine3D( CvPoint3D32f * points, int count, int dist,
 472               float _param, float reps, float aeps, float *line )
 473 {
 474     void (*calc_weights) (float *, int, float *) = 0;
 475     void (*calc_weights_param) (float *, int, float *, float) = 0;
 476     float *w;                   /* weights */
 477     float *r;                   /* square distances */
 478     int i, j;
 479     float _line[6], _lineprev[6];
 480     int first = 1;
 481     float rdelta = reps != 0 ? reps : 1.0f;
 482     float adelta = aeps != 0 ? aeps : 0.01f;
 483
 484     memset( line, 0, 6*sizeof(line[0]) );
 485
 486     switch (dist)
 487     {
 488     case CV_DIST_L2:
 489         return icvFitLine3D_wods( points, count, 0, line );
 490
 491     case CV_DIST_L1:
 492         calc_weights = icvWeightL1;
 493         break;
 494
 495     case CV_DIST_L12:
 496         calc_weights = icvWeightL12;
 497         break;
 498
 499     case CV_DIST_FAIR:
 500         calc_weights_param = icvWeightFair;
 501         break;
 502
 503     case CV_DIST_WELSCH:
 504         calc_weights_param = icvWeightWelsch;
 505         break;
 506
 507     case CV_DIST_HUBER:
 508         calc_weights_param = icvWeightHuber;
 509         break;
 510
 511     /*case CV_DIST_USER:
 512         _PFP.p = param;
 513         calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
 514         break;*/
 515
 516     default:
 517         return CV_BADFACTOR_ERR;
 518     }
 519
 520     w = (float *) cvAlloc( count * sizeof( float ));
 521     r = (float *) cvAlloc( count * sizeof( float ));
 522
 523     for( i = 0; i < count; i++ )
 524         w[i] = 1.0f;
 525
 526     icvFitLine3D_wods( points, count, 0, _line );
 527     for( i = 0; i < 100; i++ )
 528     {
 529         double sum_w = 0;
 530
 531         if( first )
 532         {
 533             first = 0;
 534         }
 535         else
 536         {
 537             double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2];
 538             t = MAX(t,-1.);
 539             t = MIN(t,1.);
 540             if( fabs(acos(t)) < adelta )
 541             {
 542                 float x, y, z, ax, ay, az, dx, dy, dz, d;
 543
 544                 x = _line[3] - _lineprev[3];
 545                 y = _line[4] - _lineprev[4];
 546                 z = _line[5] - _lineprev[5];
 547                 ax = _line[0] - _lineprev[0];
 548                 ay = _line[1] - _lineprev[1];
 549                 az = _line[2] - _lineprev[2];
 550                 dx = (float) fabs( y * az - z * ay );
 551                 dy = (float) fabs( z * ax - x * az );
 552                 dz = (float) fabs( x * ay - y * ax );
 553
 554                 d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz);
 555                 if( d < rdelta )
 556                     goto _exit_;
 557             }
 558         }
 559         /* calculate distances */
 560         if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count )
 561             break;
 562
 563         /* calculate weights */
 564         if( calc_weights )
 565         {
 566             calc_weights( r, count, w );
 567         }
 568         else if( calc_weights_param )
 569         {
 570             calc_weights_param( r, count, w, _param );
 571         }
 572         else
 573             goto _exit_;
 574
 575         for( j = 0; j < count; j++ )
 576             sum_w += w[j];
 577
 578         if( fabs(sum_w) > FLT_EPSILON )
 579         {
 580             sum_w = 1./sum_w;
 581             for( j = 0; j < count; j++ )
 582                 w[j] = (float)(w[j]*sum_w);
 583         }
 584         else
 585         {
 586             for( j = 0; j < count; j++ )
 587                 w[j] = 1.f;
 588         }
 589
 590         /* save the line parameters */
 591         memcpy( _lineprev, _line, 6 * sizeof( float ));
 592
 593         /* Run again... */
 594         icvFitLine3D_wods( points, count, w, _line );
 595     }
 596 _exit_:
 597     // Return...
 598     memcpy( line, _line, 6 * sizeof(line[0]));
 599     cvFree( &w );
 600     cvFree( &r );
 601     return CV_OK;
 602 }
 603
 604
 605 CV_IMPL void
 606 cvFitLine( const CvArr* array, int dist, double param,
 607            double reps, double aeps, float *line )
 608 {
 609     char* buffer = 0;
 610     CV_FUNCNAME( "cvFitLine" );
 611
 612     __BEGIN__;
 613
 614     char* points = 0;
 615     union { CvContour contour; CvSeq seq; } header;
 616     CvSeqBlock block;
 617     CvSeq* ptseq = (CvSeq*)array;
 618     int type;
 619
 620     if( !line )
 621         CV_ERROR( CV_StsNullPtr, "NULL pointer to line parameters" );
 622
 623     if( CV_IS_SEQ(ptseq) )
 624     {
 625         type = CV_SEQ_ELTYPE(ptseq);
 626         if( ptseq->total == 0 )
 627             CV_ERROR( CV_StsBadSize, "The sequence has no points" );
 628         if( (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) ||
 629             CV_ELEM_SIZE(type) != ptseq->elem_size )
 630             CV_ERROR( CV_StsUnsupportedFormat,
 631                 "Input sequence must consist of 2d points or 3d points" );
 632     }
 633     else
 634     {
 635         CvMat* mat = (CvMat*)array;
 636         type = CV_MAT_TYPE(mat->type);
 637         if( !CV_IS_MAT(mat))
 638             CV_ERROR( CV_StsBadArg, "Input array is not a sequence nor matrix" );
 639
 640         if( !CV_IS_MAT_CONT(mat->type) ||
 641             (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) ||
 642             (mat->width != 1 && mat->height != 1))
 643             CV_ERROR( CV_StsBadArg,
 644             "Input array must be 1d continuous array of 2d or 3d points" );
 645
 646         CV_CALL( ptseq = cvMakeSeqHeaderForArray(
 647             CV_SEQ_KIND_GENERIC|type, sizeof(CvContour), CV_ELEM_SIZE(type), mat->data.ptr,
 648             mat->width + mat->height - 1, &header.seq, &block ));
 649     }
 650
 651     if( reps < 0 || aeps < 0 )
 652         CV_ERROR( CV_StsOutOfRange, "Both reps and aeps must be non-negative" );
 653
 654     if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first )
 655     {
 656         /* no need to copy data in this case */
 657         points = ptseq->first->data;
 658     }
 659     else
 660     {
 661         CV_CALL( buffer = points = (char*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) ));
 662         CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ ));
 663
 664         if( CV_MAT_DEPTH(type) != CV_32F )
 665         {
 666             int i, total = ptseq->total*CV_MAT_CN(type);
 667             assert( CV_MAT_DEPTH(type) == CV_32S );
 668
 669             for( i = 0; i < total; i++ )
 670                 ((float*)points)[i] = (float)((int*)points)[i];
 671         }
 672     }
 673
 674     if( dist == CV_DIST_USER )
 675         CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" );
 676
 677     if( CV_MAT_CN(type) == 2 )
 678     {
 679         IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total,
 680                                  dist, (float)param, (float)reps, (float)aeps, line ));
 681     }
 682     else
 683     {
 684         IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total,
 685                                  dist, (float)param, (float)reps, (float)aeps, line ));
 686     }
 687
 688     __END__;
 689
 690     cvFree( &buffer );
 691 }
 692
 693 /* End of file. */