def test_transform_sim3(self): path = helpers.fake_path(10) path_transformed = copy.deepcopy(path) t = lie.sim3(r=lie.random_so3(), t=np.ones(3), s=1.234) path_transformed.transform(t) self.assertAlmostEqual(path_transformed.path_length, path.path_length * 1.234)
def ape(traj_ref: PosePath3D, traj_est: PosePath3D, pose_relation: metrics.PoseRelation, align: bool = False, correct_scale: bool = False, n_to_align: int = -1, align_origin: bool = False, ref_name: str = "reference", est_name: str = "estimate") -> Result: # Align the trajectories. only_scale = correct_scale and not align alignment_transformation = None if align or correct_scale: logger.debug(SEP) alignment_transformation = lie_algebra.sim3( *traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align)) elif align_origin: logger.debug(SEP) alignment_transformation = traj_est.align_origin(traj_ref) # Calculate APE. logger.debug(SEP) data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" if (align or correct_scale) and n_to_align != -1: title += " (aligned poses: {})".format(n_to_align) ape_result = ape_metric.get_result(ref_name, est_name) ape_result.info["title"] = title logger.debug(SEP) logger.info(ape_result.pretty_str()) ape_result.add_trajectory(ref_name, traj_ref) ape_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, PoseTrajectory3D): seconds_from_start = np.array( [t - traj_est.timestamps[0] for t in traj_est.timestamps]) ape_result.add_np_array("seconds_from_start", seconds_from_start) ape_result.add_np_array("timestamps", traj_est.timestamps) if alignment_transformation is not None: ape_result.add_np_array("alignment_transformation_sim3", alignment_transformation) return ape_result
def test_sim3_inverse(self): r = lie.random_so3() t = np.array([1, 2, 3]) s = random.random() * 10 p = lie.sim3(r, t, s) self.assertTrue(lie.is_sim3(p, s)) p_inv = lie.sim3_inverse(p) self.assertTrue(np.allclose(p_inv.dot(p), np.eye(4)))
def test_sim3_scale_effect(self): r = lie.random_so3() t = np.array([0, 0, 0]) s = random.random() * 10 x = np.array([1, 0, 0, 1]).T # homogeneous vector p = lie.sim3(r, t, s) self.assertTrue(lie.is_sim3(p, s)) x = p.dot(x) # apply Sim(3) transformation self.assertTrue(np.equal(x, lie.se3(r).dot(np.array([s, 0, 0, 1]))).all())
def test_is_sim3(self): r = lie.random_so3() t = np.array([1, 2, 3]) s = 3 p = lie.sim3(r, t, s) self.assertTrue(lie.is_sim3(p, s))
def rpe(traj_ref: PosePath3D, traj_est: PosePath3D, pose_relation: metrics.PoseRelation, delta: float, delta_unit: metrics.Unit, rel_delta_tol: float = 0.1, all_pairs: bool = False, align: bool = False, correct_scale: bool = False, n_to_align: int = -1, align_origin: bool = False, ref_name: str = "reference", est_name: str = "estimate", support_loop: bool = False) -> Result: # Align the trajectories. only_scale = correct_scale and not align alignment_transformation = None if align or correct_scale: logger.debug(SEP) alignment_transformation = lie_algebra.sim3( *traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align)) elif align_origin: logger.debug(SEP) alignment_transformation = traj_est.align_origin(traj_ref) # Calculate RPE. logger.debug(SEP) data = (traj_ref, traj_est) rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol, all_pairs) rpe_metric.process_data(data) title = str(rpe_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" if (align or correct_scale) and n_to_align != -1: title += " (aligned poses: {})".format(n_to_align) rpe_result = rpe_metric.get_result(ref_name, est_name) rpe_result.info["title"] = title logger.debug(SEP) logger.info(rpe_result.pretty_str()) # Restrict trajectories to delta ids for further processing steps. if support_loop: # Avoid overwriting if called repeatedly e.g. in Jupyter notebook. import copy traj_ref = copy.deepcopy(traj_ref) traj_est = copy.deepcopy(traj_est) traj_ref.reduce_to_ids(rpe_metric.delta_ids) traj_est.reduce_to_ids(rpe_metric.delta_ids) rpe_result.add_trajectory(ref_name, traj_ref) rpe_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, PoseTrajectory3D): seconds_from_start = np.array( [t - traj_est.timestamps[0] for t in traj_est.timestamps]) rpe_result.add_np_array("seconds_from_start", seconds_from_start) rpe_result.add_np_array("timestamps", traj_est.timestamps) if alignment_transformation is not None: rpe_result.add_np_array("alignment_transformation_sim3", alignment_transformation) return rpe_result
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)