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