| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| #include "_cv.h" |
| |
| static const double eps = 1e-6; |
| |
| static CvStatus |
| icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line ) |
| { |
| double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0; |
| double dx2, dy2, dxy; |
| int i; |
| int count = _count; |
| float t; |
| |
| /* Calculating the average of x and y... */ |
| |
| if( weights == 0 ) |
| { |
| for( i = 0; i < count; i += 1 ) |
| { |
| x += points[i].x; |
| y += points[i].y; |
| x2 += points[i].x * points[i].x; |
| y2 += points[i].y * points[i].y; |
| xy += points[i].x * points[i].y; |
| } |
| w = (float) count; |
| } |
| else |
| { |
| for( i = 0; i < count; i += 1 ) |
| { |
| x += weights[i] * points[i].x; |
| y += weights[i] * points[i].y; |
| x2 += weights[i] * points[i].x * points[i].x; |
| y2 += weights[i] * points[i].y * points[i].y; |
| xy += weights[i] * points[i].x * points[i].y; |
| w += weights[i]; |
| } |
| } |
| |
| x /= w; |
| y /= w; |
| x2 /= w; |
| y2 /= w; |
| xy /= w; |
| |
| dx2 = x2 - x * x; |
| dy2 = y2 - y * y; |
| dxy = xy - x * y; |
| |
| t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2; |
| line[0] = (float) cos( t ); |
| line[1] = (float) sin( t ); |
| |
| line[2] = (float) x; |
| line[3] = (float) y; |
| |
| return CV_NO_ERR; |
| } |
| |
| static CvStatus |
| icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line ) |
| { |
| int i; |
| float w0 = 0; |
| float x0 = 0, y0 = 0, z0 = 0; |
| float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0; |
| float dx2, dy2, dz2, dxy, dxz, dyz; |
| float *v; |
| float n; |
| float det[9], evc[9], evl[3]; |
| |
| memset( evl, 0, 3*sizeof(evl[0])); |
| memset( evc, 0, 9*sizeof(evl[0])); |
| |
| if( weights ) |
| { |
| for( i = 0; i < count; i++ ) |
| { |
| float x = points[i].x; |
| float y = points[i].y; |
| float z = points[i].z; |
| float w = weights[i]; |
| |
| |
| x2 += x * x * w; |
| xy += x * y * w; |
| xz += x * z * w; |
| y2 += y * y * w; |
| yz += y * z * w; |
| z2 += z * z * w; |
| x0 += x * w; |
| y0 += y * w; |
| z0 += z * w; |
| w0 += w; |
| } |
| } |
| else |
| { |
| for( i = 0; i < count; i++ ) |
| { |
| float x = points[i].x; |
| float y = points[i].y; |
| float z = points[i].z; |
| |
| x2 += x * x; |
| xy += x * y; |
| xz += x * z; |
| y2 += y * y; |
| yz += y * z; |
| z2 += z * z; |
| x0 += x; |
| y0 += y; |
| z0 += z; |
| } |
| w0 = (float) count; |
| } |
| |
| x2 /= w0; |
| xy /= w0; |
| xz /= w0; |
| y2 /= w0; |
| yz /= w0; |
| z2 /= w0; |
| |
| x0 /= w0; |
| y0 /= w0; |
| z0 /= w0; |
| |
| dx2 = x2 - x0 * x0; |
| dxy = xy - x0 * y0; |
| dxz = xz - x0 * z0; |
| dy2 = y2 - y0 * y0; |
| dyz = yz - y0 * z0; |
| dz2 = z2 - z0 * z0; |
| |
| det[0] = dz2 + dy2; |
| det[1] = -dxy; |
| det[2] = -dxz; |
| det[3] = det[1]; |
| det[4] = dx2 + dz2; |
| det[5] = -dyz; |
| det[6] = det[2]; |
| det[7] = det[5]; |
| det[8] = dy2 + dx2; |
| |
| /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */ |
| #if 1 |
| { |
| CvMat _det = cvMat( 3, 3, CV_32F, det ); |
| CvMat _evc = cvMat( 3, 3, CV_32F, evc ); |
| CvMat _evl = cvMat( 3, 1, CV_32F, evl ); |
| cvEigenVV( &_det, &_evc, &_evl, 0 ); |
| i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2); |
| } |
| #else |
| { |
| CvMat _det = cvMat( 3, 3, CV_32F, det ); |
| CvMat _evc = cvMat( 3, 3, CV_32F, evc ); |
| CvMat _evl = cvMat( 1, 3, CV_32F, evl ); |
| |
| cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T ); |
| } |
| i = 2; |
| #endif |
| v = &evc[i * 3]; |
| n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] ); |
| n = (float)MAX(n, eps); |
| line[0] = v[0] / n; |
| line[1] = v[1] / n; |
| line[2] = v[2] / n; |
| line[3] = x0; |
| line[4] = y0; |
| line[5] = z0; |
| |
| return CV_NO_ERR; |
| } |
| |
| static double |
| icvCalcDist2D( CvPoint2D32f * points, int count, float *_line, float *dist ) |
| { |
| int j; |
| float px = _line[2], py = _line[3]; |
| float nx = _line[1], ny = -_line[0]; |
| double sum_dist = 0.; |
| |
| for( j = 0; j < count; j++ ) |
| { |
| float x, y; |
| |
| x = points[j].x - px; |
| y = points[j].y - py; |
| |
| dist[j] = (float) fabs( nx * x + ny * y ); |
| sum_dist += dist[j]; |
| } |
| |
| return sum_dist; |
| } |
| |
| static double |
| icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist ) |
| { |
| int j; |
| float px = _line[3], py = _line[4], pz = _line[5]; |
| float vx = _line[0], vy = _line[1], vz = _line[2]; |
| double sum_dist = 0.; |
| |
| for( j = 0; j < count; j++ ) |
| { |
| float x, y, z; |
| double p1, p2, p3; |
| |
| x = points[j].x - px; |
| y = points[j].y - py; |
| z = points[j].z - pz; |
| |
| p1 = vy * z - vz * y; |
| p2 = vz * x - vx * z; |
| p3 = vx * y - vy * x; |
| |
| dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 ); |
| sum_dist += dist[j]; |
| } |
| |
| return sum_dist; |
| } |
| |
| static void |
| icvWeightL1( float *d, int count, float *w ) |
| { |
| int i; |
| |
| for( i = 0; i < count; i++ ) |
| { |
| double t = fabs( (double) d[i] ); |
| w[i] = (float)(1. / MAX(t, eps)); |
| } |
| } |
| |
| static void |
| icvWeightL12( float *d, int count, float *w ) |
| { |
| int i; |
| |
| for( i = 0; i < count; i++ ) |
| { |
| w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) ); |
| } |
| } |
| |
| |
| static void |
| icvWeightHuber( float *d, int count, float *w, float _c ) |
| { |
| int i; |
| const float c = _c <= 0 ? 1.345f : _c; |
| |
| for( i = 0; i < count; i++ ) |
| { |
| if( d[i] < c ) |
| w[i] = 1.0f; |
| else |
| w[i] = c/d[i]; |
| } |
| } |
| |
| |
| static void |
| icvWeightFair( float *d, int count, float *w, float _c ) |
| { |
| int i; |
| const float c = _c == 0 ? 1 / 1.3998f : 1 / _c; |
| |
| for( i = 0; i < count; i++ ) |
| { |
| w[i] = 1 / (1 + d[i] * c); |
| } |
| } |
| |
| static void |
| icvWeightWelsch( float *d, int count, float *w, float _c ) |
| { |
| int i; |
| const float c = _c == 0 ? 1 / 2.9846f : 1 / _c; |
| |
| for( i = 0; i < count; i++ ) |
| { |
| w[i] = (float) exp( -d[i] * d[i] * c * c ); |
| } |
| } |
| |
| |
| /* Takes an array of 2D points, type of distance (including user-defined |
| distance specified by callbacks, fills the array of four floats with line |
| parameters A, B, C, D, where (A, B) is the normalized direction vector, |
| (C, D) is the point that belongs to the line. */ |
| |
| static CvStatus icvFitLine2D( CvPoint2D32f * points, int count, int dist, |
| float _param, float reps, float aeps, float *line ) |
| { |
| double EPS = count*FLT_EPSILON; |
| void (*calc_weights) (float *, int, float *) = 0; |
| void (*calc_weights_param) (float *, int, float *, float) = 0; |
| float *w; /* weights */ |
| float *r; /* square distances */ |
| int i, j, k; |
| float _line[6], _lineprev[6]; |
| float rdelta = reps != 0 ? reps : 1.0f; |
| float adelta = aeps != 0 ? aeps : 0.01f; |
| double min_err = DBL_MAX, err = 0; |
| CvRNG rng = cvRNG(-1); |
| |
| memset( line, 0, 4*sizeof(line[0]) ); |
| |
| switch (dist) |
| { |
| case CV_DIST_L2: |
| return icvFitLine2D_wods( points, count, 0, line ); |
| |
| case CV_DIST_L1: |
| calc_weights = icvWeightL1; |
| break; |
| |
| case CV_DIST_L12: |
| calc_weights = icvWeightL12; |
| break; |
| |
| case CV_DIST_FAIR: |
| calc_weights_param = icvWeightFair; |
| break; |
| |
| case CV_DIST_WELSCH: |
| calc_weights_param = icvWeightWelsch; |
| break; |
| |
| case CV_DIST_HUBER: |
| calc_weights_param = icvWeightHuber; |
| break; |
| |
| /*case CV_DIST_USER: |
| calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; |
| break;*/ |
| |
| default: |
| return CV_BADFACTOR_ERR; |
| } |
| |
| w = (float *) cvAlloc( count * sizeof( float )); |
| r = (float *) cvAlloc( count * sizeof( float )); |
| |
| for( k = 0; k < 20; k++ ) |
| { |
| int first = 1; |
| for( i = 0; i < count; i++ ) |
| w[i] = 0.f; |
| |
| for( i = 0; i < MIN(count,10); ) |
| { |
| j = cvRandInt(&rng) % count; |
| if( w[j] < FLT_EPSILON ) |
| { |
| w[j] = 1.f; |
| i++; |
| } |
| } |
| |
| icvFitLine2D_wods( points, count, w, _line ); |
| for( i = 0; i < 30; i++ ) |
| { |
| double sum_w = 0; |
| |
| if( first ) |
| { |
| first = 0; |
| } |
| else |
| { |
| double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1]; |
| t = MAX(t,-1.); |
| t = MIN(t,1.); |
| if( fabs(acos(t)) < adelta ) |
| { |
| float x, y, d; |
| |
| x = (float) fabs( _line[2] - _lineprev[2] ); |
| y = (float) fabs( _line[3] - _lineprev[3] ); |
| |
| d = x > y ? x : y; |
| if( d < rdelta ) |
| break; |
| } |
| } |
| /* calculate distances */ |
| err = icvCalcDist2D( points, count, _line, r ); |
| if( err < EPS ) |
| break; |
| |
| /* calculate weights */ |
| if( calc_weights ) |
| calc_weights( r, count, w ); |
| else |
| calc_weights_param( r, count, w, _param ); |
| |
| for( j = 0; j < count; j++ ) |
| sum_w += w[j]; |
| |
| if( fabs(sum_w) > FLT_EPSILON ) |
| { |
| sum_w = 1./sum_w; |
| for( j = 0; j < count; j++ ) |
| w[j] = (float)(w[j]*sum_w); |
| } |
| else |
| { |
| for( j = 0; j < count; j++ ) |
| w[j] = 1.f; |
| } |
| |
| /* save the line parameters */ |
| memcpy( _lineprev, _line, 4 * sizeof( float )); |
| |
| /* Run again... */ |
| icvFitLine2D_wods( points, count, w, _line ); |
| } |
| |
| if( err < min_err ) |
| { |
| min_err = err; |
| memcpy( line, _line, 4 * sizeof(line[0])); |
| if( err < EPS ) |
| break; |
| } |
| } |
| |
| cvFree( &w ); |
| cvFree( &r ); |
| return CV_OK; |
| } |
| |
| |
| /* Takes an array of 3D points, type of distance (including user-defined |
| distance specified by callbacks, fills the array of four floats with line |
| parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector, |
| (D, E, F) is the point that belongs to the line. */ |
| |
| static CvStatus |
| icvFitLine3D( CvPoint3D32f * points, int count, int dist, |
| float _param, float reps, float aeps, float *line ) |
| { |
| double EPS = count*FLT_EPSILON; |
| void (*calc_weights) (float *, int, float *) = 0; |
| void (*calc_weights_param) (float *, int, float *, float) = 0; |
| float *w; /* weights */ |
| float *r; /* square distances */ |
| int i, j, k; |
| float _line[6], _lineprev[6]; |
| float rdelta = reps != 0 ? reps : 1.0f; |
| float adelta = aeps != 0 ? aeps : 0.01f; |
| double min_err = DBL_MAX, err = 0; |
| CvRNG rng = cvRNG(-1); |
| |
| memset( line, 0, 6*sizeof(line[0]) ); |
| |
| switch (dist) |
| { |
| case CV_DIST_L2: |
| return icvFitLine3D_wods( points, count, 0, line ); |
| |
| case CV_DIST_L1: |
| calc_weights = icvWeightL1; |
| break; |
| |
| case CV_DIST_L12: |
| calc_weights = icvWeightL12; |
| break; |
| |
| case CV_DIST_FAIR: |
| calc_weights_param = icvWeightFair; |
| break; |
| |
| case CV_DIST_WELSCH: |
| calc_weights_param = icvWeightWelsch; |
| break; |
| |
| case CV_DIST_HUBER: |
| calc_weights_param = icvWeightHuber; |
| break; |
| |
| /*case CV_DIST_USER: |
| _PFP.p = param; |
| calc_weights = (void ( * )(float *, int, float *)) _PFP.fp; |
| break;*/ |
| |
| default: |
| return CV_BADFACTOR_ERR; |
| } |
| |
| w = (float *) cvAlloc( count * sizeof( float )); |
| r = (float *) cvAlloc( count * sizeof( float )); |
| |
| for( k = 0; k < 20; k++ ) |
| { |
| int first = 1; |
| for( i = 0; i < count; i++ ) |
| w[i] = 0.f; |
| |
| for( i = 0; i < MIN(count,10); ) |
| { |
| j = cvRandInt(&rng) % count; |
| if( w[j] < FLT_EPSILON ) |
| { |
| w[j] = 1.f; |
| i++; |
| } |
| } |
| |
| icvFitLine3D_wods( points, count, w, _line ); |
| for( i = 0; i < 30; i++ ) |
| { |
| double sum_w = 0; |
| |
| if( first ) |
| { |
| first = 0; |
| } |
| else |
| { |
| double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2]; |
| t = MAX(t,-1.); |
| t = MIN(t,1.); |
| if( fabs(acos(t)) < adelta ) |
| { |
| float x, y, z, ax, ay, az, dx, dy, dz, d; |
| |
| x = _line[3] - _lineprev[3]; |
| y = _line[4] - _lineprev[4]; |
| z = _line[5] - _lineprev[5]; |
| ax = _line[0] - _lineprev[0]; |
| ay = _line[1] - _lineprev[1]; |
| az = _line[2] - _lineprev[2]; |
| dx = (float) fabs( y * az - z * ay ); |
| dy = (float) fabs( z * ax - x * az ); |
| dz = (float) fabs( x * ay - y * ax ); |
| |
| d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz); |
| if( d < rdelta ) |
| break; |
| } |
| } |
| /* calculate distances */ |
| if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count ) |
| break; |
| |
| /* calculate weights */ |
| if( calc_weights ) |
| calc_weights( r, count, w ); |
| else |
| calc_weights_param( r, count, w, _param ); |
| |
| for( j = 0; j < count; j++ ) |
| sum_w += w[j]; |
| |
| if( fabs(sum_w) > FLT_EPSILON ) |
| { |
| sum_w = 1./sum_w; |
| for( j = 0; j < count; j++ ) |
| w[j] = (float)(w[j]*sum_w); |
| } |
| else |
| { |
| for( j = 0; j < count; j++ ) |
| w[j] = 1.f; |
| } |
| |
| /* save the line parameters */ |
| memcpy( _lineprev, _line, 6 * sizeof( float )); |
| |
| /* Run again... */ |
| icvFitLine3D_wods( points, count, w, _line ); |
| } |
| |
| if( err < min_err ) |
| { |
| min_err = err; |
| memcpy( line, _line, 6 * sizeof(line[0])); |
| if( err < EPS ) |
| break; |
| } |
| } |
| |
| // Return... |
| cvFree( &w ); |
| cvFree( &r ); |
| return CV_OK; |
| } |
| |
| |
| CV_IMPL void |
| cvFitLine( const CvArr* array, int dist, double param, |
| double reps, double aeps, float *line ) |
| { |
| schar* buffer = 0; |
| CV_FUNCNAME( "cvFitLine" ); |
| |
| __BEGIN__; |
| |
| schar* points = 0; |
| union { CvContour contour; CvSeq seq; } header; |
| CvSeqBlock block; |
| CvSeq* ptseq = (CvSeq*)array; |
| int type; |
| |
| if( !line ) |
| CV_ERROR( CV_StsNullPtr, "NULL pointer to line parameters" ); |
| |
| if( CV_IS_SEQ(ptseq) ) |
| { |
| type = CV_SEQ_ELTYPE(ptseq); |
| if( ptseq->total == 0 ) |
| CV_ERROR( CV_StsBadSize, "The sequence has no points" ); |
| if( (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) || |
| CV_ELEM_SIZE(type) != ptseq->elem_size ) |
| CV_ERROR( CV_StsUnsupportedFormat, |
| "Input sequence must consist of 2d points or 3d points" ); |
| } |
| else |
| { |
| CvMat* mat = (CvMat*)array; |
| type = CV_MAT_TYPE(mat->type); |
| if( !CV_IS_MAT(mat)) |
| CV_ERROR( CV_StsBadArg, "Input array is not a sequence nor matrix" ); |
| |
| if( !CV_IS_MAT_CONT(mat->type) || |
| (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) || |
| (mat->width != 1 && mat->height != 1)) |
| CV_ERROR( CV_StsBadArg, |
| "Input array must be 1d continuous array of 2d or 3d points" ); |
| |
| CV_CALL( ptseq = cvMakeSeqHeaderForArray( |
| CV_SEQ_KIND_GENERIC|type, sizeof(CvContour), CV_ELEM_SIZE(type), mat->data.ptr, |
| mat->width + mat->height - 1, &header.seq, &block )); |
| } |
| |
| if( reps < 0 || aeps < 0 ) |
| CV_ERROR( CV_StsOutOfRange, "Both reps and aeps must be non-negative" ); |
| |
| if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first ) |
| { |
| /* no need to copy data in this case */ |
| points = ptseq->first->data; |
| } |
| else |
| { |
| CV_CALL( buffer = points = (schar*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) )); |
| CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ )); |
| |
| if( CV_MAT_DEPTH(type) != CV_32F ) |
| { |
| int i, total = ptseq->total*CV_MAT_CN(type); |
| assert( CV_MAT_DEPTH(type) == CV_32S ); |
| |
| for( i = 0; i < total; i++ ) |
| ((float*)points)[i] = (float)((int*)points)[i]; |
| } |
| } |
| |
| if( dist == CV_DIST_USER ) |
| CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" ); |
| |
| if( CV_MAT_CN(type) == 2 ) |
| { |
| IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total, |
| dist, (float)param, (float)reps, (float)aeps, line )); |
| } |
| else |
| { |
| IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total, |
| dist, (float)param, (float)reps, (float)aeps, line )); |
| } |
| |
| __END__; |
| |
| cvFree( &buffer ); |
| } |
| |
| /* End of file. */ |