Merge branch 'main' of https://github.com/sair-lab/AirDOS

Evaluation/README.md

@@ -0,0 +1,21 @@
+## Trajectory Format
+
+The output trajectory's format is the same as TUM trajectory format under EDN coordinate, with 8 columns, each representing
+
+```
+timestamp tx ty tz qx qy qz qw
+```
+
+## Environment
+
+`evo` ([Link to pypi](https://pypi.org/project/evo/)) and `matplotlib` are required for this evaluation script.
+
+## Run Evaluation
+
+To run evaluation, use `evaluate.py` file in this directory with following CLI arguments:
+
+```
+python ./evaluate.py --estimate <airdos_output_path> --gt <tartanair_shibuya_path> [--real_perspective]?
+```
+
+The `--real_perspective` flag is optional. Adding this flag will plot the resulted trajectory while enforcing the 1:1 perspective ratio.

Evaluation/evaluate.py

@@ -0,0 +1,45 @@
+from evo.tools.file_interface import read_tum_trajectory_file
+from evo.main_ape import ape
+from evo.main_rpe import rpe
+from evo.core.metrics import PoseRelation, Unit
+from evo.tools.plot import traj, PlotMode
+from evo.tools.settings import SETTINGS
+from pathlib import Path
+
+import argparse
+import matplotlib.pyplot as plt
+
+if __name__ == "__main__":
+    Parser = argparse.ArgumentParser(description="Plot and evaluate the AirDOS output trajectory on TartanAir-shibuya Dataset")
+    Parser.add_argument("--estimate", action="store", required=True, help="Estimated trajectory file (*.txt) with 8 columns")
+    Parser.add_argument("--gt", action="store", required=True, help="Ground truth trajectory file (*.txt) in TartanAir-shibuya dataset")
+    Parser.add_argument("--real_perspective", action="store_true", default=False, help="Add this option to enforce 1:1 perspective ratio in plotting")
+
+
+    args = Parser.parse_args()
+    print(args)
+    est_traj_path = Path(args.estimate)
+    gt_traj_path = Path(args.gt)
+
+    assert est_traj_path.exists()
+    assert gt_traj_path.exists()
+
+    SETTINGS.plot_xyz_realistic = args.real_perspective
+    est_traj = read_tum_trajectory_file(est_traj_path)
+    gt_traj = read_tum_trajectory_file(gt_traj_path)
+
+    ape_output = ape(gt_traj, est_traj, pose_relation=PoseRelation.translation_part, align=True, correct_scale=False)
+    rpe_t_output = rpe(gt_traj, est_traj, pose_relation=PoseRelation.translation_part, delta=1.0, delta_unit=Unit.frames,
+                    all_pairs=True, align=True, correct_scale=False)
+    rpe_r_output = rpe(gt_traj, est_traj, pose_relation=PoseRelation.rotation_part, delta=1.0, delta_unit=Unit.frames,
+                    all_pairs=True, align=True, correct_scale=False)
+
+    print("ATE:", round(ape_output.stats["rmse"], 3))
+    print("RPE (Translation)", round(rpe_t_output.stats["rmse"], 3))
+    print("RPE (Rotation)", round(rpe_r_output.stats["rmse"], 3))
+
+    fig = plt.figure(dpi=200)
+    ax = fig.add_subplot(1, 1, 1)
+    traj(ax, plot_mode=PlotMode.xz, traj=est_traj, color="r", label="AirDOS")
+    traj(ax, plot_mode=PlotMode.xz, traj=gt_traj, color="b", label="Ground Truth")
+    plt.show()

README.md

@@ -36,8 +36,6 @@ Currently, the project only supports `Stereo` mode. The `stereo_human` executabl
 
 ## Evaluate Trajectory
 
-To plot the trajectories (in xy, yz, xz views), run `./Evaluation/Plot.py`
-
 For evaluation, please check https://github.com/castacks/tartanair_tools.git.
 
 > Note: the TartanAir Tools expect to read the trajectory format with 7 columns, representing the translation and quaternion.
@@ -46,6 +44,8 @@ For evaluation, please check https://github.com/castacks/tartanair_tools.git.
 > 
 > To evaluate the exported trajectory using TartanAir Tools, you need to remove the first column of exported trajectory file.
 
+We also provided an evaluation tool based on `evo` package, for more detail, please check this [README.md](Evaluation/README.md).
+
 # Publications
 
 [AirDOS: Dynamic SLAM benefits from Articulated Objects