OpenCV实现了直线的拟合。
CV_IMPL void
cvFitLine( const CvArr* array, int dist, double param,
double reps, double aeps, float *line )
{
cv::AutoBuffer<schar> buffer;
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" );
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
{
buffer.allocate(ptseq->total*CV_ELEM_SIZE(type));
points = buffer;
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 ));
}
}
二维的直线拟合？
/* 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;
}
1 static CvStatus icvFitLine2D_wods( CvPoint2D32f * points, int _count, float *weights, float *line ) 2 { 3 double x = 0, y = 0, x2 = 0, y2 = 0, xy = 0, w = 0; 4 double dx2, dy2, dxy; 5 int i; 6 int count = _count; 7 float t; 8 9 /* Calculating the average of x and y... */ 10 11 if( weights == 0 ) 12 { 13 for( i = 0; i < count; i += 1 ) 14 { 15 x += points[i].x; 16 y += points[i].y; 17 x2 += points[i].x * points[i].x; 18 y2 += points[i].y * points[i].y; 19 xy += points[i].x * points[i].y; 20 } 21 w = (float) count; 22 } 23 else 24 { 25 for( i = 0; i < count; i += 1 ) 26 { 27 x += weights[i] * points[i].x; 28 y += weights[i] * points[i].y; 29 x2 += weights[i] * points[i].x * points[i].x; 30 y2 += weights[i] * points[i].y * points[i].y; 31 xy += weights[i] * points[i].x * points[i].y; 32 w += weights[i]; 33 } 34 } 35 36 x /= w; 37 y /= w; 38 x2 /= w; 39 y2 /= w; 40 xy /= w; 41 42 dx2 = x2 - x * x; 43 dy2 = y2 - y * y; 44 dxy = xy - x * y; 45 46 t = (float) atan2( 2 * dxy, dx2 - dy2 ) / 2; 47 line[0] = (float) cos( t ); 48 line[1] = (float) sin( t ); 49 50 line[2] = (float) x; 51 line[3] = (float) y; 52 53 return CV_NO_ERR; 54 }
三维的直线拟合？
/* 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]={0,0,0,0,0,0}, _lineprev[6]={0,0,0,0,0,0};
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);
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;
}
1 static CvStatus icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line ) 2 { 3 int i; 4 float w0 = 0; 5 float x0 = 0, y0 = 0, z0 = 0; 6 float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0; 7 float dx2, dy2, dz2, dxy, dxz, dyz; 8 float *v; 9 float n; 10 float det[9], evc[9], evl[3]; 11 12 memset( evl, 0, 3*sizeof(evl[0])); 13 memset( evc, 0, 9*sizeof(evl[0])); 14 15 if( weights ) 16 { 17 for( i = 0; i < count; i++ ) 18 { 19 float x = points[i].x; 20 float y = points[i].y; 21 float z = points[i].z; 22 float w = weights[i]; 23 24 25 x2 += x * x * w; 26 xy += x * y * w; 27 xz += x * z * w; 28 y2 += y * y * w; 29 yz += y * z * w; 30 z2 += z * z * w; 31 x0 += x * w; 32 y0 += y * w; 33 z0 += z * w; 34 w0 += w; 35 } 36 } 37 else 38 { 39 for( i = 0; i < count; i++ ) 40 { 41 float x = points[i].x; 42 float y = points[i].y; 43 float z = points[i].z; 44 45 x2 += x * x; 46 xy += x * y; 47 xz += x * z; 48 y2 += y * y; 49 yz += y * z; 50 z2 += z * z; 51 x0 += x; 52 y0 += y; 53 z0 += z; 54 } 55 w0 = (float) count; 56 } 57 58 x2 /= w0; 59 xy /= w0; 60 xz /= w0; 61 y2 /= w0; 62 yz /= w0; 63 z2 /= w0; 64 65 x0 /= w0; 66 y0 /= w0; 67 z0 /= w0; 68 69 dx2 = x2 - x0 * x0; 70 dxy = xy - x0 * y0; 71 dxz = xz - x0 * z0; 72 dy2 = y2 - y0 * y0; 73 dyz = yz - y0 * z0; 74 dz2 = z2 - z0 * z0; 75 76 det[0] = dz2 + dy2; 77 det[1] = -dxy; 78 det[2] = -dxz; 79 det[3] = det[1]; 80 det[4] = dx2 + dz2; 81 det[5] = -dyz; 82 det[6] = det[2]; 83 det[7] = det[5]; 84 det[8] = dy2 + dx2; 85 86 /* Searching for a eigenvector of det corresponding to the minimal eigenvalue */ 87 #if 1 88 { 89 CvMat _det = cvMat( 3, 3, CV_32F, det ); 90 CvMat _evc = cvMat( 3, 3, CV_32F, evc ); 91 CvMat _evl = cvMat( 3, 1, CV_32F, evl ); 92 cvEigenVV( &_det, &_evc, &_evl, 0 ); 93 i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2); 94 } 95 #else 96 { 97 CvMat _det = cvMat( 3, 3, CV_32F, det ); 98 CvMat _evc = cvMat( 3, 3, CV_32F, evc ); 99 CvMat _evl = cvMat( 1, 3, CV_32F, evl ); 100 101 cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T ); 102 } 103 i = 2; 104 #endif 105 v = &evc[i * 3]; 106 n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] ); 107 n = (float)MAX(n, eps); 108 line[0] = v[0] / n; 109 line[1] = v[1] / n; 110 line[2] = v[2] / n; 111 line[3] = x0; 112 line[4] = y0; 113 line[5] = z0; 114 115 return CV_NO_ERR; 116 }
参考文献：