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_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
rotvec = lie.vee(log)
self.assertTrue(np.allclose(r, lie.so3_exp(rotvec)))
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_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 calc_angular_speed(p_1, p_2, t_1, t_2, degrees=False):
"""
:param p_1: pose at timestamp 1
:param p_2: pose at timestamp 2
:param t_1: timestamp 1
:param t_2: timestamp 2
:param degrees: set to True to return deg/s
:return: speed in rad/s
"""
if (t_2 - t_1) <= 0:
raise TrajectoryException("bad timestamps: " + str(t_1) + " & " +
str(t_2))
if degrees:
angle_1 = lie.so3_log(p_1[:3, :3]) * 180 / np.pi
angle_2 = lie.so3_log(p_2[:3, :3]) * 180 / np.pi
else:
angle_1 = lie.so3_log(p_1[:3, :3])
angle_2 = lie.so3_log(p_2[:3, :3])
return (angle_2 - angle_1) / (t_2 - t_1)
ids = range(len(poses))
if degrees:
angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
else:
angles = [lie.so3_log(p[:3, :3]) for p in poses]
for i in ids:
for j in ids[i + 1:]:
current_angle = abs(angles[i] - angles[j])
if lower_bound <= current_angle <= upper_bound:
id_pairs.append((i, j))
else:
ids = []
if degrees:
angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
else:
angles = [lie.so3_log(p[:3, :3]) for p in poses]
previous_angle = angles[0]
current_delta = 0.0
ids.append(0)
for i, current_angle in enumerate(angles):
current_delta += abs(current_angle - previous_angle)
previous_angle = current_angle
if current_delta >= delta:
ids.append(i)
current_delta = 0.0
id_pairs = [(i, j) for i, j in zip(ids, ids[1:])]
return id_pairs
:param p_1: pose at timestamp 1
:param p_2: pose at timestamp 2
:param t_1: timestamp 1
:param t_2: timestamp 2
:param degrees: set to True to return deg/s
:return: speed in rad/s
"""
if (t_2 - t_1) <= 0:
raise TrajectoryException("bad timestamps: " + str(t_1) + " & " +
str(t_2))
if degrees:
angle_1 = lie.so3_log(p_1[:3, :3]) * 180 / np.pi
angle_2 = lie.so3_log(p_2[:3, :3]) * 180 / np.pi
else:
angle_1 = lie.so3_log(p_1[:3, :3])
angle_2 = lie.so3_log(p_2[:3, :3])
return (angle_2 - angle_1) / (t_2 - t_1)
# E is an array of position vectors only in this case
self.error = [np.linalg.norm(E_i) for E_i in self.E]
elif self.pose_relation == PoseRelation.rotation_part:
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:
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")
poses is considered
:param poses: list of SE(3) poses
:param delta: the angle in radians used for filtering
:param tol: absolute angle tolerance to accept or reject pairs
in all_pairs mode
:param degrees: set to True if is in degrees instead of radians
:param all_pairs: use all pairs instead of consecutive pairs
:return: list of index tuples of the filtered pairs
"""
if all_pairs:
upper_bound = delta + tol
lower_bound = delta - tol
id_pairs = []
ids = range(len(poses))
if degrees:
angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
else:
angles = [lie.so3_log(p[:3, :3]) for p in poses]
for i in ids:
for j in ids[i + 1:]:
current_angle = abs(angles[i] - angles[j])
if lower_bound <= current_angle <= upper_bound:
id_pairs.append((i, j))
else:
ids = []
if degrees:
angles = [lie.so3_log(p[:3, :3]) * 180 / np.pi for p in poses]
else:
angles = [lie.so3_log(p[:3, :3]) for p in poses]
previous_angle = angles[0]
current_delta = 0.0
ids.append(0)
(self.pose_relation.value)))
if self.pose_relation == PoseRelation.translation_part:
# E is an array of position vectors only in this case
self.error = [np.linalg.norm(E_i) for E_i in self.E]
elif self.pose_relation == PoseRelation.rotation_part:
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:
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")