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