linefit.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                        Intel License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of Intel Corporation may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the Intel Corporation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 #include "precomp.hpp"
00042 
00043 namespace cv
00044 {
00045 
00046 static const double eps = 1e-6;
00047 
00048 static void fitLine2D_wods( const Point2f* points, int count, float *weights, float *line )
00049 {
00050     double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0;
00051     double dx2, dy2, dxy;
00052     int i;
00053     float t;
00054 
00055     // Calculating the average of x and y...
00056     if( weights == 0 )
00057     {
00058         for( i = 0; i < count; i += 1 )
00059         {
00060             x += points[i].x;
00061             y += points[i].y;
00062             x2 += points[i].x * points[i].x;
00063             y2 += points[i].y * points[i].y;
00064             xy += points[i].x * points[i].y;
00065         }
00066         w = (float) count;
00067     }
00068     else
00069     {
00070         for( i = 0; i < count; i += 1 )
00071         {
00072             x += weights[i] * points[i].x;
00073             y += weights[i] * points[i].y;
00074             x2 += weights[i] * points[i].x * points[i].x;
00075             y2 += weights[i] * points[i].y * points[i].y;
00076             xy += weights[i] * points[i].x * points[i].y;
00077             w += weights[i];
00078         }
00079     }
00080 
00081     x /= w;
00082     y /= w;
00083     x2 /= w;
00084     y2 /= w;
00085     xy /= w;
00086 
00087     dx2 = x2 - x * x;
00088     dy2 = y2 - y * y;
00089     dxy = xy - x * y;
00090 
00091     t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2;
00092     line[0] = (float) cos( t );
00093     line[1] = (float) sin( t );
00094 
00095     line[2] = (float) x;
00096     line[3] = (float) y;
00097 }
00098 
00099 static void fitLine3D_wods( const Point3f * points, int count, float *weights, float *line )
00100 {
00101     int i;
00102     float w0 = 0;
00103     float x0 = 0, y0 = 0, z0 = 0;
00104     float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
00105     float dx2, dy2, dz2, dxy, dxz, dyz;
00106     float *v;
00107     float n;
00108     float det[9], evc[9], evl[3];
00109 
00110     memset( evl, 0, 3*sizeof(evl[0]));
00111     memset( evc, 0, 9*sizeof(evl[0]));
00112 
00113     if( weights )
00114     {
00115         for( i = 0; i < count; i++ )
00116         {
00117             float x = points[i].x;
00118             float y = points[i].y;
00119             float z = points[i].z;
00120             float w = weights[i];
00121 
00122 
00123             x2 += x * x * w;
00124             xy += x * y * w;
00125             xz += x * z * w;
00126             y2 += y * y * w;
00127             yz += y * z * w;
00128             z2 += z * z * w;
00129             x0 += x * w;
00130             y0 += y * w;
00131             z0 += z * w;
00132             w0 += w;
00133         }
00134     }
00135     else
00136     {
00137         for( i = 0; i < count; i++ )
00138         {
00139             float x = points[i].x;
00140             float y = points[i].y;
00141             float z = points[i].z;
00142 
00143             x2 += x * x;
00144             xy += x * y;
00145             xz += x * z;
00146             y2 += y * y;
00147             yz += y * z;
00148             z2 += z * z;
00149             x0 += x;
00150             y0 += y;
00151             z0 += z;
00152         }
00153         w0 = (float) count;
00154     }
00155 
00156     x2 /= w0;
00157     xy /= w0;
00158     xz /= w0;
00159     y2 /= w0;
00160     yz /= w0;
00161     z2 /= w0;
00162 
00163     x0 /= w0;
00164     y0 /= w0;
00165     z0 /= w0;
00166 
00167     dx2 = x2 - x0 * x0;
00168     dxy = xy - x0 * y0;
00169     dxz = xz - x0 * z0;
00170     dy2 = y2 - y0 * y0;
00171     dyz = yz - y0 * z0;
00172     dz2 = z2 - z0 * z0;
00173 
00174     det[0] = dz2 + dy2;
00175     det[1] = -dxy;
00176     det[2] = -dxz;
00177     det[3] = det[1];
00178     det[4] = dx2 + dz2;
00179     det[5] = -dyz;
00180     det[6] = det[2];
00181     det[7] = det[5];
00182     det[8] = dy2 + dx2;
00183 
00184     // Searching for a eigenvector of det corresponding to the minimal eigenvalue
00185     Mat _det( 3, 3, CV_32F, det );
00186     Mat _evc( 3, 3, CV_32F, evc );
00187     Mat _evl( 3, 1, CV_32F, evl );
00188     eigen( _det, _evl, _evc );
00189     i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2);
00190 
00191     v = &evc[i * 3];
00192     n = (float) std::sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] );
00193     n = (float)MAX(n, eps);
00194     line[0] = v[0] / n;
00195     line[1] = v[1] / n;
00196     line[2] = v[2] / n;
00197     line[3] = x0;
00198     line[4] = y0;
00199     line[5] = z0;
00200 }
00201 
00202 static double calcDist2D( const Point2f* points, int count, float *_line, float *dist )
00203 {
00204     int j;
00205     float px = _line[2], py = _line[3];
00206     float nx = _line[1], ny = -_line[0];
00207     double sum_dist = 0.;
00208 
00209     for( j = 0; j < count; j++ )
00210     {
00211         float x, y;
00212 
00213         x = points[j].x - px;
00214         y = points[j].y - py;
00215 
00216         dist[j] = (float) fabs( nx * x + ny * y );
00217         sum_dist += dist[j];
00218     }
00219 
00220     return sum_dist;
00221 }
00222 
00223 static double calcDist3D( const Point3f* points, int count, float *_line, float *dist )
00224 {
00225     int j;
00226     float px = _line[3], py = _line[4], pz = _line[5];
00227     float vx = _line[0], vy = _line[1], vz = _line[2];
00228     double sum_dist = 0.;
00229 
00230     for( j = 0; j < count; j++ )
00231     {
00232         float x, y, z;
00233         double p1, p2, p3;
00234 
00235         x = points[j].x - px;
00236         y = points[j].y - py;
00237         z = points[j].z - pz;
00238 
00239         p1 = vy * z - vz * y;
00240         p2 = vz * x - vx * z;
00241         p3 = vx * y - vy * x;
00242 
00243         dist[j] = (float) std::sqrt( p1*p1 + p2*p2 + p3*p3 );
00244         sum_dist += dist[j];
00245     }
00246 
00247     return sum_dist;
00248 }
00249 
00250 static void weightL1( float *d, int count, float *w )
00251 {
00252     int i;
00253 
00254     for( i = 0; i < count; i++ )
00255     {
00256         double t = fabs( (double) d[i] );
00257         w[i] = (float)(1. / MAX(t, eps));
00258     }
00259 }
00260 
00261 static void weightL12( float *d, int count, float *w )
00262 {
00263     int i;
00264 
00265     for( i = 0; i < count; i++ )
00266     {
00267         w[i] = 1.0f / (float) std::sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
00268     }
00269 }
00270 
00271 
00272 static void weightHuber( float *d, int count, float *w, float _c )
00273 {
00274     int i;
00275     const float c = _c <= 0 ? 1.345f : _c;
00276 
00277     for( i = 0; i < count; i++ )
00278     {
00279         if( d[i] < c )
00280             w[i] = 1.0f;
00281         else
00282             w[i] = c/d[i];
00283     }
00284 }
00285 
00286 
00287 static void weightFair( float *d, int count, float *w, float _c )
00288 {
00289     int i;
00290     const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;
00291 
00292     for( i = 0; i < count; i++ )
00293     {
00294         w[i] = 1 / (1 + d[i] * c);
00295     }
00296 }
00297 
00298 static void weightWelsch( float *d, int count, float *w, float _c )
00299 {
00300     int i;
00301     const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;
00302 
00303     for( i = 0; i < count; i++ )
00304     {
00305         w[i] = (float) std::exp( -d[i] * d[i] * c * c );
00306     }
00307 }
00308 
00309 
00310 /* Takes an array of 2D points, type of distance (including user-defined
00311  distance specified by callbacks, fills the array of four floats with line
00312  parameters A, B, C, D, where (A, B) is the normalized direction vector,
00313  (C, D) is the point that belongs to the line. */
00314 
00315 static void fitLine2D( const Point2f * points, int count, int dist,
00316                       float _param, float reps, float aeps, float *line )
00317 {
00318     double EPS = count*FLT_EPSILON;
00319     void (*calc_weights) (float *, int, float *) = 0;
00320     void (*calc_weights_param) (float *, int, float *, float) = 0;
00321     int i, j, k;
00322     float _line[6], _lineprev[6];
00323     float rdelta = reps != 0 ? reps : 1.0f;
00324     float adelta = aeps != 0 ? aeps : 0.01f;
00325     double min_err = DBL_MAX, err = 0;
00326     RNG rng((uint64)-1);
00327 
00328     memset( line, 0, 4*sizeof(line[0]) );
00329 
00330     switch (dist)
00331     {
00332     case CV_DIST_L2:
00333         return fitLine2D_wods( points, count, 0, line );
00334 
00335     case CV_DIST_L1:
00336         calc_weights = weightL1;
00337         break;
00338 
00339     case CV_DIST_L12:
00340         calc_weights = weightL12;
00341         break;
00342 
00343     case CV_DIST_FAIR:
00344         calc_weights_param = weightFair;
00345         break;
00346 
00347     case CV_DIST_WELSCH:
00348         calc_weights_param = weightWelsch;
00349         break;
00350 
00351     case CV_DIST_HUBER:
00352         calc_weights_param = weightHuber;
00353         break;
00354 
00355     /*case DIST_USER:
00356      calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
00357      break;*/
00358     default:
00359         CV_Error(CV_StsBadArg, "Unknown distance type");
00360     }
00361 
00362     AutoBuffer<float> wr(count*2);
00363     float *w = wr, *r = w + count;
00364 
00365     for( k = 0; k < 20; k++ )
00366     {
00367         int first = 1;
00368         for( i = 0; i < count; i++ )
00369             w[i] = 0.f;
00370 
00371         for( i = 0; i < MIN(count,10); )
00372         {
00373             j = rng.uniform(0, count);
00374             if( w[j] < FLT_EPSILON )
00375             {
00376                 w[j] = 1.f;
00377                 i++;
00378             }
00379         }
00380 
00381         fitLine2D_wods( points, count, w, _line );
00382         for( i = 0; i < 30; i++ )
00383         {
00384             double sum_w = 0;
00385 
00386             if( first )
00387             {
00388                 first = 0;
00389             }
00390             else
00391             {
00392                 double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1];
00393                 t = MAX(t,-1.);
00394                 t = MIN(t,1.);
00395                 if( fabs(acos(t)) < adelta )
00396                 {
00397                     float x, y, d;
00398 
00399                     x = (float) fabs( _line[2] - _lineprev[2] );
00400                     y = (float) fabs( _line[3] - _lineprev[3] );
00401 
00402                     d = x > y ? x : y;
00403                     if( d < rdelta )
00404                         break;
00405                 }
00406             }
00407             /* calculate distances */
00408             err = calcDist2D( points, count, _line, r );
00409             if( err < EPS )
00410                 break;
00411 
00412             /* calculate weights */
00413             if( calc_weights )
00414                 calc_weights( r, count, w );
00415             else
00416                 calc_weights_param( r, count, w, _param );
00417 
00418             for( j = 0; j < count; j++ )
00419                 sum_w += w[j];
00420 
00421             if( fabs(sum_w) > FLT_EPSILON )
00422             {
00423                 sum_w = 1./sum_w;
00424                 for( j = 0; j < count; j++ )
00425                     w[j] = (float)(w[j]*sum_w);
00426             }
00427             else
00428             {
00429                 for( j = 0; j < count; j++ )
00430                     w[j] = 1.f;
00431             }
00432 
00433             /* save the line parameters */
00434             memcpy( _lineprev, _line, 4 * sizeof( float ));
00435 
00436             /* Run again... */
00437             fitLine2D_wods( points, count, w, _line );
00438         }
00439 
00440         if( err < min_err )
00441         {
00442             min_err = err;
00443             memcpy( line, _line, 4 * sizeof(line[0]));
00444             if( err < EPS )
00445                 break;
00446         }
00447     }
00448 }
00449 
00450 
00451 /* Takes an array of 3D points, type of distance (including user-defined
00452  distance specified by callbacks, fills the array of four floats with line
00453  parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector,
00454  (D, E, F) is the point that belongs to the line. */
00455 static void fitLine3D( Point3f * points, int count, int dist,
00456                        float _param, float reps, float aeps, float *line )
00457 {
00458     double EPS = count*FLT_EPSILON;
00459     void (*calc_weights) (float *, int, float *) = 0;
00460     void (*calc_weights_param) (float *, int, float *, float) = 0;
00461     int i, j, k;
00462     float _line[6]={0,0,0,0,0,0}, _lineprev[6]={0,0,0,0,0,0};
00463     float rdelta = reps != 0 ? reps : 1.0f;
00464     float adelta = aeps != 0 ? aeps : 0.01f;
00465     double min_err = DBL_MAX, err = 0;
00466     RNG rng((uint64)-1);
00467 
00468     switch (dist)
00469     {
00470     case CV_DIST_L2:
00471         return fitLine3D_wods( points, count, 0, line );
00472 
00473     case CV_DIST_L1:
00474         calc_weights = weightL1;
00475         break;
00476 
00477     case CV_DIST_L12:
00478         calc_weights = weightL12;
00479         break;
00480 
00481     case CV_DIST_FAIR:
00482         calc_weights_param = weightFair;
00483         break;
00484 
00485     case CV_DIST_WELSCH:
00486         calc_weights_param = weightWelsch;
00487         break;
00488 
00489     case CV_DIST_HUBER:
00490         calc_weights_param = weightHuber;
00491         break;
00492 
00493     default:
00494         CV_Error(CV_StsBadArg, "Unknown distance");
00495     }
00496 
00497     AutoBuffer<float> buf(count*2);
00498     float *w = buf, *r = w + count;
00499 
00500     for( k = 0; k < 20; k++ )
00501     {
00502         int first = 1;
00503         for( i = 0; i < count; i++ )
00504             w[i] = 0.f;
00505 
00506         for( i = 0; i < MIN(count,10); )
00507         {
00508             j = rng.uniform(0, count);
00509             if( w[j] < FLT_EPSILON )
00510             {
00511                 w[j] = 1.f;
00512                 i++;
00513             }
00514         }
00515 
00516         fitLine3D_wods( points, count, w, _line );
00517         for( i = 0; i < 30; i++ )
00518         {
00519             double sum_w = 0;
00520 
00521             if( first )
00522             {
00523                 first = 0;
00524             }
00525             else
00526             {
00527                 double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2];
00528                 t = MAX(t,-1.);
00529                 t = MIN(t,1.);
00530                 if( fabs(acos(t)) < adelta )
00531                 {
00532                     float x, y, z, ax, ay, az, dx, dy, dz, d;
00533 
00534                     x = _line[3] - _lineprev[3];
00535                     y = _line[4] - _lineprev[4];
00536                     z = _line[5] - _lineprev[5];
00537                     ax = _line[0] - _lineprev[0];
00538                     ay = _line[1] - _lineprev[1];
00539                     az = _line[2] - _lineprev[2];
00540                     dx = (float) fabs( y * az - z * ay );
00541                     dy = (float) fabs( z * ax - x * az );
00542                     dz = (float) fabs( x * ay - y * ax );
00543 
00544                     d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz);
00545                     if( d < rdelta )
00546                         break;
00547                 }
00548             }
00549             /* calculate distances */
00550             err = calcDist3D( points, count, _line, r );
00551             //if( err < FLT_EPSILON*count )
00552             //    break;
00553 
00554             /* calculate weights */
00555             if( calc_weights )
00556                 calc_weights( r, count, w );
00557             else
00558                 calc_weights_param( r, count, w, _param );
00559 
00560             for( j = 0; j < count; j++ )
00561                 sum_w += w[j];
00562 
00563             if( fabs(sum_w) > FLT_EPSILON )
00564             {
00565                 sum_w = 1./sum_w;
00566                 for( j = 0; j < count; j++ )
00567                     w[j] = (float)(w[j]*sum_w);
00568             }
00569             else
00570             {
00571                 for( j = 0; j < count; j++ )
00572                     w[j] = 1.f;
00573             }
00574 
00575             /* save the line parameters */
00576             memcpy( _lineprev, _line, 6 * sizeof( float ));
00577 
00578             /* Run again... */
00579             fitLine3D_wods( points, count, w, _line );
00580         }
00581 
00582         if( err < min_err )
00583         {
00584             min_err = err;
00585             memcpy( line, _line, 6 * sizeof(line[0]));
00586             if( err < EPS )
00587                 break;
00588         }
00589     }
00590 }
00591 
00592 }
00593 
00594 void cv::fitLine( InputArray _points, OutputArray _line, int distType,
00595                  double param, double reps, double aeps )
00596 {
00597     Mat points = _points.getMat();
00598 
00599     float linebuf[6]={0.f};
00600     int npoints2 = points.checkVector(2, -1, false);
00601     int npoints3 = points.checkVector(3, -1, false);
00602 
00603     CV_Assert( npoints2 >= 0 || npoints3 >= 0 );
00604 
00605     if( points.depth() != CV_32F || !points.isContinuous() )
00606     {
00607         Mat temp;
00608         points.convertTo(temp, CV_32F);
00609         points = temp;
00610     }
00611 
00612     if( npoints2 >= 0 )
00613         fitLine2D( points.ptr<Point2f >(), npoints2, distType,
00614                    (float)param, (float)reps, (float)aeps, linebuf);
00615     else
00616         fitLine3D( points.ptr<Point3f>(), npoints3, distType,
00617                    (float)param, (float)reps, (float)aeps, linebuf);
00618 
00619     Mat(npoints2 >= 0 ? 4 : 6, 1, CV_32F, linebuf).copyTo(_line);
00620 }
00621 
00622 
00623 CV_IMPL void
00624 cvFitLine( const CvArr* array, int dist, double param,
00625            double reps, double aeps, float *line )
00626 {
00627     CV_Assert(line != 0);
00628 
00629     cv::AutoBuffer<double> buf;
00630     cv::Mat points = cv::cvarrToMat(array, false, false, 0, &buf);
00631     cv::Mat linemat(points.checkVector(2) >= 0 ? 4 : 6, 1, CV_32F, line);
00632 
00633     cv::fitLine(points, linemat, dist, param, reps, aeps);
00634 }
00635 
00636 /* End of file. */
00637