/* 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 void fitLine3D( Point3f * 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;
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;
RNG rng((uint64)-1);
switch (dist)
{
case CV_DIST_L2:
return fitLine3D_wods( points, count, 0, line );
case CV_DIST_L1:
calc_weights = weightL1;
break;
case CV_DIST_L12:
calc_weights = weightL12;
break;
case CV_DIST_FAIR:
calc_weights_param = weightFair;
break;
case CV_DIST_WELSCH:
calc_weights_param = weightWelsch;
break;
case CV_DIST_HUBER:
calc_weights_param = weightHuber;
break;
default:
CV_Error(CV_StsBadArg, "Unknown distance");
}
AutoBuffer<float> buf(count*2);
float *w = buf, *r = w + count;
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 = rng.uniform(0, count);
if( w[j] < FLT_EPSILON )
{
w[j] = 1.f;
i++;
}
}
fitLine3D_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 */
err = calcDist3D( points, count, _line, r );
//if( err < 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... */
fitLine3D_wods( points, count, w, _line );
}
if( err < min_err )
{
min_err = err;
memcpy( line, _line, 6 * sizeof(line[0]));
if( err < EPS )
break;
}
}
}
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;
}
static void fitLine2D( const Point2f * 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;
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;
RNG rng((uint64)-1);
memset( line, 0, 4*sizeof(line[0]) );
switch (dist)
{
case CV_DIST_L2:
return fitLine2D_wods( points, count, 0, line );
case CV_DIST_L1:
calc_weights = weightL1;
break;
case CV_DIST_L12:
calc_weights = weightL12;
break;
case CV_DIST_FAIR:
calc_weights_param = weightFair;
break;
case CV_DIST_WELSCH:
calc_weights_param = weightWelsch;
break;
case CV_DIST_HUBER:
calc_weights_param = weightHuber;
break;
/*case DIST_USER:
calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
break;*/
default:
CV_Error(CV_StsBadArg, "Unknown distance type");
}
AutoBuffer<float> wr(count*2);
float *w = wr, *r = w + count;
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 = rng.uniform(0, count);
if( w[j] < FLT_EPSILON )
{
w[j] = 1.f;
i++;
}
}
fitLine2D_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 = calcDist2D( 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... */
fitLine2D_wods( points, count, w, _line );
}
if( err < min_err )
{
min_err = err;
memcpy( line, _line, 4 * sizeof(line[0]));
if( err < EPS )
break;
}
}
}
