def smooth_orientations_internal(self, times, orientation_list):
alpha = 1
smooth = self.get_user_option_value("smoothness")
if smooth > 0:
alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth)
smoothed_orientation = np.zeros(orientation_list.shape)
value = orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.single_slerp(value, orientation_list[i, :], alpha)
smoothed_orientation[i] = value
# reverse pass
start_orientations = np.zeros(orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.single_slerp(value2, smoothed_orientation[i, :],
alpha)
start_orientations[i] = value2
else:
start_orientations = np.array(orientation_list)
# swap around
start_orientations[:, [0, 1, 2, 3]] = start_orientations[:,
[1, 2, 3, 0]]
eul = Rotation(start_orientations).as_euler("zxy")
#plt.figure()
#plt.plot(eul[:,0])
#plt.plot(eul[:,1])
#plt.plot(eul[:,2])
#plt.show()
eul[:, 0] = self.get_user_option_value("horizon_angle") * np.pi / 180
#new_quat = Rotation.from_euler(["xyz", "zxy", "yzx", "xzy", "zyx", "yxz"][self.get_user_option_value("eul")], eul).as_quat()
new_quat = Rotation.from_euler("zxy", eul).as_quat()
new_quat[:, [0, 1, 2, 3]] = new_quat[:, [3, 0, 1, 2]]
return times, new_quat
def smooth_orientations_internal(self, times, orientation_list):
# To be overloaded
# https://en.wikipedia.org/wiki/Exponential_smoothing
# the smooth value corresponds to the time constant
alpha = 1
smooth = self.get_user_option_value("smoothness")
if smooth > 0:
alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth)
smoothed_orientation = np.zeros(orientation_list.shape)
value = orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.single_slerp(value, orientation_list[i, :], alpha)
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.single_slerp(value2, smoothed_orientation[i, :],
alpha)
smoothed_orientation2[i] = value2
# Test rotation lock (doesn't work)
#if test:
# from scipy.spatial.transform import Rotation
# for i in range(self.num_data_points):
# quat = smoothed_orientation2[i,:]
# eul = Rotation([quat[1], quat[2], quat[3], quat[0]]).as_euler("xyz")
# new_quat = Rotation.from_euler('xyz', [eul[0], eul[1], np.pi]).as_quat()
# smoothed_orientation2[i,:] = [new_quat[3], new_quat[0], new_quat[1], new_quat[2]]
return times, smoothed_orientation2
def get_interpolated_stab_transform_old(self, start=0, interval=1 / 29.97):
if self.smoothing_algo:
if self.smoothing_algo.bypass_external_processing:
print("Bypassing quaternion orientation integration")
time_list, smoothed_orientation = self.smoothing_algo.get_stabilize_transform(
self.gyro)
else:
time_list, smoothed_orientation = self.get_stabilize_transform(
)
else:
time_list, smoothed_orientation = self.get_stabilize_transform()
time = start
out_times = []
slerped_rotations = []
while time < 0:
slerped_rotations.append(smoothed_orientation[0])
out_times.append(time)
time += interval
while time_list[0] >= time:
slerped_rotations.append(smoothed_orientation[0])
out_times.append(time)
time += interval
for i in range(len(time_list) - 1):
while time_list[i] <= time < time_list[i + 1]:
# interpolate between two quaternions
weight = (time - time_list[i]) / (time_list[i + 1] -
time_list[i])
slerped_rotations.append(
quat.single_slerp(smoothed_orientation[i],
smoothed_orientation[i + 1], weight))
out_times.append(time)
time += interval
if time < time_list[i]:
# continue even if missing gyro data
slerped_rotations.append(smoothed_orientation[i])
out_times.append(time)
time += interval
return (out_times, slerped_rotations)
def get_interpolated_orientations(self, start=0, interval=1 / 29.97):
time_list, smoothed_orientation = self.get_orientations()
time = start
out_times = []
slerped_rotations = []
while time < 0:
slerped_rotations.append(smoothed_orientation[0])
out_times.append(time)
time += interval
while time_list[0] >= time:
slerped_rotations.append(smoothed_orientation[0])
out_times.append(time)
time += interval
for i in range(len(time_list) - 1):
while time_list[i] <= time < time_list[i + 1]:
# interpolate between two quaternions
weight = (time - time_list[i]) / (time_list[i + 1] -
time_list[i])
slerped_rotations.append(
quat.single_slerp(smoothed_orientation[i],
smoothed_orientation[i + 1], weight))
out_times.append(time)
time += interval
if time < time_list[i]:
# continue even if missing gyro data
slerped_rotations.append(smoothed_orientation[i])
out_times.append(time)
time += interval
self.interp_times = np.array(out_times)
self.interp_orientations = np.array(slerped_rotations)
return (self.interp_times, self.interp_orientations)
def smooth_orientations_internal(self, times, orientation_list):
# To be overloaded
# https://en.wikipedia.org/wiki/Exponential_smoothing
# the smooth value corresponds to the time constant
alpha = 1
smooth = self.get_user_option_value("smoothness")
print(f"Smoothing orientation with smoothness={smooth}")
smooth2 = min(smooth * 0.1, 0.1) # When outside zone
alpha2 = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth2)
if smooth > 0:
alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth)
smoothed_orientation = np.zeros(orientation_list.shape)
value = orientation_list[0, :]
rotlimit = self.get_user_option_value("rotlimit") * np.pi / 180
begin_curve = rotlimit * 0.6
# Forward pass
for i in range(self.num_data_points):
temp_value = quat.single_slerp(value, orientation_list[i, :],
alpha)
anglebetween = abs(
quat.angle_between(temp_value, orientation_list[i, :]))
if begin_curve < anglebetween <= rotlimit:
smoothinterp = smooth + (anglebetween - begin_curve) * (
smooth2 - smooth) / (rotlimit - begin_curve)
alphainterp = 1 - np.exp(
-(1 / self.gyro_sample_rate) / smoothinterp)
temp_value = quat.single_slerp(value, orientation_list[i, :],
alphainterp)
elif anglebetween > rotlimit: # new smoothed orientation over angle limit
temp_value = quat.single_slerp(value, orientation_list[i, :],
alpha2)
value = temp_value
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
temp_value2 = quat.single_slerp(value2, smoothed_orientation[i, :],
alpha)
anglebetween = abs(
quat.angle_between(temp_value2, orientation_list[i, :]))
#print(anglebetween, rotlimit)
if begin_curve < anglebetween <= rotlimit:
smoothinterp = smooth + (anglebetween - begin_curve) * (
smooth2 - smooth) / (rotlimit - begin_curve)
alphainterp = 1 - np.exp(
-(1 / self.gyro_sample_rate) / smoothinterp)
temp_value2 = quat.single_slerp(value2,
smoothed_orientation[i, :],
alphainterp)
elif anglebetween > rotlimit: # new smoothed orientation over angle limit
temp_value2 = quat.single_slerp(value2,
smoothed_orientation[i, :],
alpha2)
value2 = temp_value2
smoothed_orientation2[i] = value2
return times, smoothed_orientation2