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.
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/PKG-INFO +2 -1
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/pyproject.toml +3 -2
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/math/quaternion.py +9 -0
- yostlabs-2025.10.6/src/yostlabs/tss3/utils/calibration.py +363 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/parser.py +1 -1
- yostlabs-2025.9.17/src/yostlabs/tss3/utils/calibration.py +0 -153
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/.gitignore +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/embedded_2024_dec_20.xml +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_ble.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_commands.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_component_specific_settings_and_commands.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_firmware_upload.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_parsing_stored_binary.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_read_settings.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_streaming.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_streaming_manager.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/Examples/example_write_settings.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/LICENSE +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/README.md +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/__init__.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/__init__.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/base.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/ble.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/communication/serial.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/math/__init__.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/math/vector.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/__init__.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/api.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/consts.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/eepts.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/__init__.py +0 -0
- {yostlabs-2025.9.17 → yostlabs-2025.10.6}/src/yostlabs/tss3/utils/streaming.py +0 -0
- {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.
|
|
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.
|
|
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
|
-
|
|
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
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|