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(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_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_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)))
all_pairs=True)
self.assertEqual(id_pairs, [(0, 3)])
def test_convergence_all_pairs(self):
target_path = 1.0
tol = 0.2
id_pairs = filters.filter_pairs_by_path(poses_3,
target_path,
tol,
all_pairs=True)
self.assertEqual(id_pairs, [(0, 7)])
axis = np.array([1, 0, 0])
poses_5 = [
lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis * math.pi), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis * math.pi / 3), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis * math.pi), np.array([0, 0, 0]))
]
stamps_5 = np.array([0, 1, 2, 3, 4])
axis = np.array([1, 0, 0])
p0 = lie.se3(lie.so3_exp(axis * 0.0), np.array([0, 0, 0]))
pd = lie.se3(lie.so3_exp(axis * (math.pi / 3.)), np.array([1, 2, 3]))
p1 = np.dot(p0, pd)
p2 = np.dot(p1, pd)
p3 = np.dot(p2, pd)
poses_6 = [p0, p1, p2, p3, p3]
stamps_6 = np.array([0, 1, 2, 3, 4])
tol = 0.001
id_pairs = filters.filter_pairs_by_distance(
poses_2, target_path, tol, all_pairs=True)
self.assertEqual(id_pairs, [(0, 3)])
def test_poses4_all_pairs(self):
target_path = 1.0
tol = 0.2
id_pairs = filters.filter_pairs_by_distance(
poses_4, target_path, tol, all_pairs=True)
self.assertEqual(id_pairs, [(0, 1), (0, 2), (0, 3)])
# some synthetic poses for testing
axis = np.array([1, 0, 0])
poses_5 = [lie.se3(lie.so3_exp(axis, 0.0), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis, math.pi), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis, 0.0), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis, math.pi / 3), np.array([0, 0, 0])),
lie.se3(lie.so3_exp(axis, math.pi), np.array([0, 0, 0]))]
stamps_5 = np.array([0, 1, 2, 3, 4])
axis = np.array([1, 0, 0])
p0 = lie.se3(lie.so3_exp(axis, 0.0), np.array([0, 0, 0]))
pd = lie.se3(lie.so3_exp(axis, math.pi / 3), np.array([1, 2, 3]))
p1 = np.dot(p0, pd)
p2 = np.dot(p1, pd)
p3 = np.dot(p2, pd)
poses_6 = [p0, p1, p2, p3, p3]
stamps_6 = np.array([0, 1, 2, 3, 4])
