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