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))
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 process_data(self, data, id_pairs=None):
"""
calculate relative poses on a batch of SE(3) poses
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
:param id_pairs: pre-computed pair indices if you know what you're doing (ignores delta)
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
if id_pairs is None:
id_pairs = id_pairs_from_delta(traj_est.poses_se3,
self.delta,
self.delta_unit,
self.rel_delta_tol,
all_pairs=self.all_pairs)
if not self.all_pairs:
self.delta_ids = [j for i, j in id_pairs
] # store flat id list e.g. for plotting
self.E = [
self.rpe_base(traj_ref.poses_se3[i], traj_ref.poses_se3[j],
traj_est.poses_se3[i], traj_est.poses_se3[j])
for i, j in id_pairs
]
logger.debug("compared " + str(len(self.E)) +
" relative pose pairs, delta = " + str(self.delta) +
" (" + str(self.delta_unit.value) + ") " +
("with all possible pairs" if self.
all_pairs else "with consecutive pairs"))
logger.debug("calculating RPE for " + str(self.pose_relation.value) +
" pose relation...")
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)
def process_data(self, data):
"""
Calculates the RPE on a batch of SE(3) poses from trajectories.
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
id_pairs = filters.id_pairs_from_delta(
traj_est.poses_se3, self.delta, self.delta_unit,
self.rel_delta_tol, all_pairs=self.all_pairs)
if not self.all_pairs:
# Store flat id list e.g. for plotting.
self.delta_ids = [j for i, j in id_pairs]
self.E = [
self.rpe_base(traj_ref.poses_se3[i], traj_ref.poses_se3[j],
traj_est.poses_se3[i], traj_est.poses_se3[j])
for i, j in id_pairs
]
logger.debug(
"Compared {} relative pose pairs, delta = {} ({}) {}".format(
len(self.E), self.delta, self.delta_unit.value,
("with all pairs." if self.all_pairs \
else "with consecutive pairs.")))
logger.debug("Calculating RPE for {} pose relation...".format(
self.pose_relation.value))
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)
def process_data(self, data):
"""
Calculates the RPE on a batch of SE(3) poses from trajectories.
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
id_pairs = filters.id_pairs_from_delta(
traj_est.poses_se3, self.delta, self.delta_unit,
self.rel_delta_tol, all_pairs=self.all_pairs)
if not self.all_pairs:
# Store flat id list e.g. for plotting.
self.delta_ids = [j for i, j in id_pairs]
self.E = [self.rpe_base(traj_ref.poses_se3[i], traj_ref.poses_se3[j],
traj_est.poses_se3[i], traj_est.poses_se3[j])
for i, j in id_pairs]
logger.debug(
"Compared {} relative pose pairs, delta = {} ({}) {}".format(
len(self.E), self.delta, self.delta_unit.value,
("with all pairs." if self.all_pairs \
else "with consecutive pairs.")))
logger.debug("Calculating RPE for {} pose relation...".format(
self.pose_relation.value))
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)
def process_data(self, data: PathPair) -> None:
"""
Calculates the APE on a batch of SE(3) poses from trajectories.
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
if self.pose_relation == PoseRelation.translation_part:
# don't require full SE(3) matrices for faster computation
self.E = traj_est.positions_xyz - traj_ref.positions_xyz
elif self.pose_relation == PoseRelation.z:
self.E = traj_est.positions_xyz - traj_ref.positions_xyz
elif self.pose_relation == PoseRelation.xy:
self.E = traj_est.positions_xyz - traj_ref.positions_xyz
else:
self.E = [
self.ape_base(x_t, x_t_star) for x_t, x_t_star in zip(
traj_est.poses_se3, traj_ref.poses_se3)
]
logger.debug("Compared {} absolute pose pairs.".format(len(self.E)))
logger.debug("Calculating APE for {} pose relation...".format(
(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.array([np.linalg.norm(E_i) for E_i in self.E])
elif self.pose_relation == PoseRelation.z:
self.error = np.array([E_i[2] for E_i in self.E])
elif self.pose_relation == PoseRelation.xy:
self.error = np.array([np.linalg.norm(E_i[0:2]) 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")
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 filter_pairs_by_angle(poses,
delta,
tol=0.0,
degrees=False,
all_pairs=False):
"""
filters pairs in a list of SE(3) poses by their absolute relative angle
- by default, the angle accumulated on the path between the two pair poses
is considered
- if <all_pairs> is set to True, the direct angle between the two pair
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 <delta> 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)
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
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 calc_angular_speed(p_1: np.ndarray, p_2: np.ndarray, t_1: float,
t_2: float, degrees: bool = False) -> float:
"""
: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))
angle_1 = lie.so3_log(p_1[:3, :3], degrees)
angle_2 = lie.so3_log(p_2[:3, :3], degrees)
return (angle_2 - angle_1) / (t_2 - t_1)
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)
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)
def filter_pairs_by_angle(poses, delta, tol=0.0, degrees=False, all_pairs=False):
"""
filters pairs in a list of SE(3) poses by their absolute relative angle
- by default, the angle accumulated on the path between the two pair poses is considered
- if <all_pairs> is set to True, the direct angle between the two pair 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 <delta> 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)
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
def process_data(self, data):
"""
Calculates the APE on a batch of SE(3) poses from trajectories.
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
if self.pose_relation == PoseRelation.translation_part:
# don't require full SE(3) matrices for faster computation
self.E = traj_est.positions_xyz - traj_ref.positions_xyz
else:
self.E = [self.ape_base(x_t, x_t_star) for x_t, x_t_star in
zip(traj_est.poses_se3, traj_ref.poses_se3)]
logger.debug("Compared {} absolute pose pairs.".format(len(self.E)))
logger.debug("Calculating APE for {} pose relation...".format(
(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")
def process_data(self,
data: PathPair,
align=False,
align_origin=False,
align_odom=False) -> None:
"""
Calculate the APE of segments from a batch of SE(3) poses from trajectories
:param data: tuple (traj_ref, traj_est) with:
traj_ref: reference evo.trajectory.PosePath or derived
traj_est: estimated evo.trajectory.PosePath or derived
"""
if len(data) != 2:
raise MetricsException(
"please provide data tuple as: (traj_ref, traj_est)")
traj_ref, traj_est = data
if traj_ref.num_poses != traj_est.num_poses:
raise MetricsException(
"trajectories must have same number of poses")
step_size = 10
i = 0
for first_pose in range(0, traj_ref.num_poses, step_size):
print('processing segment ', i)
i = i + 1
last_pose = self.last_pose_from_segment_length(
traj_ref, first_pose)
if (last_pose == -1):
break
traj_ref_segment = trajectory.PosePath3D(
poses_se3=traj_ref.poses_se3[first_pose:last_pose])
traj_est_segment = trajectory.PosePath3D(
poses_se3=traj_est.poses_se3[first_pose:last_pose])
# if align or correct_scale:
if align:
alignment_transformation = lie.sim3(*traj_est_segment.align(
traj_ref_segment, False, False, -1))
elif align_origin:
print('align origin: ', align_origin)
alignment_transformation = traj_est_segment.align_origin(
traj_ref_segment)
elif align_odom:
alignment_transformation = traj_est_segment.align_odom(
traj_ref_segment)
self.segment_index.append([first_pose, last_pose])
if i == 1:
print('starting idx: ', first_pose)
print('ending idx: ', last_pose)
# print('origin 0 position: ', traj_est_segment.poses_se3[0])
# print('origin 1 position: ', traj_est_segment.poses_se3[1])
# print('ref 0 position: ', traj_ref_segment.poses_se3[0])
# print('results 1 position: ', traj_ref_segment.poses_se3[1])
fig = plt.figure()
plot_option = '3D'
if plot_option == '2D':
ax = fig.add_subplot() #projection="3d"
elif plot_option == '3D':
ax = fig.add_subplot(projection="3d")
x_ref = traj_ref_segment.positions_xyz[:, 0]
y_ref = traj_ref_segment.positions_xyz[:, 1]
z_ref = traj_ref_segment.positions_xyz[:, 2]
front_vector_origin = np.array([1, 0, 0, 1])
f_vecs = np.array([
(np.dot(pose, front_vector_origin) - pose[:, 3]) * 10
for pose in traj_ref_segment.poses_se3
])
if plot_option == '2D':
ax.plot(x_ref, y_ref) #, z_ref)
# plt.quiver(x_ref, y_ref, f_vecs[:,0], f_vecs[:,1], color='g')#, arrow_length_ratio=0.05)
elif plot_option == '3D':
ax.plot(x_ref, y_ref, z_ref)
plt.quiver(x_ref,
y_ref,
z_ref,
f_vecs[:, 0],
f_vecs[:, 1],
f_vecs[:, 2],
color='g',
arrow_length_ratio=0.05)
x_est = traj_est_segment.positions_xyz[:, 0]
y_est = traj_est_segment.positions_xyz[:, 1]
z_est = traj_est_segment.positions_xyz[:, 2]
f_vecs = np.array([
(np.dot(pose, front_vector_origin) - pose[:, 3]) * 10
for pose in traj_est_segment.poses_se3
])
if plot_option == '2D':
ax.plot(x_est, y_est, color='r')
# plt.quiver(x_est, y_est, f_vecs[:,0], f_vecs[:,1], color='b')
elif plot_option == '3D':
ax.plot(x_est, y_est, z_est, color='r')
plt.quiver(x_est,
y_est,
z_est,
f_vecs[:, 0],
f_vecs[:, 1],
f_vecs[:, 2],
color='b',
arrow_length_ratio=0.05)
plt.title('segment_length: ' + str(self.segment_length) + 'm')
plt.savefig('first_segment_aligned.png')
if self.plot_show:
plt.show()
if self.pose_relation == PoseRelation.translation_part:
segment_E = traj_est_segment.positions_xyz - traj_ref_segment.positions_xyz
elif self.pose_relation == PoseRelation.z:
segment_E = traj_est_segment.positions_xyz - traj_ref_segment.positions_xyz
elif self.pose_relation == PoseRelation.xy:
segment_E = traj_est_segment.positions_xyz - traj_ref_segment.positions_xyz
else:
segment_E = [
self.ape_base(x_t, x_t_star) for x_t, x_t_star in zip(
traj_est_segment.poses_se3, traj_ref_segment.poses_se3)
]
logger.debug("Compared {} absolute pose pairs.".format(len(
self.E)))
logger.debug("Calculating APE for {} pose relation...".format(
(self.pose_relation.value)))
if self.pose_relation == PoseRelation.translation_part:
segment_error = np.array(
[np.linalg.norm(E_i) for E_i in segment_E])
elif self.pose_relation == PoseRelation.z:
segment_error = np.array([E_i[2] for E_i in segment_E])
elif self.pose_relation == PoseRelation.xy:
segment_error = np.array(
[np.linalg.norm(E_i[0:2]) for E_i in segment_E])
elif self.pose_relation == PoseRelation.rotation_part:
segment_error = np.array([
np.linalg.norm(lie.so3_from_se3(E_i) - np.eye(3))
for E_i in segment_E
])
elif self.pose_relation == PoseRelation.full_transformation:
segment_error = np.array(
[np.linalg.norm(E_i - np.eye(4)) for E_i in segment_E])
elif self.pose_relation == PoseRelation.rotation_angle_rad:
segment_error = np.array(
[abs(lie.so3_log(E_i[:3, :3])) for E_i in segment_E])
elif self.pose_relation == PoseRelation.rotation_angle_deg:
segment_error = np.array([
abs(lie.so3_log(E_i[:3, :3])) * 180 / np.pi
for E_i in segment_E
])
else:
raise MetricsException("unsupported pose_relation")
self.E.append(segment_E)
self.seg_error.append(segment_error)
