def test_right_jacobians_of_boxminusr(self):
for i in range(100):
q1, q2 = Quaternion.random(), Quaternion.random()
theta, Jr1 = q1.boxminusr(q2, Jr1=np.eye(3))
dq = q2.inv() * q1
_, Jr1_true = Quaternion.Log(dq, Jr=np.eye(3))
_, Jr2 = q1.boxminusr(q2, Jr2=np.eye(3))
_, Jr2_true = Quaternion.Log(dq, Jl=np.eye(3))
np.testing.assert_allclose(Jr1_true, Jr1)
np.testing.assert_allclose(-Jr2_true, Jr2)
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 LogJacobian(q):
Jr, Jl = np.zeros((3, 3)), np.zeros((3, 3))
dx = 1e-4
for i in range(3):
delta = np.zeros(3)
delta[i] = dx
q2r = q.boxplusr(delta)
q2l = q.boxplusl(delta)
vecr = Quaternion.Log(q2r) - Quaternion.Log(q)
vecl = Quaternion.Log(q2l) - Quaternion.Log(q)
Jr[:, i] = vecr / dx
Jl[:, i] = vecl / dx
return Jr, Jl
def testLogTaylor(self):
for i in range(100):
theta = np.random.uniform(0, 1e-3)
v = np.random.uniform(-10, 10, size=3)
vec = theta * v / np.linalg.norm(v)
q = Quaternion.fromAxisAngle(vec)
w = Quaternion.Log(q)
def testLog(self):
for i in range(100):
q = Quaternion.random()
R = SO3.fromQuaternion(q.q)
w_true = SO3.Log(R)
w = Quaternion.Log(q)
np.testing.assert_allclose(w_true, w)
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
# Jacobian of inverse se3 works
T = SE3.random()
T_inv, Jr = T.inv(Jr=np.eye(6))
T_inv, Jl = T.inv(Jl=np.eye(6))
Jrn, Jln = se3InverseJacobian(T)
