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