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