def run_tests():
# run some math helper tests
# Test vectorized quat from two unit vectors
v1 = np.random.uniform(-1, 1, (3, 100))
v2 = np.random.uniform(-1, 1, (3, 1))
v3 = np.random.uniform(-1, 1, (3, 1))
v1 /= norm(v1, axis=0)
v2 /= norm(v2)
v3 /= norm(v3)
# On a single vector
assert norm(
Quaternion(q_array_from_two_unit_vectors(v3, v2)).rot(v3) - v2) < 1e-8
# on a bunch of vectors
quat_array = q_array_from_two_unit_vectors(v2, v1)
for q, v in zip(quat_array.T, v1.T):
Quaternion(q[:, None]).rot(v2) - v[:, None]
# Test T_zeta
q2 = q_array_from_two_unit_vectors(e_z, v2)
assert norm(T_zeta(Quaternion(q2)).T.dot(v2)) < 1e-8
# Check derivative of T_zeta - This was giving me trouble
d_dTdq = np.zeros((2, 2))
q = Quaternion(np.random.uniform(-1, 1, (4, 1)))
q.arr[3] = 0.0
q.normalize()
x0 = T_zeta(q).T.dot(v2)
epsilon = 1e-6
I = np.eye(2) * epsilon
for i in range(2):
qplus = q_feat_boxplus(q, I[:, i, None])
xprime = T_zeta(qplus).T.dot(v2)
d_dTdq[i, :, None] = (xprime - x0) / epsilon
a_dTdq = -T_zeta(q).T.dot(skew(v2).dot(T_zeta(q)))
assert (abs(a_dTdq - d_dTdq) < 1e-6).all()
# Check Derivative dqzeta/dqzeta <- this was also giving me trouble
for j in range(1000):
d_dqdq = np.zeros((2, 2))
if j == 0:
q = Quaternion.Identity()
else:
q = Quaternion(np.random.uniform(-1, 1, (4, 1)))
q.arr[3] = 0.0
q.normalize()
for i in range(2):
d_dqdq[i, :, None] = q_feat_boxminus(
q_feat_boxplus(q, I[:, i, None]), q) / epsilon
a_dqdq = T_zeta(q).T.dot(T_zeta(q))
assert (abs(a_dqdq - d_dqdq) < 1e-1).all()
# Check Manifold Consistency
for i in range(1000):
omega = np.random.uniform(-1, 1, (3, 1))
omega2 = np.random.uniform(-1, 1, (3, 1))
omega[2] = 0.0
omega2[2] = 0.0
x = Quaternion.exp(omega)
y = Quaternion.exp(omega2)
dx = np.random.normal(0.0, 0.5, (2, 1))
# Check x [+] 0 == x
assert norm(q_feat_boxplus(x, np.zeros((2, 1))) - x) < 1e-8
# Check x [+] (y [-] x) == y (compare the rotated zetas, because there are infinitely
# many quaternions which return the same zeta.) We don't have the framework to handle
# forcing the quaternion to actually be the same
assert norm((q_feat_boxplus(x, q_feat_boxminus(y, x))).rot(e_z) -
y.rot(e_z)) < 1e-8
# Check (x [+] dx) [-] x == dx
assert norm(q_feat_boxminus(q_feat_boxplus(x, dx), x) - dx) < 1e-8
# assert norm( q_feat_boxminus(q_feat_boxplus(qzeta, dqzeta), qzeta) - dqzeta) < 1e-8
print "math helper test: [PASSED]"
u = np.array([[0, 0, 1.]]).T
# u = np.random.random((3,1))
u /= norm(u)
psi_hist, yaw_hist, s_hist, v_hist, qz_hist, qx_hist, qy_hist = [], [], [], [], [], [], []
for i in tqdm(range(10000)):
# qm0 = Quaternion.from_euler(0.0, 10.0, 2*np.pi*np.random.random() - np.pi)
# qm0 = Quaternion.from_euler(0.0, np.pi*np.random.random() - np.pi/2.0, 0.0)
qm0 = Quaternion.from_euler(2*np.pi*np.random.random() - np.pi, np.pi * np.random.random() - np.pi / 2.0, 0.0)
# qm0 = Quaternion.from_euler(270.0 * np.pi / 180.0, 85.0 * np.pi / 180.0, 90.0 * np.pi / 180.0)
# qm0 = Quaternion.random()
w = qm0.rot(u)
th = u.T.dot(w)
ve = skew(u).dot(w)
qp0 = Quaternion.exp(th * ve)
epsilon = np.eye(3) * e
t = u.T.dot(qm0.rot(u))
v = skew(u).dot(qm0.rot(u))
tv0 = u.T.dot(qm0.rot(u)) * (skew(u).dot(qm0.rot(u)))
a_dtvdq = -v.dot(u.T.dot(qm0.R.T).dot(skew(u))) - t*skew(u).dot(qm0.R.T.dot(skew(u)))
d_dtvdq = np.zeros_like(a_dtvdq)
nd = norm(t * v)
d0 = t * v
qd0 = Quaternion.exp(d0)
sk_tv = skew(t * v)
Tau = (np.eye(3) + ((1. - np.cos(t)) * sk_tv) / (t * t) + ((t - np.sin(t)) * sk_tv.dot(sk_tv)) / (t * t * t)).T
def q_feat_boxplus(q, dq):
# assert isinstance(q, Quaternion) and dq.shape == (2,1)
return Quaternion.exp(T_zeta(q).dot(dq)) * q
from pyquat import Quaternion from math_helper import norm, skew import numpy as np # for i in range(100): q = Quaternion.random() # q = Quaternion.from_axis_angle(np.array([[0, 0, 1]]).T, np.pi/2.) yaw_true = q.yaw v = np.array([[0, 0, 1]]).T beta = q.elements[1:] beta /= norm(beta) alpha = 2. * np.arccos(q.elements[0, 0]) yaw_steven = 2. * np.arctan(beta.T.dot(v) * np.tan(alpha / 2.)) w = q.rot(v) s = v.T.dot(w) delta = skew(v).dot(w) qhat = Quaternion.exp(s * delta) qstar = q * qhat.inverse yaw_superjax = qstar.yaw print "superjax", (yaw_superjax) print "steven", (yaw_steven) print "true", (yaw_true) # assert abs(yaw_true - yaw_test) < 1e-8, "wrong: true = %f, test = %f" % (yaw_true, yaw_test)