def test_jacobians_of_exponential(self):
for i in range(100):
tau = np.random.uniform(-np.pi, np.pi, size=3)
R, Jr = SO3.Exp(tau, Jr=np.eye(3))
_, Jl = SO3.Exp(tau, Jl=np.eye(3))
Adj_R = R.Adj
res = Jl @ np.linalg.inv(Jr)
np.testing.assert_allclose(Adj_R, res)
def testAdjoint(self):
for i in range(100):
delta = np.random.uniform(-np.pi, np.pi, size=3)
rot = Rotation.random().as_matrix()
R = SO3(rot)
Adj_R = R.Adj
T_true = R * SO3.Exp(delta)
T = SO3.Exp(Adj_R @ delta) * R
np.testing.assert_allclose(T_true.R, T.R)
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 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)