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_transform_sim3(self): path = helpers.fake_path(10) path_transformed = copy.deepcopy(path) t = lie.sim3(r=lie.random_so3(), t=np.ones(3), s=1.234) path_transformed.transform(t) self.assertAlmostEqual(path_transformed.path_length, path.path_length * 1.234)
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_so3_log_exp_skew(self): r = lie.random_so3() log = lie.so3_log(r, return_skew=True) # skew-symmetric tangent space # here, axis is a rotation vector with norm = angle axis = lie.vee(log) angle = np.linalg.norm(axis) self.assertTrue(np.allclose(r, lie.so3_exp(axis, angle)))
def test_sim3_inverse(self): r = lie.random_so3() t = np.array([1, 2, 3]) s = random.random() * 10 p = lie.sim3(r, t, s) self.assertTrue(lie.is_sim3(p, s)) p_inv = lie.sim3_inverse(p) self.assertTrue(np.allclose(p_inv.dot(p), np.eye(4)))
def test_sim3_scale_effect(self): r = lie.random_so3() t = np.array([0, 0, 0]) s = random.random() * 10 x = np.array([1, 0, 0, 1]).T # homogeneous vector p = lie.sim3(r, t, s) self.assertTrue(lie.is_sim3(p, s)) x = p.dot(x) # apply Sim(3) transformation self.assertTrue(np.equal(x, lie.se3(r).dot(np.array([s, 0, 0, 1]))).all())
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_random_so3(self): r = lie.random_so3() self.assertTrue(lie.is_so3(r))
def test_is_sim3(self): r = lie.random_so3() t = np.array([1, 2, 3]) s = 3 p = lie.sim3(r, t, s) self.assertTrue(lie.is_sim3(p, s))