yostlabs 2025.9.17__tar.gz → 2025.10.6__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (33) hide show
  1. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/PKG-INFO +2 -1
  2. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/pyproject.toml +3 -2
  3. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/math/quaternion.py +9 -0
  4. yostlabs-2025.10.6/src/yostlabs/tss3/utils/calibration.py +363 -0
  5. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/parser.py +1 -1
  6. yostlabs-2025.9.17/src/yostlabs/tss3/utils/calibration.py +0 -153
  7. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/.gitignore +0 -0
  8. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/embedded_2024_dec_20.xml +0 -0
  9. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_ble.py +0 -0
  10. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_commands.py +0 -0
  11. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_component_specific_settings_and_commands.py +0 -0
  12. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_firmware_upload.py +0 -0
  13. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_parsing_stored_binary.py +0 -0
  14. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_read_settings.py +0 -0
  15. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_streaming.py +0 -0
  16. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_streaming_manager.py +0 -0
  17. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_write_settings.py +0 -0
  18. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/LICENSE +0 -0
  19. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/README.md +0 -0
  20. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/__init__.py +0 -0
  21. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/__init__.py +0 -0
  22. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/base.py +0 -0
  23. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/ble.py +0 -0
  24. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/serial.py +0 -0
  25. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/math/__init__.py +0 -0
  26. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/math/vector.py +0 -0
  27. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/__init__.py +0 -0
  28. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/api.py +0 -0
  29. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/consts.py +0 -0
  30. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/eepts.py +0 -0
  31. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/__init__.py +0 -0
  32. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/streaming.py +0 -0
  33. {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/version.py +0 -0
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: yostlabs
3
- Version: 2025.9.17
3
+ Version: 2025.10.6
4
4
  Summary: Python resources and API for 3Space sensors from Yost Labs Inc.
5
5
  Project-URL: Homepage, https://yostlabs.com/
6
6
  Project-URL: Repository, https://github.com/YostLabs/3SpacePythonPackage/tree/main
@@ -13,6 +13,7 @@ Classifier: License :: OSI Approved :: MIT License
13
13
  Classifier: Operating System :: OS Independent
14
14
  Classifier: Programming Language :: Python :: 3
15
15
  Requires-Python: >=3.10
16
+ Requires-Dist: async-timeout
16
17
  Requires-Dist: bleak
17
18
  Requires-Dist: numpy
18
19
  Requires-Dist: pyserial
@@ -5,7 +5,7 @@ build-backend = "hatchling.build"
5
5
  [project]
6
6
  name = "yostlabs"
7
7
  #If uploading again on the same day, add a fourth number
8
- version = "2025.09.17"
8
+ version = "2025.10.06"
9
9
  authors = [
10
10
  { name="Yost Labs Inc.", email="techsupport@yostlabs.com" },
11
11
  { name="Andy Riedlinger", email="techsupport@yostlabs.com" },
@@ -23,7 +23,8 @@ keywords = ["3space", "threespace", "yost"]
23
23
  dependencies = [
24
24
  "pyserial",
25
25
  "numpy",
26
- "bleak"
26
+ "bleak",
27
+ "async-timeout"
27
28
  ]
28
29
 
29
30
  [project.urls]
@@ -51,6 +51,15 @@ def quat_from_axis_angle(axis: list[float], angle: float):
51
51
  quat.append(math.cos(angle / 2))
52
52
  return quat
53
53
 
54
+ #There are multiple valid quats that can be returned by this. The intention of this function
55
+ #is to be able to rotate an arrow by the quat such that it points the correct direction. The rotation
56
+ #of that arrow along its axis may differ though
57
+ def quat_from_one_vector(vec: list[float]):
58
+ vec = _vec.vec_normalize(vec)
59
+ perpendicular = _vec.vec_normalize(_vec.vec_cross([0, 0, 1], vec))
60
+ angle = math.acos(_vec.vec_dot([0, 0, 1], vec))
61
+ return quat_from_axis_angle(perpendicular, angle)
62
+
54
63
  def quat_from_two_vectors(forward: list[float], down: list[float]):
55
64
  """
56
65
  This function requires two orthogonal vectors to work
@@ -0,0 +1,363 @@
1
+ import yostlabs.math.quaternion as quat
2
+ import yostlabs.math.vector as vec
3
+
4
+ import numpy as np
5
+ from dataclasses import dataclass
6
+ import copy
7
+
8
+ class ThreespaceGradientDescentCalibration:
9
+
10
+ @dataclass
11
+ class StageInfo:
12
+ start_vector: int
13
+ end_vector: int
14
+ stage: int
15
+ scale: float
16
+
17
+ count: int = 0
18
+
19
+ MAX_SCALE = 1000000000
20
+ MIN_SCALE = 1
21
+ STAGES = [
22
+ StageInfo(0, 6, 0, MAX_SCALE),
23
+ StageInfo(0, 12, 1, MAX_SCALE),
24
+ StageInfo(0, 24, 2, MAX_SCALE)
25
+ ]
26
+
27
+ #Note that each entry has a positive and negative vector included in this list
28
+ CHANGE_VECTORS = [
29
+ np.array([0,0,0,0,0,0,0,0,0,.0001,0,0], dtype=np.float64),
30
+ np.array([0,0,0,0,0,0,0,0,0,-.0001,0,0], dtype=np.float64),
31
+ np.array([0,0,0,0,0,0,0,0,0,0,.0001,0], dtype=np.float64),
32
+ np.array([0,0,0,0,0,0,0,0,0,0,-.0001,0], dtype=np.float64),
33
+ np.array([0,0,0,0,0,0,0,0,0,0,0,.0001], dtype=np.float64),
34
+ np.array([0,0,0,0,0,0,0,0,0,0,0,-.0001], dtype=np.float64), #First 6 only try to change the bias
35
+ np.array([.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
36
+ np.array([-.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
37
+ np.array([0,0,0,0,.001,0,0,0,0,0,0,0], dtype=np.float64),
38
+ np.array([0,0,0,0,-.001,0,0,0,0,0,0,0], dtype=np.float64),
39
+ np.array([0,0,0,0,0,0,0,0,.001,0,0,0], dtype=np.float64),
40
+ np.array([0,0,0,0,0,0,0,0,-.001,0,0,0], dtype=np.float64), #Next 6 only try to change the scale
41
+ np.array([0,.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
42
+ np.array([0,-.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
43
+ np.array([0,0,.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
44
+ np.array([0,0,-.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
45
+ np.array([0,0,0,.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
46
+ np.array([0,0,0,-.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
47
+ np.array([0,0,0,0,0,.0001,0,0,0,0,0,0], dtype=np.float64),
48
+ np.array([0,0,0,0,0,-.0001,0,0,0,0,0,0], dtype=np.float64),
49
+ np.array([0,0,0,0,0,0,.0001,0,0,0,0,0], dtype=np.float64),
50
+ np.array([0,0,0,0,0,0,-.0001,0,0,0,0,0], dtype=np.float64),
51
+ np.array([0,0,0,0,0,0,0,.0001,0,0,0,0], dtype=np.float64),
52
+ np.array([0,0,0,0,0,0,0,-.0001,0,0,0,0], dtype=np.float64), #Next 12 only try to change the shear
53
+ ]
54
+
55
+ def __init__(self, relative_sensor_orients: list[np.ndarray[float]], no_inverse=False):
56
+ """
57
+ Params
58
+ ------
59
+ relative_sensor_orients : The orientation of the sensor during which each sample is taken if it was tared as if pointing into the screen.
60
+ The inverse of these will be used to calculate where the axes should be located relative to the sensor
61
+ no_inverse : The relative_sensor_orients will be treated as the sample_rotations
62
+ """
63
+ if no_inverse:
64
+ self.rotation_quats = relative_sensor_orients
65
+ else:
66
+ self.rotation_quats = [np.array(quat.quat_inverse(orient)) for orient in relative_sensor_orients]
67
+
68
+ def apply_parameters(self, sample: np.ndarray[float], params: np.ndarray[float]):
69
+ bias = params[9:]
70
+ scale = params[:9]
71
+ scale = scale.reshape((3, 3))
72
+ return scale @ (sample + bias)
73
+
74
+ def rate_parameters(self, params: np.ndarray[float], samples: list[np.ndarray[float]], targets: list[np.ndarray[float]]):
75
+ total_error = 0
76
+ for i in range(len(samples)):
77
+ sample = samples[i]
78
+ target = targets[i]
79
+
80
+ sample = self.apply_parameters(sample, params)
81
+
82
+ error = target - sample
83
+ total_error += vec.vec_len(error)
84
+ return total_error
85
+
86
+ def generate_target_list(self, origin: np.ndarray):
87
+ targets = []
88
+ for orient in self.rotation_quats:
89
+ new_vec = np.array(quat.quat_rotate_vec(orient, origin), dtype=np.float64)
90
+ targets.append(new_vec)
91
+ return targets
92
+
93
+ def __get_stage(self, stage_number: int):
94
+ if stage_number >= len(self.STAGES):
95
+ return None
96
+ #Always get a shallow copy of the stage so can modify without removing the initial values
97
+ return copy.copy(self.STAGES[stage_number])
98
+
99
+ def calculate(self, samples: list[np.ndarray[float]], origin: np.ndarray[float], verbose=False, max_cycles_per_stage=1000):
100
+ targets = self.generate_target_list(origin)
101
+ initial_params = np.array([1,0,0,0,1,0,0,0,1,0,0,0], dtype=np.float64)
102
+ stage = self.__get_stage(0)
103
+
104
+ best_params = initial_params
105
+ best_rating = self.rate_parameters(best_params, samples, targets)
106
+ count = 0
107
+ while True:
108
+ last_best_rating = best_rating
109
+ params = best_params
110
+
111
+ #Apply all the changes to see if any improve the result
112
+ for change_index in range(stage.start_vector, stage.end_vector):
113
+ change_vector = self.CHANGE_VECTORS[change_index]
114
+ new_params = params + (change_vector * stage.scale)
115
+ rating = self.rate_parameters(new_params, samples, targets)
116
+
117
+ #A better rating, store it
118
+ if rating < best_rating:
119
+ best_params = new_params
120
+ best_rating = rating
121
+
122
+ if verbose and count % 100 == 0:
123
+ print(f"Round {count}: {best_rating=} {stage=}")
124
+
125
+ #Decide if need to go to the next stage or not
126
+ count += 1
127
+ stage.count += 1
128
+ if stage.count >= max_cycles_per_stage:
129
+ stage = self.__get_stage(stage.stage + 1)
130
+ if stage is None:
131
+ if verbose: print("Done from reaching count limit")
132
+ break
133
+ if verbose: print("Going to next stage from count limit")
134
+
135
+ if best_rating == last_best_rating: #The rating did not improve
136
+ if stage.scale == self.MIN_SCALE: #Go to the next stage since can't get any better in this stage!
137
+ stage = self.__get_stage(stage.stage + 1)
138
+ if stage is None:
139
+ if verbose: print("Done from exhaustion")
140
+ break
141
+ if verbose: print("Going to next stage from exhaustion")
142
+ else: #Reduce the size of the changes to hopefully get more accurate tuning
143
+ stage.scale *= 0.1
144
+ if stage.scale < self.MIN_SCALE:
145
+ stage.scale = self.MIN_SCALE
146
+ else: #Rating got better! To help avoid falling in a local minimum, increase the size of the change to see if that could make it better
147
+ stage.scale *= 1.1
148
+
149
+ if verbose:
150
+ print(f"Final Rating: {best_rating}")
151
+ print(f"Final Params: {best_params}")
152
+
153
+ return best_params
154
+
155
+ def fibonacci_sphere(samples=1000):
156
+ points = []
157
+ phi = np.pi * (3. - np.sqrt(5.)) # golden angle
158
+
159
+ for i in range(samples):
160
+ y = 1 - (i / float(samples - 1)) * 2 # y goes from 1 to -1
161
+ radius = np.sqrt(1 - y * y)
162
+ theta = phi * i
163
+ x = np.cos(theta) * radius
164
+ z = np.sin(theta) * radius
165
+ points.append((x, y, z))
166
+
167
+ return np.array(points)
168
+
169
+ class ThreespaceSphereCalibration:
170
+
171
+ def __init__(self, max_comparison_points=500):
172
+ self.buffer = 0.1
173
+ self.test_points = fibonacci_sphere(samples=max_comparison_points)
174
+ self.clear()
175
+
176
+ def process_point(self, raw_mag: list[float]):
177
+ if len(self.samples) == 0:
178
+ self.samples = np.array([raw_mag])
179
+ return True
180
+
181
+ raw_mag = np.array(raw_mag, dtype=np.float64)
182
+ new_len = np.linalg.norm(raw_mag)
183
+
184
+ avg_len = np.linalg.norm(self.samples, axis=1)
185
+ avg_len = (avg_len + new_len) / 2 * self.buffer
186
+
187
+ dist = np.linalg.norm(self.samples - raw_mag, axis=1)
188
+ if np.any(dist < avg_len):
189
+ return False
190
+
191
+ self.samples = np.concatenate((self.samples, [raw_mag]))
192
+ self.__update_density(raw_mag / new_len)
193
+ return True
194
+
195
+ def __update_density(self, normalized_point: np.ndarray):
196
+ #First check to see if the new point is the closest point for any of the previous points
197
+ dots = np.sum(self.test_points * normalized_point, axis=1)
198
+ self.closest_dot = np.maximum(self.closest_dot, dots)
199
+ self.largest_delta_index = np.argmin(self.closest_dot)
200
+ self.largest_delta = np.rad2deg(np.acos(self.closest_dot[self.largest_delta_index]))
201
+
202
+ def clear(self):
203
+ self.samples = np.array([])
204
+ self.closest_dot = np.array([-1] * len(self.test_points))
205
+ self.largest_delta = 180
206
+ self.largest_delta_index = 0
207
+
208
+ @property
209
+ def sparsest_vector(self):
210
+ return self.test_points[self.largest_delta_index]
211
+
212
+ def calculate(self):
213
+ """
214
+ Returns matrix and bias
215
+ """
216
+ comp_A, comp_b, comp_d = ThreespaceSphereCalibration.alternating_least_squares(np.array(self.samples))
217
+ comp_A, comp_b = ThreespaceSphereCalibration.make_calibration_params(comp_A, comp_b, comp_d, 1)
218
+
219
+ comp_scale: float = comp_A[0][0] + comp_A[1][1] + comp_A[2][2]
220
+ comp_scale /= 3
221
+
222
+ comp_A /= comp_scale
223
+
224
+ return comp_A.flatten().tolist(), comp_b.tolist()
225
+
226
+ @staticmethod
227
+ def alternating_least_squares(data: np.ndarray[np.ndarray]) -> tuple[np.ndarray, np.ndarray, float]:
228
+ n = 3
229
+ m = len(data)
230
+
231
+ #Step 1
232
+ noise_variance = 0.1
233
+ sigma2 = noise_variance ** 2
234
+ rotini = data.T
235
+ rotini2 = rotini ** 2
236
+
237
+ T = np.zeros((5,n,m))
238
+ T[0,:,:] = 1
239
+ T[1,:,:] = rotini
240
+ T[2,:,:] = rotini2 - sigma2
241
+ T[3,:,:] = rotini2 * rotini - 3 * rotini * sigma2
242
+ T[4,:,:] = rotini2 * rotini2 - 6 * rotini2 * sigma2 + 3 * sigma2 * sigma2
243
+
244
+ #Step 2
245
+ one = np.ones((n+1,))
246
+ i = np.arange(1, n+1)
247
+ i = np.append(i, 0)
248
+
249
+ m1 = np.outer(i.T, one)
250
+ m2 = np.outer(one.T, i)
251
+
252
+ M = np.array([ThreespaceSphereCalibration.vec_s(m1), ThreespaceSphereCalibration.vec_s(m2)])
253
+
254
+ #Step 3
255
+ nb = int((n + 1) * n / 2 + n + 1)
256
+ R = np.zeros((nb,nb,n), dtype=np.int32)
257
+
258
+ for p in range(nb):
259
+ for q in range(p, nb):
260
+ for i in range(1, n+1):
261
+ R[p,q,i-1] = int(M[0,p] == i) + int(M[1,p] == i) + int(M[0,q] == i) + int(M[1,q] == i)
262
+
263
+ #Step 4
264
+ nals = np.zeros((nb, nb))
265
+ for p in range(nb):
266
+ for q in range(p, nb):
267
+ sum = 0
268
+ for l in range(m):
269
+ prod = 1
270
+ for i in range(n):
271
+ prod *= T[R[p,q,i],i,l]
272
+ sum += prod
273
+ nals[p,q] = sum
274
+
275
+ #Step 5
276
+ D2 = [i * (i + 1) / 2 for i in range(1, n+1)]
277
+ D = [d for d in range(1, (n+1) * n // 2 + 1) if not d in D2]
278
+
279
+ #Step 6
280
+ menorah_als = np.zeros((nb, nb))
281
+
282
+ for p in range(nb):
283
+ for q in range(p, nb):
284
+ coeff = 2
285
+ if p + 1 in D and q + 1 in D:
286
+ coeff = 4
287
+ elif not p + 1 in D and not q + 1 in D:
288
+ coeff = 1
289
+ menorah_als[p,q] = coeff * nals[p,q]
290
+
291
+ # Fill the lower triangle with the upper triangle values
292
+ i_lower, j_lower = np.tril_indices(nb, k=-1)
293
+ menorah_als[i_lower, j_lower] = menorah_als[j_lower, i_lower]
294
+
295
+ #Step 7
296
+ eigenmat = menorah_als
297
+
298
+ #It is unclear if this is correct, there are differences in sign and positions
299
+ eigenvalues, eigenvectors = np.linalg.eig(eigenmat)
300
+ eigenvectors = eigenvectors.T
301
+
302
+ #Looks like this section gets the eigen vector with the largest eigen value?
303
+ combinedmatr = [[abs(eigenvalues[i]), eigenvectors[i]] for i in range(len(eigenvalues))]
304
+ combinedmatr.sort(key=lambda a: a[0], reverse=True)
305
+
306
+ bals = combinedmatr[-1][1]
307
+
308
+ #Step 8 : ensure normalized
309
+ bals = bals / np.linalg.norm(bals)
310
+
311
+ #Step 9:
312
+ triangle = n*(n+1)//2
313
+ A = ThreespaceSphereCalibration.inv_vec_s(bals[:triangle])
314
+ b = bals[triangle:nb-1]
315
+ d = bals[-1]
316
+
317
+ return A, b, d
318
+
319
+ @staticmethod
320
+ def make_calibration_params(Q: np.ndarray, u: np.ndarray, k: float, H_m: float) -> tuple[np.ndarray,np.ndarray]:
321
+ pa = np.linalg.inv(Q)
322
+ pb = u.T
323
+ b = np.dot(pa, pb) * 0.5
324
+
325
+ eigenvalues, V = np.linalg.eig(Q)
326
+ D = np.diag(eigenvalues)
327
+
328
+ vu_prod = np.dot(V.T, u.T)
329
+ p1a = np.dot(vu_prod.T, np.linalg.inv(D))
330
+ p1b = np.dot(p1a, vu_prod)
331
+ p1 = p1b - (4 * k)
332
+
333
+ alpha = 4 * (H_m ** 2) / p1
334
+
335
+ aD = np.diag(abs(alpha * eigenvalues) ** 0.5)
336
+
337
+ A = np.dot(np.dot(V, aD), V.T)
338
+
339
+ return A, b
340
+
341
+ @staticmethod
342
+ def vec_s(matrix: np.ndarray):
343
+ rows, cols = np.tril_indices(matrix.shape[0])
344
+ return matrix[rows, cols]
345
+
346
+ @staticmethod
347
+ def inv_vec_s(vec: np.ndarray):
348
+ #Its unclear if this function works as intended. But this is how the suite does it.
349
+ size = int((-1 + (1 + 8 * len(vec)) ** 0.5) / 2)
350
+ matr = np.zeros((size,size))
351
+ base = 0
352
+ for i in range(size):
353
+ for j in range(i):
354
+ matr[i,j] = vec[base + j]
355
+ matr[j,i] = vec[base + j]
356
+ matr[i,i] = vec[base + i]
357
+ base += i + 1
358
+
359
+ return matr
360
+
361
+ @property
362
+ def num_points(self):
363
+ return len(self.samples)
@@ -27,7 +27,7 @@ class ThreespaceBufferInputStream(ThreespaceInputStream):
27
27
  length = self.buffer.index(expected) + len(expected)
28
28
  result = self.buffer[:length]
29
29
  del self.buffer[:length]
30
- raise result
30
+ return result
31
31
 
32
32
  """Allows reading without removing the data from the buffer"""
33
33
  def peek(self, num_bytes) -> bytes:
@@ -1,153 +0,0 @@
1
- import yostlabs.math.quaternion as quat
2
- import yostlabs.math.vector as vec
3
-
4
- import numpy as np
5
- from dataclasses import dataclass
6
- import copy
7
-
8
- class ThreespaceGradientDescentCalibration:
9
-
10
- @dataclass
11
- class StageInfo:
12
- start_vector: int
13
- end_vector: int
14
- stage: int
15
- scale: float
16
-
17
- count: int = 0
18
-
19
- MAX_SCALE = 1000000000
20
- MIN_SCALE = 1
21
- STAGES = [
22
- StageInfo(0, 6, 0, MAX_SCALE),
23
- StageInfo(0, 12, 1, MAX_SCALE),
24
- StageInfo(0, 24, 2, MAX_SCALE)
25
- ]
26
-
27
- #Note that each entry has a positive and negative vector included in this list
28
- CHANGE_VECTORS = [
29
- np.array([0,0,0,0,0,0,0,0,0,.0001,0,0], dtype=np.float64),
30
- np.array([0,0,0,0,0,0,0,0,0,-.0001,0,0], dtype=np.float64),
31
- np.array([0,0,0,0,0,0,0,0,0,0,.0001,0], dtype=np.float64),
32
- np.array([0,0,0,0,0,0,0,0,0,0,-.0001,0], dtype=np.float64),
33
- np.array([0,0,0,0,0,0,0,0,0,0,0,.0001], dtype=np.float64),
34
- np.array([0,0,0,0,0,0,0,0,0,0,0,-.0001], dtype=np.float64), #First 6 only try to change the bias
35
- np.array([.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
36
- np.array([-.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
37
- np.array([0,0,0,0,.001,0,0,0,0,0,0,0], dtype=np.float64),
38
- np.array([0,0,0,0,-.001,0,0,0,0,0,0,0], dtype=np.float64),
39
- np.array([0,0,0,0,0,0,0,0,.001,0,0,0], dtype=np.float64),
40
- np.array([0,0,0,0,0,0,0,0,-.001,0,0,0], dtype=np.float64), #Next 6 only try to change the scale
41
- np.array([0,.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
42
- np.array([0,-.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
43
- np.array([0,0,.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
44
- np.array([0,0,-.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
45
- np.array([0,0,0,.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
46
- np.array([0,0,0,-.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
47
- np.array([0,0,0,0,0,.0001,0,0,0,0,0,0], dtype=np.float64),
48
- np.array([0,0,0,0,0,-.0001,0,0,0,0,0,0], dtype=np.float64),
49
- np.array([0,0,0,0,0,0,.0001,0,0,0,0,0], dtype=np.float64),
50
- np.array([0,0,0,0,0,0,-.0001,0,0,0,0,0], dtype=np.float64),
51
- np.array([0,0,0,0,0,0,0,.0001,0,0,0,0], dtype=np.float64),
52
- np.array([0,0,0,0,0,0,0,-.0001,0,0,0,0], dtype=np.float64), #Next 12 only try to change the shear
53
- ]
54
-
55
- def __init__(self, relative_sensor_orients: list[np.ndarray[float]], no_inverse=False):
56
- """
57
- Params
58
- ------
59
- relative_sensor_orients : The orientation of the sensor during which each sample is taken if it was tared as if pointing into the screen.
60
- The inverse of these will be used to calculate where the axes should be located relative to the sensor
61
- no_inverse : The relative_sensor_orients will be treated as the sample_rotations
62
- """
63
- if no_inverse:
64
- self.rotation_quats = relative_sensor_orients
65
- else:
66
- self.rotation_quats = [np.array(quat.quat_inverse(orient)) for orient in relative_sensor_orients]
67
-
68
- def apply_parameters(self, sample: np.ndarray[float], params: np.ndarray[float]):
69
- bias = params[9:]
70
- scale = params[:9]
71
- scale = scale.reshape((3, 3))
72
- return scale @ (sample + bias)
73
-
74
- def rate_parameters(self, params: np.ndarray[float], samples: list[np.ndarray[float]], targets: list[np.ndarray[float]]):
75
- total_error = 0
76
- for i in range(len(samples)):
77
- sample = samples[i]
78
- target = targets[i]
79
-
80
- sample = self.apply_parameters(sample, params)
81
-
82
- error = target - sample
83
- total_error += vec.vec_len(error)
84
- return total_error
85
-
86
- def generate_target_list(self, origin: np.ndarray):
87
- targets = []
88
- for orient in self.rotation_quats:
89
- new_vec = np.array(quat.quat_rotate_vec(orient, origin), dtype=np.float64)
90
- targets.append(new_vec)
91
- return targets
92
-
93
- def __get_stage(self, stage_number: int):
94
- if stage_number >= len(self.STAGES):
95
- return None
96
- #Always get a shallow copy of the stage so can modify without removing the initial values
97
- return copy.copy(self.STAGES[stage_number])
98
-
99
- def calculate(self, samples: list[np.ndarray[float]], origin: np.ndarray[float], verbose=False, max_cycles_per_stage=1000):
100
- targets = self.generate_target_list(origin)
101
- initial_params = np.array([1,0,0,0,1,0,0,0,1,0,0,0], dtype=np.float64)
102
- stage = self.__get_stage(0)
103
-
104
- best_params = initial_params
105
- best_rating = self.rate_parameters(best_params, samples, targets)
106
- count = 0
107
- while True:
108
- last_best_rating = best_rating
109
- params = best_params
110
-
111
- #Apply all the changes to see if any improve the result
112
- for change_index in range(stage.start_vector, stage.end_vector):
113
- change_vector = self.CHANGE_VECTORS[change_index]
114
- new_params = params + (change_vector * stage.scale)
115
- rating = self.rate_parameters(new_params, samples, targets)
116
-
117
- #A better rating, store it
118
- if rating < best_rating:
119
- best_params = new_params
120
- best_rating = rating
121
-
122
- if verbose and count % 100 == 0:
123
- print(f"Round {count}: {best_rating=} {stage=}")
124
-
125
- #Decide if need to go to the next stage or not
126
- count += 1
127
- stage.count += 1
128
- if stage.count >= max_cycles_per_stage:
129
- stage = self.__get_stage(stage.stage + 1)
130
- if stage is None:
131
- if verbose: print("Done from reaching count limit")
132
- break
133
- if verbose: print("Going to next stage from count limit")
134
-
135
- if best_rating == last_best_rating: #The rating did not improve
136
- if stage.scale == self.MIN_SCALE: #Go to the next stage since can't get any better in this stage!
137
- stage = self.__get_stage(stage.stage + 1)
138
- if stage is None:
139
- if verbose: print("Done from exhaustion")
140
- break
141
- if verbose: print("Going to next stage from exhaustion")
142
- else: #Reduce the size of the changes to hopefully get more accurate tuning
143
- stage.scale *= 0.1
144
- if stage.scale < self.MIN_SCALE:
145
- stage.scale = self.MIN_SCALE
146
- else: #Rating got better! To help avoid falling in a local minimum, increase the size of the change to see if that could make it better
147
- stage.scale *= 1.1
148
-
149
- if verbose:
150
- print(f"Final Rating: {best_rating}")
151
- print(f"Final Params: {best_params}")
152
-
153
- return best_params
File without changes
File without changes
File without changes