Add yaw-only alignment to Umeyama alignment options

doc/alignment_demo.py

@@ -53,6 +53,10 @@
 traj_est_aligned_only_scaled = copy.deepcopy(traj_est)
 traj_est_aligned_only_scaled.align(traj_ref, correct_only_scale=True)
 
+logger.info("\nUmeyama alignment with yaw-only rotation")
+traj_est_aligned_only_yaw = copy.deepcopy(traj_est)
+traj_est_aligned_only_yaw.align(traj_ref, yaw_only=True)
+
 fig = plt.figure(figsize=(8, 8))
 plot_mode = plot.PlotMode.xz
 

evo/core/geometry.py

@@ -31,9 +31,8 @@ class GeometryException(EvoException):
 
 UmeyamaResult = typing.Tuple[np.ndarray, np.ndarray, float]
 
-
 def umeyama_alignment(x: np.ndarray, y: np.ndarray,
-                      with_scale: bool = False) -> UmeyamaResult:
+                      with_scale: bool = False, yaw_only: bool = False) -> UmeyamaResult:
     """
     Computes the least squares solution parameters of an Sim(m) matrix
     that minimizes the distance between a set of registered points.
@@ -76,8 +75,15 @@ def umeyama_alignment(x: np.ndarray, y: np.ndarray,
         # Ensure a RHS coordinate system (Kabsch algorithm).
         s[m - 1, m - 1] = -1
 
-    # rotation, eq. 40
-    r = u.dot(s).dot(v)
+    if yaw_only:
+        # See equations (15)-(17) in A Tutorial on Quantitative Trajectory Evaluation
+        # for Visual(-Inertial) Odometry, by Zhang and Scaramuzza
+        rot_C = (x - mean_x[:, np.newaxis]).dot((y - mean_y[:, np.newaxis]).T)
+        theta = get_best_yaw(rot_C)
+        r = rot_z(theta)
+    else:
+        # rotation, eq. 40
+        r = u.dot(s).dot(v)
 
     # scale & translation, eq. 42 and 41
     c = 1 / sigma_x * np.trace(np.diag(d).dot(s)) if with_scale else 1.0
@@ -101,3 +107,31 @@ def accumulated_distances(x: np.ndarray) -> np.ndarray:
     """
     return np.concatenate(
         (np.array([0]), np.cumsum(np.linalg.norm(x[:-1] - x[1:], axis=1))))
+
+def get_best_yaw(C: np.ndarray) -> float:
+    """Maximizes trace(Rz(theta) * C)
+    :param C: 3x3 rotation matrix
+    :return: yaw angle in radians
+    """
+    if C.shape != (3, 3):
+        raise GeometryException("C must be a 3x3 matrix")
+
+    A = C[0, 1] - C[1, 0]
+    B = C[0, 0] + C[1, 1]
+    theta = np.pi / 2.0 - np.arctan2(B, A)
+    return theta
+
+def rot_z(theta: float) -> np.ndarray:
+    """
+    Creates a rotation about the z-axis by an angle theta.
+    :param theta: rotation angle in radians
+    :return: 3x3 rotation matrix
+    """
+    cos_theta = np.cos(theta)
+    sin_theta = np.sin(theta)
+
+    return np.array([
+        [cos_theta, -sin_theta, 0],
+        [sin_theta,  cos_theta, 0],
+        [0,          0,         1]
+    ])

evo/core/trajectory.py

