def rpe_base(Q_i, Q_i_delta, P_i, P_i_delta):
"""
Computes the relative SE(3) error pose for a single pose pair
following the notation of the TUM RGB-D paper.
:param Q_i: reference SE(3) pose at i
:param Q_i_delta: reference SE(3) pose at i+delta
:param P_i: estimated SE(3) pose at i
:param P_i_delta: estimated SE(3) pose at i+delta
:return: the RPE matrix E_i in SE(3)
"""
Q_rel = lie.relative_se3(Q_i, Q_i_delta)
P_rel = lie.relative_se3(P_i, P_i_delta)
E_i = lie.relative_se3(Q_rel, P_rel)
return E_i
def rpe_base(Q_i, Q_i_delta, P_i, P_i_delta):
"""
Computes the relative SE(3) error pose for a single pose pair
following the notation of the TUM RGB-D paper.
:param Q_i: reference SE(3) pose at i
:param Q_i_delta: reference SE(3) pose at i+delta
:param P_i: estimated SE(3) pose at i
:param P_i_delta: estimated SE(3) pose at i+delta
:return: the RPE matrix E_i in SE(3)
"""
Q_rel = lie.relative_se3(Q_i, Q_i_delta)
P_rel = lie.relative_se3(P_i, P_i_delta)
E_i = lie.relative_se3(Q_rel, P_rel)
return E_i
def transform(self, t, right_mul=False, propagate=False):
"""
apply a left or right multiplicative SE(3) transformation to the whole path
:param t: a valid SE(3) matrix
:param right_mul: whether to apply it right-multiplicative or not
:param propagate: whether to propagate drift with RHS transformations
"""
if not lie.is_se3(t):
raise TrajectoryException(
"transformation is not a valid SE(3) matrix")
if right_mul and not propagate:
# Transform each pose individually.
self._poses_se3 = [np.dot(p, t) for p in self.poses_se3]
elif right_mul and propagate:
# Transform each pose and propagate resulting drift to the next.
ids = np.arange(0, self.num_poses, 1)
rel_poses = [
lie.relative_se3(self.poses_se3[i], self.poses_se3[j]).dot(t)
for i, j in zip(ids, ids[1:])
]
self._poses_se3 = [self.poses_se3[0]]
for i, j in zip(ids[:-1], ids):
self._poses_se3.append(self._poses_se3[j].dot(rel_poses[i]))
else:
self._poses_se3 = [np.dot(t, p) for p in self.poses_se3]
self._positions_xyz, self._orientations_quat_wxyz \
= se3_poses_to_xyz_quat_wxyz(self.poses_se3)
def transform(self, t: np.ndarray, right_mul: bool = False,
propagate: bool = False) -> None:
"""
apply a left or right multiplicative transformation to the whole path
:param t: a 4x4 transformation matrix (e.g. SE(3) or Sim(3))
:param right_mul: whether to apply it right-multiplicative or not
:param propagate: whether to propagate drift with RHS transformations
"""
if right_mul and not propagate:
# Transform each pose individually.
self._poses_se3 = [np.dot(p, t) for p in self.poses_se3]
elif right_mul and propagate:
# Transform each pose and propagate resulting drift to the next.
ids = np.arange(0, self.num_poses, 1)
rel_poses = [
lie.relative_se3(self.poses_se3[i], self.poses_se3[j]).dot(t)
for i, j in zip(ids, ids[1:])
]
self._poses_se3 = [self.poses_se3[0]]
for i, j in zip(ids[:-1], ids):
self._poses_se3.append(self._poses_se3[j].dot(rel_poses[i]))
else:
self._poses_se3 = [np.dot(t, p) for p in self.poses_se3]
self._positions_xyz, self._orientations_quat_wxyz \
= se3_poses_to_xyz_quat_wxyz(self.poses_se3)
def test_relative_se3(self):
a = lie.random_se3()
b = lie.random_se3()
self.assertTrue(lie.is_se3(a) and lie.is_se3(b))
a_to_b = lie.relative_se3(a, b)
self.assertTrue(lie.is_se3(a_to_b))
b_from_a = a.dot(a_to_b)
self.assertTrue(np.allclose(b_from_a, b))
def ape_base(x_t, x_t_star):
"""
Computes the absolute error pose for a single SE(3) pose pair
following the notation of the Kümmerle paper.
:param x_t: estimated absolute pose at t
:param x_t_star: reference absolute pose at t
.:return: the delta pose
"""
return lie.relative_se3(x_t, x_t_star)
def ape_base(x_t, x_t_star):
"""
Computes the absolute error pose for a single SE(3) pose pair
following the notation of the Kümmerle paper.
:param x_t: estimated absolute pose at t
:param x_t_star: reference absolute pose at t
.:return: the delta pose
"""
return lie.relative_se3(x_t, x_t_star)
def align_trajectory_origin(traj, traj_ref):
"""
align a trajectory's origin to the origin of a reference trajectory
:param traj: the trajectory to align
:param traj_ref: reference trajectory
:return: the aligned trajectory
"""
if traj.num_poses == 0 or traj_ref.num_poses == 0:
raise TrajectoryException("can't align an empty trajectory...")
traj_aligned = copy.deepcopy(traj)
to_ref_origin = lie.relative_se3(traj.poses_se3[0], traj_ref.poses_se3[0])
logger.debug("Origin alignment transformation:\n{}".format(to_ref_origin))
traj_aligned.transform(to_ref_origin)
return traj_aligned
def convert_abs_traj_to_rel_traj(traj, up_to_scale=False):
""" Converts an absolute-pose trajectory to a relative-pose trajectory.
The incoming trajectory is processed element-wise. At each timestamp
starting from the second (index 1), the relative pose
from the previous timestamp to the current one is calculated (in the previous-
timestamp's coordinate frame). This relative pose is then appended to the
resulting trajectory.
The resulting trajectory has timestamp indices corresponding to poses that represent
the relative transformation between that timestamp and the **next** one.
Args:
traj: A PoseTrajectory3D object with timestamps as indices containing, at a minimum,
columns representing the xyz position and wxyz quaternion-rotation at each
timestamp, corresponding to the absolute pose at that time.
up_to_scale: A boolean. If set to True, relative poses will have their translation
part normalized.
Returns:
A PoseTrajectory3D object with xyz position and wxyz quaternion fields for the
relative pose trajectory corresponding to the absolute one given in `traj`.
"""
from evo.core import transformations
from evo.core import lie_algebra as lie
new_poses = []
for i in range(1, len(traj.timestamps)):
rel_pose = lie.relative_se3(traj.poses_se3[i - 1], traj.poses_se3[i])
if up_to_scale:
bim1_t_bi = rel_pose[:3, 3]
norm = np.linalg.norm(bim1_t_bi)
if norm > 1e-6:
bim1_t_bi = bim1_t_bi / norm
rel_pose[:3, 3] = bim1_t_bi
new_poses.append(rel_pose)
return trajectory.PoseTrajectory3D(timestamps=traj.timestamps[1:],
poses_se3=new_poses)
def get_gt_rel_pose(gt_df, match_ts, query_ts, to_scale=True):
"""Returns the relative pose from match to query for given timestamps.
Args:
gt_df: A pandas.DataFrame object with timestamps as indices containing, at a minimum,
columns representing the xyz position and wxyz quaternion-rotation at each
timestamp, corresponding to the absolute pose at that time.
match_ts: An integer representing the match frame timestamp.
query_ts: An integer representing the query frame timestamp.
to_scale: A boolean. If set to False, relative poses will have their translation
part normalized.
Returns:
A 4x4 numpy array representing the relative pose from match to query frame.
"""
w_T_bmatch = None
w_T_bquery = None
try:
closest_ts = closest_num(gt_df.index, match_ts)
if closest_ts != match_ts:
# print("using closest match for timestamps")
pass
w_t_bmatch = np.array(
[gt_df.at[closest_ts, idx] for idx in ["x", "y", "z"]])
w_q_bmatch = np.array(
[gt_df.at[closest_ts, idx] for idx in ["qw", "qx", "qy", "qz"]])
w_T_bmatch = transformations.quaternion_matrix(w_q_bmatch)
w_T_bmatch[:3, 3] = w_t_bmatch
except:
print(
"Failed to convert an abs pose to a rel pose. Timestamp ",
match_ts,
" is not available in ground truth df.",
)
return None
try:
closest_ts = closest_num(gt_df.index, query_ts)
if closest_ts != query_ts:
# print("using closest match for timestamps")
pass
w_t_bquery = np.array(
[gt_df.at[closest_ts, idx] for idx in ["x", "y", "z"]])
w_q_bquery = np.array(
[gt_df.at[closest_ts, idx] for idx in ["qw", "qx", "qy", "qz"]])
w_T_bquery = transformations.quaternion_matrix(w_q_bquery)
w_T_bquery[:3, 3] = w_t_bquery
except:
print(
"Failed to convert an abs pose to a rel pose. Timestamp ",
query_ts,
" is not available in ground truth df.",
)
return None
bmatch_T_bquery = lie.relative_se3(w_T_bmatch, w_T_bquery)
bmatch_t_bquery = bmatch_T_bquery[:3, 3]
if not to_scale:
norm = np.linalg.norm(bmatch_t_bquery)
if norm > 1e-6:
bmatch_t_bquery = bmatch_t_bquery / np.linalg.norm(bmatch_t_bquery)
bmatch_T_bquery[:3, 3] = bmatch_t_bquery
return bmatch_T_bquery
