-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgraph_gen.py
211 lines (207 loc) · 5.52 KB
/
graph_gen.py
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
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
#!/usr/bin/python3
# -*- coding: utf-8 -*-
tagMap = {
1: ( 2, 0.587, 111, 0.487),
2: ( 3, 0.217, 11, 0.881),
3: ( 4, 0.145, 112, 0.375),
4: (101, 0.300, 6, 0.376),
5: (103, 0.340, 106, 0.315),
6: ( 13, 0.447, 117, 0.253),
7: (108, 0.341, 114, 0.222),
8: (113, 0.265, 7, 0.261),
9: ( 0, 0.000, 139, 0.985),
10: ( 0, 0.000, 131, 0.459),
11: (129, 0.410, 126, 0.260),
12: (127, 0.262, 128, 0.275),
13: (136, 0.605, 0, 0.000),
14: (121, 0.197, 15, 0.357),
15: (124, 0.309, 123, 0.247),
16: ( 8, 0.360, 14, 0.180),
17: ( 16, 0.233, 125, 0.487),
18: (134, 0.285, 17, 0.290, 132, 0.245),
19: (137, 0.302, 141, 0.220),
20: (138, 0.295, 133, 0.595),
101: (102, 0.300, 0, 0.000),
102: ( 5, 0.280, 0, 0.000),
103: (104, 0.225, 0, 0.000),
104: ( 1, 1.910, 0, 0.000),
105: ( 0, 0.000, 102, 0.163),
106: ( 0, 0.000, 0, 0.000, 109, 0.163),
107: ( 0, 0.000, 104, 0.202),
108: ( 0, 0.000, 0, 0.000, 107, 0.163),
109: (116, 0.207, 0, 0.000),
110: ( 0, 0.000, 1, 1.998),
111: ( 10, 0.600, 9, 0.520),
112: ( 0, 0.000, 12, 0.437),
113: ( 0, 0.000, 5, 0.415),
114: (115, 0.155, 0, 0.000),
115: ( 0, 0.000, 116, 0.220),
116: (110, 0.207, 0, 0.000),
117: ( 12, 0.273, 0, 0.000),
118: ( 0, 0.000, 110, 0.310),
119: ( 0, 0.000, 115, 0.352),
120: (118, 0.374, 0, 0.000),
121: (119, 0.203, 120, 0.298),
122: ( 0, 0.000, 118, 0.320),
123: (130, 0.190, 0, 0.000),
124: (122, 0.183, 0, 0.000),
125: ( 0, 0.000, 130, 0.420),
126: (131, 0.288, 0, 0.000),
127: ( 0, 0.000, 136, 0.433),
128: (135, 0.340, 0, 0.000),
129: ( 0, 0.000, 135, 0.288),
130: ( 0, 0.000, 18, 0.472),
131: (139, 0.615, 0, 0.000),
132: ( 0, 0.000, 0, 0.000, 143, 1.103),
133: ( 0, 0.000, 122, 0.498),
134: ( 20, 0.540, 0, 0.000),
135: (140, 0.267, 0, 0.000),
136: ( 19, 0.462, 0, 0.000),
137: ( 0, 0.000, 0, 0.000, 142, 1.225),
138: ( 18, 0.550, 0, 0.000),
139: ( 0, 0.000, 140, 0.337),
140: ( 0, 0.000, 19, 0.340),
141: ( 0, 0.000, 20, 0.460),
142: (105, 0.153, 0, 0.000),
143: ( 0, 0.000, 105, 0.157),
}
tagPoses = {
1: ( 0.389, 1.720, 3.068),
2: ( 0.205, 1.494,-0.491),
3: ( 0.348, 1.570, 0.080),
4: ( 0.490, 1.574, 0.015),
5: ( 1.373, 1.574, 0.016),
6: ( 0.706, 1.398,-1.292),
7: ( 1.375, 1.215, 1.061),
8: ( 1.324, 0.999, 1.556),
9: ( 0.124, 0.949,-1.555),
10: ( 0.275, 0.955,-1.581),
11: ( 0.425, 0.958,-1.581),
12: ( 0.575, 0.960,-1.571),
13: ( 0.725, 0.953,-1.571),
14: ( 1.380, 0.772, 0.948),
15: ( 1.711, 0.822, 0.047),
16: ( 1.327, 0.639, 1.601),
17: ( 1.368, 0.433, 2.406),
18: ( 1.613, 0.373, 3.110),
19: ( 0.947, 0.075, 0.000),
20: ( 1.627, 0.075, 0.000),
101: ( 0.856, 1.575),
102: ( 1.156, 1.575),
103: ( 1.776, 1.575),
104: ( 2.000, 1.575),
105: ( 1.025, 1.510),
106: ( 1.722, 1.490),
107: ( 1.825, 1.489),
108: ( 1.723, 1.361),
109: ( 1.824, 1.362),
110: ( 2.075, 1.446),
111: ( 0.125, 1.406),
112: ( 0.575, 1.331),
113: ( 1.325, 1.331),
114: ( 1.628, 1.275),
115: ( 1.786, 1.275),
116: ( 2.006, 1.275),
117: ( 0.650, 1.122),
118: ( 2.075, 1.136),
119: ( 1.624, 1.050),
120: ( 1.831, 0.975),
121: ( 1.606, 0.852),
122: ( 2.075, 0.817),
123: ( 1.925, 0.694),
124: ( 2.000, 0.674),
125: ( 1.644, 0.675),
126: ( 0.350, 0.674),
127: ( 0.650, 0.675),
128: ( 0.575, 0.619),
129: ( 0.500, 0.526),
130: ( 1.925, 0.506),
131: ( 0.275, 0.431),
132: ( 1.306, 0.375),
133: ( 2.075, 0.319),
134: ( 1.325, 0.281),
135: ( 0.575, 0.281),
136: ( 0.725, 0.284),
137: ( 1.175, 0.244),
138: ( 1.925, 0.169),
139: ( 0.331, 0.075),
140: ( 0.669, 0.075),
141: ( 1.231, 0.075),
142: ( 0.920, 1.400),
143: ( 1.130, 1.400),
}
tagDets = {
1: ( 0.324, 1.725),
2: ( 0.263, 1.463),
3: ( 0.410, 1.575),
4: ( 0.555, 1.575),
5: ( 1.435, 1.575),
6: ( 0.724, 1.337),
7: ( 1.408, 1.275),
8: ( 1.325, 1.065),
9: ( 0.125, 0.885),
10: ( 0.274, 0.889),
11: ( 0.424, 0.893),
12: ( 0.575, 0.895),
13: ( 0.725, 0.890),
14: ( 1.418, 0.824),
15: ( 1.775, 0.825),
16: ( 1.325, 0.705),
17: ( 1.325, 0.472),
18: ( 1.550, 0.375),
19: ( 1.010, 0.075),
20: ( 1.690, 0.075),
101: ( 0.855, 1.575),
102: ( 1.155, 1.575),
103: ( 1.775, 1.575),
104: ( 2.000, 1.575),
105: ( 1.027, 1.510),
106: ( 1.723, 1.489),
107: ( 1.825, 1.488),
108: ( 1.723, 1.361),
109: ( 1.825, 1.362),
110: ( 2.075, 1.445),
111: ( 0.125, 1.405),
112: ( 0.574, 1.332),
113: ( 1.325, 1.330),
114: ( 1.630, 1.275),
115: ( 1.785, 1.275),
116: ( 2.005, 1.275),
117: ( 0.651, 1.125),
118: ( 2.075, 1.135),
119: ( 1.625, 1.050),
120: ( 1.829, 0.974),
121: ( 1.607, 0.852),
122: ( 2.075, 0.815),
123: ( 1.925, 0.695),
124: ( 1.999, 0.675),
125: ( 1.642, 0.673),
126: ( 0.351, 0.675),
127: ( 0.649, 0.675),
128: ( 0.575, 0.620),
129: ( 0.499, 0.525),
130: ( 1.925, 0.505),
131: ( 0.275, 0.430),
132: ( 1.305, 0.375),
133: ( 2.073, 0.317),
134: ( 1.326, 0.282),
135: ( 0.575, 0.280),
136: ( 0.725, 0.285),
137: ( 1.174, 0.245),
138: ( 1.924, 0.168),
139: ( 0.333, 0.077),
140: ( 0.670, 0.075),
141: ( 1.230, 0.075),
142: ( 0.920, 1.403),
143: ( 1.133, 1.399),
}
def findClosestNode(p, nodes=tagDets):
opt = (None, 1.0**2)
for k, v in nodes.items():
d = (v[0] - p[0])**2 + (v[1] - p[1])**2
if d < opt[1]:
opt = (k, d)
if opt[0] is not None:
return (opt[0], nodes[opt[0]])
else:
return None