def test_jacobians_of_exponential(self):
for i in range(100):
tau = np.random.uniform(-np.pi, np.pi, size=3)
q, Jr = Quaternion.Exp(tau, Jr=np.eye(3))
_, Jl = Quaternion.Exp(-tau, Jl=np.eye(3))
np.testing.assert_allclose(Jl, Jr)
def testAdj(self):
for i in range(100):
q = Quaternion.random()
w = np.random.uniform(-np.pi, np.pi, size=3)
p_true = q * Quaternion.Exp(w)
p = Quaternion.Exp(q.Adj @ w) * q
np.testing.assert_allclose(p_true.q, p.q)
def test_left_jacobian_or_logarithm(self):
for i in range(100):
q = Quaternion.random()
logq, Jl_inv = Quaternion.Log(q, Jl=np.eye(3))
_, Jl = Quaternion.Exp(logq, Jl=np.eye(3))
np.testing.assert_allclose(np.linalg.inv(Jl), Jl_inv)
def ExponentialJacobian(theta):
Jr = np.zeros((3, 3))
Jl = np.zeros((3, 3))
dx = 1e-4
for i in range(3):
theta2 = theta.copy()
theta2[i] += dx
q = Quaternion.Exp(theta)
q2 = Quaternion.Exp(theta2)
vecr = q2.boxminusr(q)
vecl = q2.boxminusl(q)
Jr[:, i] = vecr / dx
Jl[:, i] = vecl / dx
return Jr, Jl
def testExp(self):
for i in range(100):
theta = np.random.uniform(-np.pi, np.pi)
v = np.random.uniform(-1.0, 1.0, size=3)
w = theta * v / np.linalg.norm(v)
R = SO3.Exp(w)
q = Quaternion.Exp(w)
np.testing.assert_allclose(R.R, q.R.T)
q = Quaternion.random()
q_inv, Jr = q.inv(Jr=np.eye(3))
q_inv, Jl = q.inv(Jl=np.eye(3))
Jr_num, Jl_num = inverseJacobian(q)
# Composition is correct
q2 = Quaternion.random()
q3, Jr1 = q.compose(q2, Jr=np.eye(3))
q3, Jr2 = q.compose(q2, Jr2=np.eye(3))
q3, Jl1 = q.compose(q2, Jl=np.eye(3))
q3, Jl2 = q.compose(q2, Jl2=np.eye(3))
Jr1n, Jl1n, Jr2n, Jl2n = compositionJacobian(q, q2)
# Exponential is correct
theta = np.random.uniform(-np.pi, np.pi, size=3)
q, Jr = Quaternion.Exp(theta, Jr=np.eye(3))
q, Jl = Quaternion.Exp(theta, Jl=np.eye(3))
Jrn, Jln = ExponentialJacobian(theta)
debug = 1
# Logarithm Jacobian is correct
theta, Jr = Quaternion.Log(q, Jr=np.eye(3))
theta, Jl = Quaternion.Log(q, Jl=np.eye(3))
Jrn, Jln = LogJacobian(q)
# Rotation jacobians
vec = np.random.uniform(-10, 10, size=3)
v2, Jr = q.rota(vec, Jr=np.eye(3))
v2, Jl = q.rota(vec, Jl=np.eye(3))
Jrn, Jln = rotationJacobian(q, vec)
debug = 1
