Improve interplay of alignment flags (#694)

- Allow scale correction together with origin alignment.
- Handle strictly that `--align` and `--align_origin` shall be used mutually exclusive.

Relates to issue #693

evo/main_ape.py

@@ -54,7 +54,7 @@ def ape(traj_ref: PosePath3D, traj_est: PosePath3D,
         logger.debug(SEP)
         alignment_transformation = lie_algebra.sim3(
             *traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align))
-    elif align_origin:
+    if align_origin:
         logger.debug(SEP)
         alignment_transformation = traj_est.align_origin(traj_ref)
 
@@ -82,12 +82,12 @@ def ape(traj_ref: PosePath3D, traj_est: PosePath3D,
         title += "\n(with Sim(3) Umeyama alignment)"
     elif only_scale:
         title += "\n(scale corrected)"
-    elif align_origin:
-        title += "\n(with origin alignment)"
-    else:
+    elif not align_origin:
         title += "\n(not aligned)"
     if (align or correct_scale) and n_to_align != -1:
         title += " (aligned poses: {})".format(n_to_align)
+    if align_origin:
+        title += "\n(with origin alignment)"
 
     if project_to_plane:
         title += f"\n(projected to {project_to_plane.value} plane)"

evo/main_ape_parser.py

@@ -19,19 +19,12 @@ def parser() -> argparse.ArgumentParser:
             "full", "trans_part", "rot_part", "angle_deg", "angle_rad",
             "point_distance"
         ])
-    algo_opts.add_argument("-a", "--align",
-                           help="alignment with Umeyama's method (no scale)",
-                           action="store_true")
     algo_opts.add_argument("-s", "--correct_scale", action="store_true",
                            help="correct scale with Umeyama's method")
     algo_opts.add_argument(
         "--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(
-        "--align_origin",
-        help="align the trajectory origin to the origin of the reference "
-        "trajectory", action="store_true")
     algo_opts.add_argument(
         "--change_unit", default=None,
         choices=[u.value for u in (units.ANGLE_UNITS + units.LENGTH_UNITS)],
@@ -49,6 +42,15 @@ def parser() -> argparse.ArgumentParser:
         " is below the threshold distance or angle. "
         "Angle is expected in degrees.")
 
+    align_opts = algo_opts.add_mutually_exclusive_group()
+    align_opts.add_argument("-a", "--align",
+                            help="alignment with Umeyama's method (no scale)",
+                            action="store_true")
+    align_opts.add_argument(
+        "--align_origin",
+        help="align the trajectory origin to the origin of the reference "
+        "trajectory", action="store_true")
+
     output_opts.add_argument(
         "-p",
         "--plot",

evo/main_rpe.py

@@ -56,7 +56,7 @@ def rpe(traj_ref: PosePath3D, traj_est: PosePath3D,
         logger.debug(SEP)
         alignment_transformation = lie_algebra.sim3(
             *traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align))
-    elif align_origin:
+    if align_origin:
         logger.debug(SEP)
         alignment_transformation = traj_est.align_origin(traj_ref)
 
@@ -85,12 +85,12 @@ def rpe(traj_ref: PosePath3D, traj_est: PosePath3D,
         title += "\n(with Sim(3) Umeyama alignment)"
     elif only_scale:
         title += "\n(scale corrected)"
-    elif align_origin:
-        title += "\n(with origin alignment)"
-    else:
+    elif not align_origin:
         title += "\n(not aligned)"
     if (align or correct_scale) and n_to_align != -1:
         title += " (aligned poses: {})".format(n_to_align)
+    if align_origin:
+        title += "\n(with origin alignment)"
 
     if project_to_plane:
         title += f"\n(projected to {project_to_plane.value} plane)"

evo/main_rpe_parser.py

@@ -19,19 +19,12 @@ def parser() -> argparse.ArgumentParser:
             "full", "trans_part", "rot_part", "angle_deg", "angle_rad",
             "point_distance", "point_distance_error_ratio"
         ])
-    algo_opts.add_argument("-a", "--align",
-                           help="alignment with Umeyama's method (no scale)",
-                           action="store_true")
     algo_opts.add_argument("-s", "--correct_scale", action="store_true",
                            help="correct scale with Umeyama's method")
     algo_opts.add_argument(
         "--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(
-        "--align_origin",
-        help="align the trajectory origin to the origin of the reference "
-        "trajectory", 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,
@@ -64,6 +57,15 @@ def parser() -> argparse.ArgumentParser:
         " is below the threshold distance or angle. "
         "Angle is expected in degrees.")
 
+    align_opts = algo_opts.add_mutually_exclusive_group()
+    align_opts.add_argument("-a", "--align",
+                            help="alignment with Umeyama's method (no scale)",
+                            action="store_true")
+    align_opts.add_argument(
+        "--align_origin",
+        help="align the trajectory origin to the origin of the reference "
+        "trajectory", action="store_true")
+
     output_opts.add_argument(
         "-p",
         "--plot",

evo/main_traj_parser.py

@@ -13,20 +13,13 @@ def parser() -> argparse.ArgumentParser:
     shared_parser.add_argument("-f", "--full_check",
                                help="run all checks and print all stats",
                                action="store_true")
-    algo_opts.add_argument(
-        "-a", "--align", help="alignment with Umeyama's method (no scale)"
-        " - requires --ref", action="store_true")
     algo_opts.add_argument(
         "-s", "--correct_scale", help="scale correction with Umeyama's method"
         " - requires --ref", action="store_true")
     algo_opts.add_argument(
         "--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(
-        "--align_origin",
-        help="align the trajectory origin to the origin of the reference "
-        "trajectory", action="store_true")
     algo_opts.add_argument(
         "--sync",
         help="associate trajectories via matching timestamps - requires --ref",
@@ -68,6 +61,16 @@ def parser() -> argparse.ArgumentParser:
         help="Filters out poses if the distance or angle to the previous one "
         " is below the threshold distance or angle. "
         "Angle is expected in degrees.")
+
+    align_opts = algo_opts.add_mutually_exclusive_group()
+    align_opts.add_argument("-a", "--align",
+                            help="alignment with Umeyama's method (no scale)",
+                            action="store_true")
+    align_opts.add_argument(
+        "--align_origin",
+        help="align the trajectory origin to the origin of the reference "
+        "trajectory", action="store_true")
+
     output_opts.add_argument("-p", "--plot", help="show plot window",
                              action="store_true")
     output_opts.add_argument(