[OpenCV]直线拟合

 OpenCV实现了直线的拟合。

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
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 ));
    }
}

　　

 二维的直线拟合？

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
/* 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 }
icvFitLine2D_wods
复制代码

权重计算方法

复制代码
 1 static void icvWeightL1( float *d, int count, float *w )
 2 {
 3     int i;
 4 
 5     for( i = 0; i < count; i++ )
 6     {
 7         double t = fabs( (double) d[i] );
 8         w[i] = (float)(1. / MAX(t, eps));
 9     }
10 }
11 
12 static void icvWeightL12( float *d, int count, float *w )
13 {
14     int i;
15 
16     for( i = 0; i < count; i++ )
17     {
18         w[i] = 1.0f / (float) sqrt( 1 + (double) (d[i] * d[i] * 0.5) );
19     }
20 }
21 
22 
23 static void icvWeightHuber( float *d, int count, float *w, float _c )
24 {
25     int i;
26     const float c = _c <= 0 ? 1.345f : _c;
27 
28     for( i = 0; i < count; i++ )
29     {
30         if( d[i] < c )
31             w[i] = 1.0f;
32         else
33             w[i] = c/d[i];
34     }
35 }
36 
37 
38 static void icvWeightFair( float *d, int count, float *w, float _c )
39 {
40     int i;
41     const float c = _c == 0 ? 1 / 1.3998f : 1 / _c;
42 
43     for( i = 0; i < count; i++ )
44     {
45         w[i] = 1 / (1 + d[i] * c);
46     }
47 }
48 
49 static void icvWeightWelsch( float *d, int count, float *w, float _c )
50 {
51     int i;
52     const float c = _c == 0 ? 1 / 2.9846f : 1 / _c;
53 
54     for( i = 0; i < count; i++ )
55     {
56         w[i] = (float) exp( -d[i] * d[i] * c * c );
57     }
58 }
复制代码

 

三维的直线拟合？

?
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
/* 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 }
icvFitLine3D_wods
复制代码

 

参考文献：

OpenCV 学习（直线拟合)

posted @   太一吾鱼水  阅读(1610)  评论(0)  编辑  收藏  举报
编辑推荐：
· 基于Microsoft.Extensions.AI核心库实现RAG应用
· Linux系列：如何用heaptrack跟踪.NET程序的非托管内存泄露
· 开发者必知的日志记录最佳实践
· SQL Server 2025 AI相关能力初探
· Linux系列：如何用 C#调用 C方法造成内存泄露
阅读排行：
· 震惊！C++程序真的从main开始吗？99%的程序员都答错了
· 【硬核科普】Trae如何「偷看」你的代码？零基础破解AI编程运行原理
· 单元测试从入门到精通
· 上周热点回顾（3.3-3.9）
· Vue3状态管理终极指南：Pinia保姆级教程
点击右上角即可分享
微信分享提示
AI IDE Trae