@@ -231,7 +231,7 @@ def project(self, plane: Plane) -> None:
 
     def align(self, traj_ref: 'PosePath3D', correct_scale: bool = False,
               correct_only_scale: bool = False,
-              n: int = -1) -> geometry.UmeyamaResult:
+              n: int = -1, yaw_only: bool = False) -> geometry.UmeyamaResult:
         """
         align to a reference trajectory using Umeyama alignment
         :param traj_ref: reference trajectory
@@ -246,14 +246,18 @@ def align(self, traj_ref: 'PosePath3D', correct_scale: bool = False,
         else:
             logger.debug("Aligning using Umeyama's method..." +
                          (" (with scale correction)" if with_scale else ""))
+
+        if yaw_only:
+            logger.debug("Computing yaw only alignment...")
+
         if n == -1:
             r_a, t_a, s = geometry.umeyama_alignment(self.positions_xyz.T,
                                                      traj_ref.positions_xyz.T,
-                                                     with_scale)
+                                                     with_scale, yaw_only)
         else:
             r_a, t_a, s = geometry.umeyama_alignment(
                 self.positions_xyz[:n, :].T, traj_ref.positions_xyz[:n, :].T,
-                with_scale)
+                with_scale, yaw_only)
 
         if not correct_only_scale:
             logger.debug("Rotation of alignment:\n{}"

evo/main_ape.py

@@ -41,7 +41,7 @@
 
 def ape(traj_ref: PosePath3D, traj_est: PosePath3D,
         pose_relation: metrics.PoseRelation, align: bool = False,
-        correct_scale: bool = False, n_to_align: int = -1,
+        correct_scale: bool = False, yaw_only: bool = False, n_to_align: int = -1,
         align_origin: bool = False, ref_name: str = "reference",
         est_name: str = "estimate",
         change_unit: typing.Optional[metrics.Unit] = None,
@@ -53,7 +53,8 @@ def ape(traj_ref: PosePath3D, traj_est: PosePath3D,
     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))
+            *traj_est.align(traj_ref, correct_scale, only_scale, 
+                            n=n_to_align, yaw_only=yaw_only))
     if align_origin:
         logger.debug(SEP)
         alignment_transformation = traj_est.align_origin(traj_ref)
@@ -76,10 +77,12 @@ def ape(traj_ref: PosePath3D, traj_est: PosePath3D,
         ape_metric.change_unit(change_unit)
 
     title = str(ape_metric)
-    if align and not correct_scale:
+    if align and not correct_scale and not yaw_only:
         title += "\n(with SE(3) Umeyama alignment)"
     elif align and correct_scale:
         title += "\n(with Sim(3) Umeyama alignment)"
+    elif align and yaw_only:
+        title += "\n(with position-yaw alignment)"
     elif only_scale:
         title += "\n(scale corrected)"
     elif not align_origin:
@@ -155,7 +158,7 @@ def run(args: argparse.Namespace) -> None:
     result = ape(traj_ref=traj_ref, traj_est=traj_est,
                  pose_relation=pose_relation, align=args.align,
                  correct_scale=args.correct_scale, n_to_align=args.n_to_align,
-                 align_origin=args.align_origin, ref_name=ref_name,
+                 yaw_only=args.yaw_only, align_origin=args.align_origin, ref_name=ref_name,
                  est_name=est_name, change_unit=change_unit,
                  project_to_plane=plane)
 

evo/main_ape_parser.py

@@ -25,6 +25,10 @@ def parser() -> argparse.ArgumentParser:
         "--n_to_align",
         help="the number of poses to use for Umeyama alignment, "
         "counted from the start (default: all)", default=-1, type=int)
+    algo_opts.add_argument(
+        "--yaw_only", help="align yaw (z-axis) only",
+        action="store_true"
+    )
     algo_opts.add_argument(
         "--change_unit", default=None,
         choices=[u.value for u in (units.ANGLE_UNITS + units.LENGTH_UNITS)],

evo/main_rpe.py

@@ -43,8 +43,8 @@ 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, pairs_from_reference: bool = False,
-        align: bool = False, correct_scale: bool = False, n_to_align: int = -1,
-        align_origin: bool = False, ref_name: str = "reference",
+        align: bool = False, correct_scale: bool = False, yaw_only: bool = False, 
+        n_to_align: int = -1, align_origin: bool = False, ref_name: str = "reference",
         est_name: str = "estimate", support_loop: bool = False,
         change_unit: typing.Optional[metrics.Unit] = None,
         project_to_plane: typing.Optional[Plane] = None) -> Result:
@@ -55,7 +55,8 @@ def rpe(traj_ref: PosePath3D, traj_est: PosePath3D,
     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))
+            *traj_est.align(traj_ref, correct_scale, only_scale, 
+                            n=n_to_align, yaw_only=yaw_only))
     if align_origin:
         logger.debug(SEP)
         alignment_transformation = traj_est.align_origin(traj_ref)
@@ -178,9 +179,9 @@ def run(args: argparse.Namespace) -> None:
                  all_pairs=args.all_pairs,
                  pairs_from_reference=args.pairs_from_reference,
                  align=args.align, correct_scale=args.correct_scale,
-                 n_to_align=args.n_to_align, align_origin=args.align_origin,
-                 ref_name=ref_name, est_name=est_name, change_unit=change_unit,
-                 project_to_plane=plane)
+                 n_to_align=args.n_to_align, align_origin=args.align_origin, 
+                 yaw_only=args.yaw_only, ref_name=ref_name, est_name=est_name, 
+                 change_unit=change_unit, project_to_plane=plane)
 
     if args.rerun and isinstance(traj_ref, PoseTrajectory3D) and isinstance(
             traj_est, PoseTrajectory3D):

evo/main_rpe_parser.py

@@ -25,6 +25,10 @@ def parser() -> argparse.ArgumentParser:
         "--n_to_align",
         help="the number of poses to use for Umeyama alignment, "
         "counted from the start (default: all)", default=-1, type=int)
+    algo_opts.add_argument(
+        "--yaw_only", help="align yaw (z-axis) only",
+        action="store_true"
+    )
     algo_opts.add_argument("-d", "--delta", type=float, default=1,
                            help="delta between relative poses")
     algo_opts.add_argument("-t", "--delta_tol", type=float, default=0.1,

evo/main_traj.py

@@ -262,7 +262,7 @@ def run(args):
                 trajectories[name].align(
                     ref_traj_tmp, correct_scale=args.correct_scale,
                     correct_only_scale=args.correct_scale and not args.align,
-                    n=args.n_to_align)
+                    n=args.n_to_align, yaw_only=args.yaw_only)
             if args.align_origin:
                 logger.debug(SEP)
                 logger.debug("Aligning {}'s origin to reference.".format(name))

evo/main_traj_parser.py

@@ -24,6 +24,10 @@ def parser() -> argparse.ArgumentParser:
         "--sync",
         help="associate trajectories via matching timestamps - requires --ref",
         action="store_true")
+    algo_opts.add_argument(
+        "--yaw_only", help="align yaw (z-axis) only",
+        action="store_true"
+    )
     algo_opts.add_argument(
         "--transform_left", help="path to a file with a transformation"
         " to apply to the trajectories (left multiplicative)")