def test_right_jacobians_of_boxminusr(self):
for i in range(100):
R1, R2 = SO3.random(), SO3.random()
theta, Jr1 = R1.boxminusr(R2, Jr1=np.eye(3))
dR = R2.inv() * R1
_, Jr1_true = SO3.Log(dR, Jr=np.eye(3))
_, Jr2 = R1.boxminusr(R2, Jr2=np.eye(3))
_, Jr2_true = SO3.Log(dR, Jl=np.eye(3))
np.testing.assert_allclose(Jr1_true, Jr1)
np.testing.assert_allclose(-Jr2_true, Jr2)
def test_left_jacobian_of_logarithm(self):
for i in range(100):
R = SO3.random()
logR, Jl_inv = SO3.Log(R, Jl=np.eye(3))
_, Jl = SO3.Exp(logR, Jl=np.eye(3))
np.testing.assert_allclose(np.linalg.inv(Jl), Jl_inv)
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)