当前位置：网站首页>OpenCV每日函数结构分析和形状描述符(8) fitLine函数拟合直线

OpenCV每日函数结构分析和形状描述符(8) fitLine函数拟合直线

2022-06-25 06:40:00 【坐望云起】

一、fitLine函数

1、函数原型

从 2D 或 3D 点集拟合到直线。函数 fitLine 通过最小化 $\sum_i \rho(r_i)$ 将线拟合到 2D 或 3D 点集，其中 r_i 是第个点之间的距离，线和 $\rho(r)$ 是距离函数，以下之一：

DIST_L2	$\rho (r) = r^2/2$ 最简单最快的最小二乘法
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

该算法基于 M-estimator技术，该技术使用加权最小二乘算法迭代拟合线。在每次迭代之后，权重 w_i 被调整为与 $\rho(r_i)$ 成反比。

2、参数详解

points	2D 或 3D 点的输入向量，存储在 std::vector<> 或 Mat.
line	输出线参数。在 2D 拟合的情况下，它应该是 4 个元素的向量（如 Vec4f） - (vx, vy, x0, y0)，其中 (vx, vy) 是与直线共线的归一化向量， (x0, y0) 是线上的一个点。在 3D 拟合的情况下，它应该是 6 个元素的向量（如 Vec6f） - (vx, vy, vz, x0, y0, z0)，其中 (vx, vy, vz) 是与直线共线的归一化向量，并且 (x0, y0, z0) 是线上的一个点。
distType	M 估计器使用的距离，请参阅DistanceTypes
param	某些类型的距离的数值参数 ( C )。如果为 0，则选择最优值。
reps	足够的半径精度（坐标原点和直线之间的距离）。
aeps	角度精确度。对于 reps 和 aeps 来说，0.01 是一个很好的默认值。

二、OpenCV源码

1、源码路径

opencv\modules\imgproc\src\linefit.cpp

2、源码代码

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);
}