Scripts for twist evaluation and eagleye evaluation (#14)

* feat: support multi-file evalutaion for ros2bag

(a mixture of csv and ros2bag is allowed)

* fix: fix the dependencies of corresponding sub_scripts

* feat: update README

* feat: enrich error message for undefined data type

* hotfix: fix the dependencies for corresponding sub_scripts

(to be refactored)

* feat: implement the display of progress info

* feat: update the README

* feat: added evaluation scripts for single eagleye log file

* feat: changed some file names to prevent conflicts

* feat: implemented the graphs of xyz and xyz error

* feat: rearranged file strucuter

* feat: updated README

* feat: added the scripts of twist evaluation

* feat: added ellipse graph

* fix: fixed the imappropriate radian formatter

* fix: fixed the typos of config files

* fix: removed unnecessary files

* feat: simplified README

Author: ppaul12

README.md

@@ -1,23 +1,15 @@
-# localization_evaluation_toolkit -Ver.6.1 (Update 2023/03/14)
-You can evaluate your localization result by comparing it to a reliable pose trajectory.
-The start time, end time, and period can be different for each data.
-The evaluation is automatically aligned with the one with the smaller number of data.
+# localization_evaluation_toolkit
 
-## Releases
-| Version | Release Date       | Python version       | Note                                                         |
-| ------- | ------------------ | -------------------- | ------------------------------------------------------------ |
-| 6.0     | Feburary 3, 2023   | python 3.7 or higher | Support multi-file evaluation (mixtures of csv and ros2 bag) |
-| 5.0     | January 20, 2023   | python 3.7 or higher | Support multi-file evaluation (CSV only)                     |
-| 3.2     | September 29, 2022 | python 3.6 or higher | 1:1 evaluation                                               |
+An evaluator for localization results, which compares with a reliable pose trajectory. Specifically, comparisons between different start times, end times, and periods are supported, since evaluation will be automatically aligned with the one with fewer data.
 
-## Installation
+## 1. Installation & Preparation
+
+Clone this repo to the local environment.
 
 ```sh
 $ git clone https://github.com/MapIV/localization_evaluation_toolkit.git
 ```
 
-## Preparation
-
 Evaluatable data types are limited to the format of CSV or ros2 bag, and the start time, end time, and period can be different for each data.
 Specifically, a valid **CSV file** requires at least the following entries.
 
@@ -30,17 +22,19 @@ And a valid ros2 bag file should have one of the following types of topics.
 - `geometry_msgs/msg/PoseWithCovarianceStamped`
 - `nav_msgs/msg/Odometry`
 
-## Execution
+## 2. Main-evaluation
+
+### 2.1 Pose evaluation
 
-A standard configuration file for evaluation consists of the following parts.
+A standard configuration file for pose evaluation consists of the following parts.
 
 - Reference
 - Result1, Result2, ...
 - Optional settings
 
 It is required that the reference block should have the name of ***Reference*** and the names of result blocks should begin with ***Result***.
 When specifying the paths, it is recommended to use the absolute path.
-There is a sample YAML file at `config/evaluation.yaml`.
+There is a sample YAML file at `config/evaluation_pose.yaml`.
 To create your own configuration file, just use the following templates for CSV or ros2 bag,
 
 <details>
@@ -171,81 +165,236 @@ and run with
 
 ```sh
 $ cd localization_evaluation_toolkit
-$ python3 scripts/main.py config/evaluation.yaml
+$ python3 scripts/pose_main.py config/evaluation_pose.yaml
 ```
 
-## Sub-evaluation and adjustment
-
-**A. Evaluate TP, NVTL, execution time and iteration with ros2 bag files**
-
-Please input a rosbag containing the following (at least one) topic.
-```
-/localization/pose_estimator/pose_with_covariance
-/localization/pose_estimator/nearest_voxel_transformation_likelihood
-/localization/pose_estimator/transform_probability
-/localization/pose_estimator/exe_time_ms
-/localization/pose_estimator/iteration_num
-```
+For example, you may output the following graphs with the sample data.
 
 ```sh
-$ cd localization_evaluation_toolkit/sub_scripts
-$ source ~/xxxxxx/install/setup.bash
-$ python3 sub_ndt_evaluation.py [bag_path] [output_folder_path]
+$ cd localization_evaluation_toolkit
+$ python3 scripts/pose_main.py sample_data/config/sasashima_evaluation.yaml
 ```
 
-**B. Adjust time stamp with CSV files**
-```sh
-$ cd localization_evaluation_toolkit/sub_scripts
-$ python3 adjust_time_stamp.py [target_csv_path] [offset_csv_path] [ajust_time_stamp.yaml path] [output_folder_path]
-```
+<details>
+<summary>2D Trajectory</summary>
+<img src="sample_data/output_sample/2d_trajectory.png">
+When you zoom in on the graph, you can see the correspondence.
+<img src="sample_data/output_sample/2d_trajectory_zoom.png">
+</details>
 
-**C. Create CSV file with covariance column from rosbag file**  
-In detail, refer to [qiita article](https://qiita.com/koki2022/items/148d56e0f8eee45a0a62)
-```sh
-$ cd localization_evaluation_toolkit/sub_scripts
-$ python covariance_to_csv.py [input_bag_path] [output_folder_path]
+<details>
+<summary>3D Trajectory</summary>
+<img src="sample_data/output_sample/3d_trajectory.png">
+</details>
+
+<details>
+<summary>X Y Z</summary>
+<img src="sample_data/output_sample/xyz.png">
+</details>
+
+<details>
+<summary>X Y Z error</summary>
+<img src="sample_data/output_sample/xyz_error.png">
+</details>
+
+<details>
+<summary>2D Error</summary>
+<img src="sample_data/output_sample/2d_error.png">
+</details>
+
+<details>
+<summary>3D Error</summary>
+<img src="sample_data/output_sample/3d_error.png">
+</details>
+
+<details>
+<summary>Longitudinal Error</summary>
+<img src="sample_data/output_sample/longitudinal_error.png">
+</details>
+
+<details>
+<summary>Lateral Error</summary>
+<img src="sample_data/output_sample/lateral_error.png">
+</details>
+
+<details>
+<summary>Roll Pitch Yaw</summary>
+You can choose the unit of the vertical axis between radian and degree.
+<img src="sample_data/output_sample/rpy.png">
+</details>
+
+<details>
+<summary>Roll Pitch Yaw Error</summary>
+You can choose the unit of the vertical axis between radian and degree.
+<img src="sample_data/output_sample/rpy_error.png">
+</details>
+
+### 2.2 Twist evaluation
+
+A standard configuration file for twist evaluation consists of the following parts.
+
+- Reference
+- Twist
+- Optional settings
+
+It is required that the reference block should have the name of ***Reference*** and the names of result blocks should begin with ***Twist***.
+When specifying the paths, it is recommended to use the absolute path.
+There is a sample YAML file at `config/evaluation_twist.yaml`.
+To create your own configuration file, just use the following templates,
+
+<details>
+<summary>Template of Reference (click to expand)</summary>
+
+```yaml
+Reference:
+  ## Auxiliary info
+  label: reference
+  path: /path/to/reference/csv
+
+  ## Time
+  separate_time_stamp: false # [true]:Set secs_stamp_column and nsecs_stamp_column / [false]:Set stamp_column
+  #--------true--------#
+  secs_stamp_column: 1
+  nsecs_stamp_column: 2
+  #--------false-------#
+  stamp_column: 2
+
+  ## Position
+  x_column: 4
+  y_column: 5
+  z_column: 6
+
+  ## Rotation
+  use_quaternion: true # [true]:Set Quaternion / [false]:Set Euler
+  #--------true--------#
+  # Quaternion
+  ori_x_column: 7
+  ori_y_column: 8
+  ori_z_column: 9
+  ori_w_column: 10
+  #--------false-------#
+  # Euler
+  use_radian: true # [true]:radian / [false]:degree
+  roll_column: 7
+  pitch_column: 8
+  yaw_column: 9
+
+  ## Enu velocity
+  use_enu_vel: false # [true]: use enu velocity / [false]: ignored
+  vel_x_column: 0
+  vel_y_column: 0
+  vel_z_column: 0
+
+  ## Angular
+  use_angular: false # [true]: use angular / [false]: ignored
+  angular_x_column: 0
+  angular_y_column: 0
+  angular_z_column: 0
+
+  # GNSS quality
+  use_gnss_qual: false # [true]: use GNSS quality / [false]: ignored
+  gnss_qual: 10
 ```
 
-## Output graphs
-Sample output graphs are shown below. You can test this evaluation script easily with the following command. All graphs are output in full HD.
-```sh
-$ cd localization_evaluation_toolkit/scripts
-$ python3 main.py ../sample_data/config/sasashima_evaluation.yaml
+</details>
+
+<details>
+<summary>Template of Twist (click to expand)</summary>
+
+```yaml
+Twist:
+  ## Auxiliary info
+  label: twist
+  path: /path/to/twist/csv
+
+  ## Time
+  separate_time_stamp: false # [true]:Set secs_stamp_column and nsecs_stamp_column / [false]:Set stamp_column
+  #--------true--------#
+  secs_stamp_column: 1
+  nsecs_stamp_column: 2
+  #--------false-------#
+  stamp_column: 2
+
+  ## Linear
+  linear_x_column: 4
+  linear_y_column: 5
+  linear_z_column: 6
+
+  ## Angular
+  angular_x_column: 7
+  angular_y_column: 8
+  angular_z_column: 9
 ```
 
-1. 2D Trajectory
+</details>
 
-![2d_traj](/sample_data/output_sample/2d_trajectory.png)
+<details>
+<summary>Template of optional settings (click to expand)</summary>
 
-When you zoom in on the graph, you can see the correspondence.
-![2D_traj_zoom](/sample_data/output_sample/2d_trajectory_zoom.png)
+```yaml
+# Trajectory graph numbering
+progress_info: 0 # [0]:off, [1]:number, [2]:time, [3]:ros time, [4]:distance
+interval: 0      # progress_info is [2]:second, [3]:second, [4]:meter
 
-2. 2D Error
+# Misc
+sync_time_threshold: 0.01  # Time threshold for judgment for time synchronization [s]
+leap_time: 0.0             # Offset correction for time synchronizatio [s]
+based_heading_angle: false # [true]:The heading angle is based on North / [false]:The heading angle is based on East (ros data)
+distance_length: 100       # Distance to calculate relative trajectory [m]
+distance_step: 50          # Calculate relative trajectories step [m]
+eval_step_max: 3.0   # Maximum value of error to be evaluated default [m]
 
-![2d_error](/sample_data/output_sample/2d_error.png)
+# Font
+title_font_size: 14
+label_font_size: 10
+ticks_font_size: 8
 
-3. Height Error
+# Save
+save_figures: true
+save_extension_type: png # without "."
+save_dataframe: true
+output_directory: /path/to/output/directory
+```
 
-![height_error](/sample_data/output_sample/height_error.png)
+</details>
 
-4. 3D Error
+and run with
 
-![3d_error](/sample_data/output_sample/3d_error.png)
+```sh
+$ cd localization_evaluation_toolkit
+$ python3 scripts/twist_main.py config/evaluation_twist.yaml
+```
 
-5. Longitudinal Error
+## 3. Sub-evaluation
 
-![longitudinal_error](/sample_data/output_sample/longitudinal_error.png)
+### 3.1 NDT performance
 
-6. Lateral Error
+Evaluation of TP, NVTL, execution time, iteration and error ellipse with ros2 bag files.
 
-![lateral_error](/sample_data/output_sample/lateral_error.png)
+The input rosbag file should contain the following (at least one) topic.
+```
+/localization/pose_estimator/pose_with_covariance
+/localization/pose_estimator/nearest_voxel_transformation_likelihood
+/localization/pose_estimator/transform_probability
+/localization/pose_estimator/exe_time_ms
+/localization/pose_estimator/iteration_num
+```
+
+```sh
+$ cd localization_evaluation_toolkit/sub_scripts
+$ source ~/xxxxxx/install/setup.bash
+$ python3 sub_ndt_evaluation.py [bag_path] [output_folder_path]
+```
 
-7. Roll Pitch Yaw
+### 3.2 Eagleye log evaluation
 
-You can choose the unit of the vertical axis between radian and degree.
-![rpy](/sample_data/output_sample/rpy.png)
+Currently, refer to [the original doc page](https://github.com/MapIV/eagleye/tree/main-ros1/eagleye_util/trajectory_plot#eagleye_pp_single_evaluation) for details.
 
-8. Roll Pitch Yaw Error
+### 3.3 Covariance file generation
 
-You can choose the unit of the vertical axis between radian and degree.
-![rpy_error](/sample_data/output_sample/rpy_error.png)
+Create a CSV file with covariance columns from rosbag file. In detail, refer to [qiita article](https://qiita.com/koki2022/items/148d56e0f8eee45a0a62) for details.
+
+```sh
+$ cd localization_evaluation_toolkit/sub_scripts
+$ python covariance_to_csv.py [input_bag_path] [output_folder_path]
+```