def xyz_quat_wxyz_to_se3_poses( xyz: np.ndarray, quat: np.ndarray) -> typing.Sequence[np.ndarray]: poses = [ lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz) for quat, xyz in zip(quat, xyz) ] return poses
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, 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: 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 load_transform_json(json_path): """ load a transformation stored in xyz + quaternion format in a .json file :param json_path: path to the .json file :return: t (SE(3) matrix), xyz (position), quat (orientation quaternion) """ with open(json_path, 'r') as tf_file: data = json.load(tf_file) keys = ("x", "y", "z", "qx", "qy", "qz", "qw") if not all(key in data for key in keys): raise FileInterfaceException( "invalid transform file - expected keys " + str(keys)) xyz = np.array([data["x"], data["y"], data["z"]]) quat = np.array([data["qw"], data["qx"], data["qy"], data["qz"]]) t = lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz) return t, xyz, quat
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 test_so3_from_se3(self): p = lie.random_se3() r = lie.so3_from_se3(p) self.assertTrue(lie.is_so3(r))
def xyz_quat_wxyz_to_se3_poses(xyz, quat): poses = [ lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz) for quat, xyz in zip(quat, xyz) ] return poses
def xyz_quat_wxyz_to_se3_poses(xyz, quat): poses = [lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz) for quat, xyz in zip(quat, xyz)] return poses
def process_data(self, data: PathPair) -> None: """ 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 = id_pairs_from_delta(traj_est.poses_se3, self.delta, self.delta_unit, self.rel_delta_tol, all_pairs=self.all_pairs) # Store flat id list e.g. for plotting. self.delta_ids = [j for i, j in id_pairs] if self.pose_relation in (PoseRelation.point_distance, PoseRelation.point_distance_error_ratio): # Only compares the magnitude of the point distance instead of # doing the full vector comparison of 'translation_part'. # Can be directly calculated on positions instead of full poses. ref_distances = np.array([ np.linalg.norm(traj_ref.positions_xyz[i] - traj_ref.positions_xyz[j]) for i, j in id_pairs ]) est_distances = np.array([ np.linalg.norm(traj_est.positions_xyz[i] - traj_est.positions_xyz[j]) for i, j in id_pairs ]) self.error = np.abs(ref_distances - est_distances) if self.pose_relation == PoseRelation.point_distance_error_ratio: nonzero = ref_distances.nonzero()[0] if nonzero.size != ref_distances.size: logger.warning( f"Ignoring {ref_distances.size - nonzero.size} zero " "divisions in ratio calculations.") self.delta_ids = [self.delta_ids[i] for i in nonzero] self.error = np.divide(self.error[nonzero], ref_distances[nonzero]) * 100 else: # All other pose relations require the full pose error. 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 in (PoseRelation.point_distance, PoseRelation.point_distance_error_ratio): # Already computed, see above. pass elif 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_angle(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_angle(E_i[:3, :3], True)) for E_i in self.E]) else: raise MetricsException("unsupported pose_relation: ", self.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)