def test_relative_so3(self):
a = lie.random_so3()
b = lie.random_so3()
self.assertTrue(lie.is_so3(a) and lie.is_so3(b))
a_to_b = lie.relative_so3(a, b)
b_from_a = a.dot(a_to_b)
self.assertTrue(np.allclose(b_from_a, b))
def test_so3_log_exp(self):
r = lie.random_so3()
self.assertTrue(lie.is_so3(r))
rotvec = lie.so3_log(r)
self.assertTrue(np.allclose(r, lie.so3_exp(rotvec), atol=1e-6))
angle = lie.so3_log_angle(r)
self.assertAlmostEqual(np.linalg.norm(rotvec), angle)
def test_so3_log_exp(self):
r = lie.random_so3()
self.assertTrue(lie.is_so3(r))
axis, angle = lie.so3_log(r, return_angle_only=False)
self.assertTrue(np.allclose(r, lie.so3_exp(axis, angle), atol=1e-6))
angle = lie.so3_log(r)
self.assertTrue(np.allclose(r, lie.so3_exp(axis, angle), atol=1e-6))
def test_relative_se3_rot(self):
a = lie.random_se3()
b = lie.random_se3()
self.assertTrue(lie.is_se3(a) and lie.is_se3(b))
a_to_b_rot = lie.relative_se3_rot(a, b)
self.assertTrue(lie.is_so3(a_to_b_rot))
b_rot_from_a_rot = a[:3, :3].dot(a_to_b_rot)
self.assertTrue(np.allclose(b_rot_from_a_rot, b[:3, :3]))
def test_so3_log_exp(self):
r = lie.random_so3()
self.assertTrue(lie.is_so3(r))
axis, angle = lie.so3_log(r, return_angle_only=False)
self.assertTrue(np.allclose(r, lie.so3_exp(axis, angle), atol=1e-6))
angle = lie.so3_log(r)
# we ignore signs here, therefore check also transpose
self.assertTrue(
np.allclose(r,
lie.so3_exp(axis, angle).T)
or np.allclose(r, lie.so3_exp(axis, angle)))
def test_so3_from_se3(self):
p = lie.random_se3()
r = lie.so3_from_se3(p)
self.assertTrue(lie.is_so3(r))
def test_random_so3(self):
r = lie.random_so3()
self.assertTrue(lie.is_so3(r))
def test_is_so3(self):
# yapf: disable
r = np.array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]])
self.assertTrue(lie.is_so3(r))
