def update(self):
"""Read the current sensor values & store them for smoothing. No return value."""
t = time()
delta_t = t - self._last_gyro_time
if delta_t < 0.020:
#Want at least 20ms of data
return
v_gyro = np.array(self.read_gyro(), np.float)
v_acc = np.array(self.read_accel(), np.float)
v_mag = np.array(self.read_compass(), np.float)
self._last_gyro_time = t
#Gyro only quaternion calculation (expected to drift)
rot_mag = sqrt(sum(v_gyro**2))
v_rotation = v_gyro / rot_mag
q_rotation = quaternion_from_axis_angle(v_rotation, rot_mag * delta_t)
self._current_gyro_only_q = quaternion_multiply(
self._current_gyro_only_q, q_rotation)
self._current_hybrid_orientation_q = quaternion_multiply(
self._current_hybrid_orientation_q, q_rotation)
if abs(sqrt(sum(v_acc**2)) - 1) < 0.3:
#Approx 1g, should be stationary, and can use this for down axis...
v_down = v_acc * -1.0
v_east = np.cross(v_down, v_mag)
v_north = np.cross(v_east, v_down)
v_down /= sqrt((v_down**2).sum())
v_east /= sqrt((v_east**2).sum())
v_north /= sqrt((v_north**2).sum())
#Complementary Filter
#Combine (noisy) orientation from acc/mag, 2%
#with (drifting) orientation from gyro, 98%
q_mag_acc = quaternion_from_rotation_matrix_rows(
v_north, v_east, v_down)
self._current_hybrid_orientation_q = tuple(
0.02 * a + 0.98 * b
for a, b in zip(q_mag_acc, self._current_hybrid_orientation_q))
#1st order approximation of quaternion for this rotation (v_rotation, delta_t)
#using small angle approximation, cos(theta) = 1, sin(theta) = theta
#w, x, y, z = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
#q_rotation = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
return
def update(self):
"""Read the current sensor values & store them for smoothing. No return value."""
t = time()
delta_t = t - self._last_gyro_time
if delta_t < 0.020:
#Want at least 20ms of data
return
v_gyro = np.array(self.read_gyro(), np.float)
v_acc = np.array(self.read_accel(), np.float)
v_mag = np.array(self.read_compass(), np.float)
self._last_gyro_time = t
#Gyro only quaternion calculation (expected to drift)
rot_mag = sqrt(sum(v_gyro**2))
v_rotation = v_gyro / rot_mag
q_rotation = quaternion_from_axis_angle(v_rotation, rot_mag * delta_t)
self._current_gyro_only_q = quaternion_multiply(self._current_gyro_only_q, q_rotation)
self._current_hybrid_orientation_q = quaternion_multiply(self._current_hybrid_orientation_q, q_rotation)
if abs(sqrt(sum(v_acc**2)) - 1) < 0.3:
#Approx 1g, should be stationary, and can use this for down axis...
v_down = v_acc * -1.0
v_east = np.cross(v_down, v_mag)
v_north = np.cross(v_east, v_down)
v_down /= sqrt((v_down**2).sum())
v_east /= sqrt((v_east**2).sum())
v_north /= sqrt((v_north**2).sum())
#Complementary Filter
#Combine (noisy) orientation from acc/mag, 2%
#with (drifting) orientation from gyro, 98%
q_mag_acc = quaternion_from_rotation_matrix_rows(v_north, v_east, v_down)
self._current_hybrid_orientation_q = tuple(0.02*a + 0.98*b for a, b in
zip(q_mag_acc, self._current_hybrid_orientation_q))
#1st order approximation of quaternion for this rotation (v_rotation, delta_t)
#using small angle approximation, cos(theta) = 1, sin(theta) = theta
#w, x, y, z = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
#q_rotation = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2)
return
def determine_camera_matrices(self):
center = self._center
forward = numpy.array(self._forward)
forward /= math.sqrt(forward[0]**2 + forward[1]**2 + forward[2]**2)
fov = self._fov
up = numpy.array(self._up)
up /= math.sqrt(up[0]**2 + up[1]**2 + up[2]**2)
rng = numpy.array(self._range)
# I'm not dealing with non-equal range properly
distback = rng[0] * math.tan(fov)
# Is this right?
self._position_of_camera = center - distback * forward
self._camtranslate = numpy.matrix([[1., 0., 0., -center[0]],
[0., 1., 0., -center[1]],
[0., 0., 1., -center[2]],
[0., 0., 0., 1.]])
# Figure out if the camera needs to point in another direction
costheta = -forward[2] # -ẑ · forward
if costheta < 1. - 1e-8:
if costheta > -1. + 1e-6:
rotabout = numpy.array([forward[1], -forward[0],
0.]) # -ẑ × forward
rotabout /= math.sqrt(rotabout[0]**2 + rotabout[1]**2 +
rotabout[2]**2)
costheta_2 = math.sqrt((1 + costheta) / 2.)
sintheta_2 = math.sqrt((1 - costheta) / 2.)
# Negative because we really rotate objects, not camera
q = numpy.array([
-sintheta_2 * rotabout[0], -sintheta_2 * rotabout[1],
-sintheta_2 * rotabout[2], costheta_2
])
else:
# π about ŷ
q = numpy.array([0., 1., 0., 0.])
else:
q = numpy.array([0., 0., 0., 1.])
# Rotate the up vector by this world rotation. (up is a unit vector, so rotup will be too)
rotup = quat.quaternion_rotate(up, q)
# Project it into the camera (x-y) plane.
rotup[2] = 0.
magrotup = math.sqrt(rotup[0]**2 + rotup[1]**2 + rotup[2]**2)
cosphi = 0.
if magrotup > 1e-8:
# punt if the vector is too parallel to forward
rotup /= magrotup
if rotup[0] > 0:
phiaxis = 1.
else:
phiaxis = -1.
# Rotate this up projection to the y-axis
cosphi = rotup[1]
cosphi_2 = math.sqrt((1 + cosphi) / 2.)
sinphi_2 = math.sqrt((1 - cosphi) / 2.)
q = quat.quaternion_multiply(
[0., 0., sinphi_2 * phiaxis, cosphi_2], q)
self._camrotate = numpy.array(
[[
1 - 2 * q[1]**2 - 2 * q[2]**2, 2 * q[0] * q[1] -
2 * q[2] * q[3], 2 * q[0] * q[2] + 2 * q[1] * q[3], 0.
],
[
2 * q[0] * q[1] + 2 * q[2] * q[3], 1 - 2 * q[0]**2 -
2 * q[2]**2, 2 * q[1] * q[2] - 2 * q[0] * q[3], 0.
],
[
2 * q[0] * q[2] - 2 * q[1] * q[3], 2 * q[1] * q[2] +
2 * q[0] * q[3], 1 - 2 * q[0]**2 - 2 * q[1]**2, -distback
], [0., 0., 0., 1.]],
dtype=numpy.float32)