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))