feat: Removed ROS dependency and upgraded code to work with python3. Additionally implemented fix described in #37

.gitignore

@@ -0,0 +1,2 @@
+rpg_trajectory_evaluation.egg-info
+__pycache__

README.md

@@ -40,11 +40,11 @@ Zichao Zhang, Davide Scaramuzza: A Tutorial on Quantitative Trajectory Evaluatio
 6. [Credits](#credits)
 
 ## Install
-The package is written in python and tested in Ubuntu 16.04 and 18.04.
-Currently only `python2` is supported.
-The package can be used as a ROS package as well as a standalone tool.
-To use it as a ROS package, simply clone it into your workspace.
-It only depends on [`catkin_simple`](https://github.com/catkin/catkin_simple) to build.
+
+Install the package:
+```
+pip install -e .
+```
 
 **Dependencies**: You will need install the following:
 
@@ -100,22 +100,21 @@ Currently `eval_cfg.yaml` specifies two parameters for trajectory alignment (use
 
 **If this file does not exist, analysis be done for all the poses in `stamped_traj_estimate.txt`.**
 
-
 ## Run the Evaluation
 We can run the evaluation on a single estimate result or for multiple algorithms and datasets.
 
 ### Single trajectory estimate
 
-As a ROS package, run
+To evaluate a single trajectory estimate, run:
 
 ```
-rosrun rpg_trajectory_evaluation analyze_trajectory_single.py <result_folder>
+python3 scripts/analyze_trajectory_single.pyy <result_folder> 
 ```
 
-or as a standalone package, run
+or
 
 ```
-python2 analyze_trajectory_single.py <result_folder> 
+analyze_trajectory_single.pyy <result_folder> 
 ```
 
 `<result_folder>` should contain the groundtruth, trajectory estimate and optionally the evaluation configuration as mentioned above.
@@ -132,7 +131,7 @@ For multiple trials, the result for trial `n` will have the corresponding suffix
 
 As an example, after executing:
 ```
-python2 scripts/analyze_trajectory_single.py results/euroc_mono_stereo/laptop/vio_mono/laptop_vio_mono_MH_05
+python3 scripts/analyze_trajectory_single.py results/euroc_mono_stereo/laptop/vio_mono/laptop_vio_mono_MH_05
 ```
 you will find the following in `plots`:
 
@@ -155,16 +154,10 @@ The mapping from the `est_type` to file names (i.e., `stamped_*.txt`) is defined
 
 ### Multiple trajectory estimates
 
-Similar to the case of single trajectory evaluation, for ROS, run
-
-```
-rosrun rpg_trajectory_evaluation analyze_trajectories.py \
-  euroc_vislam_mono.yaml --output_dir=./results/euroc_vislam_mono --results_dir=./results/euroc_vislam_mono --platform laptop --odometry_error_per_dataset --plot_trajectories --rmse_table --rmse_boxplot --mul_trials=10
-```
-otherwise, run
+Similar to the case of single trajectory evaluation, run
 
 ```
-python2 scripts/analyze_trajectories.py \
+python3 scripts/analyze_trajectories.py \
   euroc_vislam_mono.yaml --output_dir=./results/euroc_vislam_mono --results_dir=./results/euroc_vislam_mono --platform laptop --odometry_error_per_dataset --plot_trajectories --rmse_table --rmse_boxplot --mul_trials=10
 ```
 

scripts/add_path.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import sys

scripts/analyze_trajectories.py

@@ -1,23 +1,22 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
-import os
 import argparse
-from ruamel.yaml import YAML
-import shutil
 import json
+import os
+import shutil
 from datetime import datetime
 
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import rc
-from colorama import init, Fore
-
 import add_path
-from trajectory import Trajectory
+import matplotlib.pyplot as plt
+import numpy as np
 import plot_utils as pu
 import results_writer as res_writer
 from analyze_trajectory_single import analyze_multiple_trials
-from fn_constants import kNsToEstFnMapping, kNsToMatchFnMapping, kFnExt
+from colorama import Fore, init
+from fn_constants import kFnExt, kNsToEstFnMapping, kNsToMatchFnMapping
+from matplotlib import rc
+from ruamel.yaml import YAML
+from trajectory import Trajectory
 
 init(autoreset=True)
 
@@ -26,11 +25,7 @@
 
 FORMAT = '.pdf'
 
-def spec(N):                                             
-    t = np.linspace(-510, 510, N)                                              
-    return np.round(np.clip(np.stack([-t, 510-np.abs(t), t], axis=1), 0, 255)).astype("float32")/255
-
-PALLETE = spec(20)
+PALLETE = ['b', 'g', 'r', 'c', 'k', 'y', 'm']
 
 
 def collect_odometry_error_per_dataset(dataset_multierror_list,
@@ -545,36 +540,38 @@ def parse_config_file(config_fn, sort_names):
     print("#####################################")
     print(Fore.RED+">>> Processing absolute trajectory errors...")
     if args.rmse_table:
-        rmse_table = {}
-        rmse_table['values'] = []
-        for config_mt_error in config_multierror_list:
-            cur_trans_rmse = []
-            for mt_error_d in config_mt_error:
-                print("> Processing {0}".format(mt_error_d.uid))
-                if args.rmse_median_only or n_trials == 1:
-                    cur_trans_rmse.append(
-                        "{:3.3f}".format(
-                            mt_error_d.abs_errors['rmse_trans_stats']['median']))
-                else:
-                    cur_trans_rmse.append(
-                        "{:3.3f}, {:3.3f} ({:3.3f} - {:3.3f})".format(
-                            mt_error_d.abs_errors['rmse_trans_stats']['mean'],
-                            mt_error_d.abs_errors['rmse_trans_stats']['median'],
-                            mt_error_d.abs_errors['rmse_trans_stats']['min'],
-                            mt_error_d.abs_errors['rmse_trans_stats']['max']))
-            rmse_table['values'].append(cur_trans_rmse)
-        rmse_table['rows'] = algorithms
-        rmse_table['cols'] = datasets
-        print('\n--- Generating RMSE tables... ---')
-        res_writer.write_tex_table(
-            rmse_table['values'], rmse_table['rows'], rmse_table['cols'],
-            os.path.join(output_dir, args.platform + '_translation_rmse_' +
-                         eval_uid+'.txt'))
+        for absMetricKey in ['rmse_trans', 'rmse_rot', 'rmse_scale']:
+            rmse_table = {}
+            rmse_table['values'] = []
+            for config_mt_error in config_multierror_list:
+                cur_rmse = []
+                for mt_error_d in config_mt_error:
+                    print("> Processing {0}".format(mt_error_d.uid))
+                    if args.rmse_median_only or n_trials == 1:
+                        cur_rmse.append(
+                            "{:3.3f}".format(
+                                mt_error_d.abs_errors[absMetricKey + '_stats']['median']))
+                    else:
+                        cur_rmse.append(
+                            "{:3.3f}, {:3.3f} ({:3.3f} - {:3.3f})".format(
+                                mt_error_d.abs_errors[absMetricKey + '_stats']['mean'],
+                                mt_error_d.abs_errors[absMetricKey + '_stats']['median'],
+                                mt_error_d.abs_errors[absMetricKey + '_stats']['min'],
+                                mt_error_d.abs_errors[absMetricKey + '_stats']['max']))
+                rmse_table['values'].append(cur_rmse)
+            rmse_table['rows'] = algorithms
+            rmse_table['cols'] = datasets
+            print('\n--- Generating RMSE tables... ---')
+            res_writer.write_tex_table(
+                rmse_table['values'], rmse_table['rows'], rmse_table['cols'],
+                os.path.join(output_dir, args.platform + '_' + absMetricKey + '_' +
+                            eval_uid+'.txt'))
 
     if args.rmse_boxplot and n_trials > 1:
         rmse_plot_alg = [v for v in algorithms]
         algorithm_rmse = collect_rmse_per_dataset(config_multierror_list,
                                                   rmse_plot_alg)
+        res_writer.save_algorithm_rmse(algorithm_rmse, datasets, algorithms, n_trials, output_dir)
         print("--- Generate boxplot for RMSE ---")
         plot_rmse_per_dataset(algorithm_rmse, datasets, algorithms,
                               output_dir, plot_settings)

scripts/analyze_trajectory_single.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import argparse

scripts/dataset_tools/asl_groundtruth_to_pose.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import argparse

scripts/dataset_tools/bag_to_pose.py

@@ -1,54 +1,62 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
-import os
 import argparse
-
-import rosbag
-
-
-def extract(bagfile, pose_topic, msg_type, out_filename):
-    n = 0
-    f = open(out_filename, 'w')
-    f.write('# timestamp tx ty tz qx qy qz qw\n')
-    with rosbag.Bag(bagfile, 'r') as bag:
-        for (topic, msg, ts) in bag.read_messages(topics=str(pose_topic)):
-            if msg_type == "PoseWithCovarianceStamped":
-                f.write('%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' %
-                        (msg.header.stamp.to_sec(),
-                         msg.pose.pose.position.x, msg.pose.pose.position.y,
-                         msg.pose.pose.position.z,
-                         msg.pose.pose.orientation.x,
-                         msg.pose.pose.orientation.y,
-                         msg.pose.pose.orientation.z,
-                         msg.pose.pose.orientation.w))
-            elif msg_type == "PoseStamped":
-                f.write('%.12f %.12f %.12f %.12f %.12f %.12f %.12f %.12f\n' %
-                        (msg.header.stamp.to_sec(),
-                         msg.pose.position.x, msg.pose.position.y,
-                         msg.pose.position.z,
-                         msg.pose.orientation.x, msg.pose.orientation.y,
-                         msg.pose.orientation.z, msg.pose.orientation.w))
-            else:
-                assert False, "Unknown message type"
-            n += 1
-    print('wrote ' + str(n) + ' imu messages to the file: ' + out_filename)
-
-
-if __name__ == '__main__':
-    parser = argparse.ArgumentParser(description='''
-    Extracts IMU messages from bagfile.
-    ''')
-    parser.add_argument('bag', help='Bagfile')
-    parser.add_argument('topic', help='Topic')
-    parser.add_argument('--msg_type', default='PoseStamped',
-                        help='message type')
-    parser.add_argument('--output', default='stamped_poses.txt',
-                        help='output filename')
+import os
+from pathlib import Path
+
+from rosbags.highlevel import AnyReader
+
+
+def extract(bag_path, pose_topic, msg_type_str, out_filename):
+    with AnyReader([Path(bag_path)]) as reader:
+        connections = [c for c in reader.connections if c.topic == pose_topic]
+        if not connections:
+            raise RuntimeError(f"Topic {pose_topic} not found in bag.")
+
+        with open(out_filename, 'w') as f:
+            f.write('# timestamp tx ty tz qx qy qz qw\n')
+            n = 0
+
+            for conn, timestamp, rawdata in reader.messages(connections=connections):
+                msg = reader.deserialize(rawdata, conn.msgtype)
+                stamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+
+                if msg_type_str == "PoseWithCovarianceStamped":
+                    p = msg.pose.pose.position
+                    o = msg.pose.pose.orientation
+                elif msg_type_str == "PoseStamped":
+                    p = msg.pose.position
+                    o = msg.pose.orientation
+                elif msg_type_str == "TransformStamped":
+                    p = msg.transform.translation
+                    o = msg.transform.rotation
+                elif msg_type_str == "Odometry":
+                    p = msg.pose.pose.position
+                    o = msg.pose.pose.orientation
+                else:
+                    raise RuntimeError("Unexpected message type at runtime")
+
+                f.write(f'{stamp:.12f} {p.x:.12f} {p.y:.12f} {p.z:.12f} '
+                        f'{o.x:.12f} {o.y:.12f} {o.z:.12f} {o.w:.12f}\n')
+                n += 1
+
+            print(f"Wrote {n} messages to {out_filename}")
+
+
+def main():
+    parser = argparse.ArgumentParser(description='Extract pose messages from a bag file (ROS 2 format, no ROS deps).')
+    parser.add_argument('bag', help='Path to ROS 2 bag folder (.db3 or .mcap)')
+    parser.add_argument('topic', help='Pose topic to extract')
+    parser.add_argument('--msg_type', default='PoseStamped', help='Message type')
+    parser.add_argument('--output', default='stamped_poses.txt', help='Output filename')
     args = parser.parse_args()
 
     out_dir = os.path.dirname(os.path.abspath(args.bag))
     out_fn = os.path.join(out_dir, args.output)
 
-    print('Extract pose from bag '+args.bag+' in topic ' + args.topic)
-    print('Saving to file '+out_fn)
+    print(f"Extracting from {args.bag}, topic: {args.topic}, type: {args.msg_type}")
     extract(args.bag, args.topic, args.msg_type, out_fn)
+
+
+if __name__ == '__main__':
+    main()

scripts/dataset_tools/stamp_state_est.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import argparse

scripts/dataset_tools/stamp_state_est_using_matches.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import argparse

scripts/dataset_tools/strip_gt_id.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import argparse

scripts/dataset_tools/transform_trajectory.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
 Created on Wed Apr 18 11:16:38 2018

scripts/fn_constants.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 kNsToEstFnMapping = {'traj_est': 'stamped_traj_estimate',
                      'pose_graph': 'stamped_pose_graph_estimate',

scripts/overall_odometry_errors.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 
 import os
 import numpy as np