当前位置：网站首页>Opencv daily function structure analysis and shape descriptor (8) Fitline function fitting line

Opencv daily function structure analysis and shape descriptor (8) Fitline function fitting line

2022-06-25 07:56:00 【Sit and watch the clouds rise】

One 、fitLine function

1、 The function prototype

from 2D or 3D Point set fit to line . function fitLine By minimizing $\sum_i \rho(r_i)$ Fit lines to 2D or 3D Point set , among r_i It's No The distance between the points , Line and $\rho(r)$ Is the distance function , One of the following ：

DIST_L2	$\rho (r) = r^2/2$ The simplest and fastest least square method
DIST_L1	$\rho (r) = r$
DIST_L12	$\rho (r) = 2 \cdot ( \sqrt{1 + \frac{r^2}{2}} - 1)$
DIST_FAIR	$\rho \left (r \right ) = C^2 \cdot \left ( \frac{r}{C} - \log{\left(1 + \frac{r}{C}\right)} \right ) \quad \text{where} \quad C=1.3998$
DIST_WELSCH	$\rho \left (r \right ) = \frac{C^2}{2} \cdot \left ( 1 - \exp{\left(-\left(\frac{r}{C}\right)^2\right)} \right ) \quad \text{where} \quad C=2.9846$
DIST_HUBER

The algorithm is based on M-estimator technology , This technique uses the weighted least square algorithm to iterate the fitting line . After each iteration , The weight w_i Be adjusted to be the same as $\rho(r_i)$ In inverse proportion .

2、 Parameters,

points	2D or 3D The input vector of the point , Stored in std::vector<> or Mat.
line	Output line parameters . stay 2D In the case of fitting , It should be 4 Vectors of elements （ Such as Vec4f） - (vx, vy, x0, y0), among (vx, vy) Is a normalized vector collinear with a straight line , (x0, y0) yes A point on the line . stay 3D In the case of fitting , It should be 6 Vectors of elements （ Such as Vec6f） - (vx, vy, vz, x0, y0, z0), among (vx, vy, vz) Is a normalized vector collinear with a straight line , also (x0, y0, z0) Is a point on the line .
distType	M The distance used by the estimator , see also DistanceTypes
param	Numerical parameters for certain types of distances ( C ). If 0, Then select the optimal value .
reps	Sufficient radius accuracy （ The distance between the coordinate origin and the line ）.
aeps	Angular accuracy . about reps and aeps Come on ,0.01 Is a good default value .

Two 、OpenCV Source code

1、 Source path

opencv\modules\imgproc\src\linefit.cpp

2、 Source code

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[4], _lineprev[4];
    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.data(), *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 < min_err)
            {
                min_err = err;
                memcpy(line, _line, 4 * sizeof(line[0]));
                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;
        }
    }
}


/* 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.data(), *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 < min_err)
            {
                min_err = err;
                memcpy(line, _line, 6 * sizeof(line[0]));
                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, 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;
        }
    }
}

}

void cv::fitLine( InputArray _points, OutputArray _line, int distType,
                 double param, double reps, double aeps )
{
    CV_INSTRUMENT_REGION();

    Mat points = _points.getMat();

    float linebuf[6]={0.f};
    int npoints2 = points.checkVector(2, -1, false);
    int npoints3 = points.checkVector(3, -1, false);

    CV_Assert( npoints2 >= 0 || npoints3 >= 0 );

    if( points.depth() != CV_32F || !points.isContinuous() )
    {
        Mat temp;
        points.convertTo(temp, CV_32F);
        points = temp;
    }

    if( npoints2 >= 0 )
        fitLine2D( points.ptr<Point2f>(), npoints2, distType,
                   (float)param, (float)reps, (float)aeps, linebuf);
    else
        fitLine3D( points.ptr<Point3f>(), npoints3, distType,
                   (float)param, (float)reps, (float)aeps, linebuf);

    Mat(npoints2 >= 0 ? 4 : 6, 1, CV_32F, linebuf).copyTo(_line);
}