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)