Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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))
def test_so3_log_exp(self):
r = lie.random_so3()
self.assertTrue(lie.is_so3(r))
rotvec = lie.so3_log(r, return_angle_only=False)
self.assertTrue(np.allclose(r, lie.so3_exp(rotvec), atol=1e-6))
angle = lie.so3_log(r)
self.assertAlmostEqual(np.linalg.norm(rotvec), angle)
def test_so3_from_se3(self):
p = lie.random_se3()
r = lie.so3_from_se3(p)
self.assertTrue(lie.is_so3(r))
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)
if self.pose_relation == PoseRelation.translation_part:
self.error = [np.linalg.norm(E_i[:3, 3]) for E_i in self.E]
elif self.pose_relation == PoseRelation.rotation_part:
# ideal: rot(E_i) = 3x3 identity
self.error = np.array([
np.linalg.norm(lie.so3_from_se3(E_i) - np.eye(3))
for E_i in self.E
])
elif self.pose_relation == PoseRelation.full_transformation:
# ideal: E_i = 4x4 identity
self.error = np.array(
[np.linalg.norm(E_i - np.eye(4)) for E_i in self.E])
elif self.pose_relation == PoseRelation.rotation_angle_rad:
self.error = np.array(
[abs(lie.so3_log(E_i[:3, :3])) for E_i in self.E])
elif self.pose_relation == PoseRelation.rotation_angle_deg:
self.error = np.array([
abs(lie.so3_log(E_i[:3, :3])) * 180 / np.pi for E_i in self.E
])
else:
raise MetricsException("unsupported pose_relation: ",
self.pose_relation)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left \
if args.transform_left else args.transform_right
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform, right_mul=args.transform_right,
propagate=args.propagate_transform)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(