mini车间与模组车间区分功能

LWQ

2025-07-14 52d230fd0eb38adc5c6f4c6d6ed3786a3c19354d

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
ÿþ#include "StdAfx.h" 
#include <math.h> 
#include "RANSAC_LineFittingAlgorithm.h" 
 
 
CLineFitting::CLineFitting() 
{ 
 
} 
 
CLineFitting::~CLineFitting() 
{ 
 
} 
 
bool CLineFitting::find_in_samples (sPoint *samples, int no_samples, sPoint *data) 
{ 
    for (int i=0; i<no_samples; ++i) { 
        if (samples[i].x == data->x && samples[i].y == data->y) { 
            return true; 
        } 
    } 
    return false; 
} 
 
void CLineFitting::get_samples (sPoint *samples, int no_samples, sPoint *data, int no_data) 
{ 
    // p³tÇ0ÑÐÅÁ Éõ¼´ÀÉ JŌ¬ N¬XÇ 4»‘ÇÇ HÁÕDÇ DÌèÍ\Õä². 
    for (int i=0; i<no_samples; ) { 
        int j = rand()%no_data; 
         
        if (!find_in_samples(samples, i, &data[j])) { 
            samples[i] = data[j]; 
            ++i; 
        } 
    }; 
} 
 
int CLineFitting::compute_model_parameter(sPoint samples[], int no_samples, sLine &model) 
{ 
    // PCA )¼ÝÂ<Ç\¸ ÁÉ Á ¨ºx³XÇ Ó|·Tº0Ñ|¹ Æ!Î\Õä². 
 
    double sx  = 0, sy  = 0; 
    double sxx = 0, syy = 0; 
    double sxy = 0, sw  = 0; 
 
    for(int i = 0; i<no_samples;++i) 
    { 
        double &x = samples[i].x; 
        double &y = samples[i].y; 
 
        sx  += x;     
        sy  += y; 
        sxx += x*x;  
        sxy += x*y; 
        syy += y*y; 
        sw  += 1; 
    } 
 
    //variance; 
    double vxx = (sxx - sx*sx/sw)/sw; 
    double vxy = (sxy - sx*sy/sw)/sw; 
    double vyy = (syy - sy*sy/sw)/sw; 
     
    //principal axis 
    double theta = atan2(2*vxy, vxx - vyy)/2; 
     
    model.mx = cos(theta); 
    model.my = sin(theta); 
     
    //center of mass(xc, yc) 
    model.sx = sx/sw; 
    model.sy = sy/sw; 
     
    //ÁÉ ÁXÇ )¼ÈÝÂ: sin(theta)*(x - sx) = cos(theta)*(y - sy); 
    return 1; 
} 
 
double CLineFitting::compute_distance(sLine &line, sPoint &x) 
{ 
    // \Õ È(x)\¸€½0Ñ ÁÉ Á(line)ÐÅ ´°°¹  ÁXÇ 8®tÇ(distance)|¹ Ä¬°À\Õä². 
 
    return fabs((x.x - line.sx)*line.my - (x.y - line.sy)*line.mx)/sqrt(line.mx*line.mx + line.my*line.my); 
} 
 
double CLineFitting::model_verification (sPoint *inliers, int *no_inliers, sLine &estimated_model, sPoint *data, int no_data, double distance_threshold) 
{ 
    *no_inliers = 0; 
 
    double cost = 0.; 
 
    for(int i=0; i<no_data; i++){ 
        // ÁÉ ÁÐÅ ´°°¹  ÁXÇ 8®tÇ|¹ Ä¬°À\Õä². 
        double distance = compute_distance(estimated_model, data[i]); 
     
        // Æ!δ ¨ºx³ÐÅÁ  Ç¨Ö\Õ p³tÇ0ÑxÇ ½¬°Æ,  Ç¨Ö\Õ p³tÇ0Ñ ÑÉiÕÐÅ T³\Õä². 
        if (distance < distance_threshold) { 
            cost += 1.; 
 
            inliers[*no_inliers] = data[i]; 
            ++(*no_inliers); 
        } 
    } 
 
    return cost; 
} 
 
double CLineFitting::ransac_line_fitting(sPoint *data, int no_data, sLine &model, double distance_threshold) 
{ 
    const int no_samples = 2; 
 
    if (no_data < no_samples) { 
        return 0.; 
    } 
 
    sPoint *samples = new sPoint[no_samples]; 
 
    int no_inliers = 0; 
    sPoint *inliers = new sPoint[no_data]; 
 
    sLine estimated_model; 
    double max_cost = 0.; 
 
    int max_iteration = (int)(1 + log(1. - 0.99)/log(1. - pow(0.5, no_samples))); 
 
    for (int i = 0; i<max_iteration; i++) { 
        // 1. hypothesis 
 
        // ÐÆø¼ p³tÇ0ÑÐÅÁ „ÇXÇ\¸ N¬XÇ HÁÕ p³tÇ0Ñ|¹ à¬x¹ä². 
        get_samples (samples, no_samples, data, no_data); 
 
        // tÇ p³tÇ0Ñ|¹ ÈÁÀ
ÈxÇ p³tÇ0Ñ\¸ ô¼à¬ ¨ºx³ Ó|·Tº0Ñ|¹ Æ!Î\Õä². 
        compute_model_parameter (samples, no_samples, estimated_model); 
 
        // 2. Verification 
 
        // ÐÆø¼ p³tÇ0Ѭ Æ!δ ¨ºx³ÐÅ ˜Ç Þ¹”²ÀÉ €¬¬À\Õä². 
        double cost = model_verification (inliers, &no_inliers, estimated_model, data, no_data, distance_threshold); 
 
        // Ì¹|Ç Æ!δ ¨ºx³tÇ ˜Ç Þ¹”²ä²tº, tÇ ¨ºx³ÐÅ ³\Õ  Ç¨Ö\Õ p³tÇ0Ñ\¸ ÈÀ\¸´Æ ¨ºx³DÇ l­\Õä². 
        if (max_cost < cost) { 
            max_cost = cost; 
     
            compute_model_parameter (inliers, no_inliers, model); 
        } 
    } 
     
    delete [] samples; 
    delete [] inliers; 
 
    return max_cost; 
}