def re_normalize(m3): inv = np.linalg.inv(m3) tra = m3.transpose() m1 = (inv + tra) / 2 m2 = np.linalg.inv(m1) A = tr.euler_from_matrix(m2) m = tr.euler_matrix(A[0], A[1], A[2]) return m[:3, :3]
def get_orientations_euler(self, axes="sxyz"): if hasattr(self, "_poses_se3"): return np.array( [tr.euler_from_matrix(p, axes=axes) for p in self._poses_se3]) elif hasattr(self, "_orientations_quat_wxyz"): return np.array([ tr.euler_from_quaternion(q, axes=axes) for q in self._orientations_quat_wxyz ])
def orientations_euler(self): if not hasattr(self, "_orientations_euler"): if hasattr(self, "_poses_se3"): self._orientations_euler \ = np.array([tr.euler_from_matrix(p, axes="sxyz") for p in self._poses_se3]) elif hasattr(self, "_orientations_quat_wxyz"): self._orientations_euler \ = np.array([tr.euler_from_quaternion(q, axes="sxyz") for q in self._orientations_quat_wxyz]) return self._orientations_euler
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, align_odom: 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) elif align_odom: logger.debug(SEP) alignment_transformation = traj_est.align_odom(traj_ref) if alignment_transformation is not None: print('alignment_rotation: ', transformations.euler_from_matrix(alignment_transformation)) print('alignment_translation: ', alignment_transformation[:3, 3]) # 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 get_A(Q): A = tr.euler_from_matrix(Q[:3, :3], axes='sxzy') A = np.array(A) * 180.0/np.pi return A
def fromMatrix(R, order="rXYZ"): if order == "rXYZ": x, y, z = euler_from_matrix(R, "rxyz") return Euler(x, y, z) else: raise Exception("Not Implemented Yet!")