localization_evaluation_toolkit

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.

1. Installation & Preparation

Clone this repo to the local environment.

$ git clone https://github.com/MapIV/localization_evaluation_toolkit.git

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.

• Timestamp
• Position (x,y,z)
• Rotation (Quaternion or Euler[degree or radian])

And a valid ros2 bag file should have one of the following types of topics.

• geometry_msgs/msg/PoseWithCovarianceStamped
• nav_msgs/msg/Odometry

2. Main-evaluation

2.1 Pose evaluation

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_pose.yaml. To create your own configuration file, just use the following templates for CSV or ros2 bag,

Template of CSV (click to expand)

Reference/Result:
## Auxiliary infolabel: anytype: 0# [0]: csv, [1]: ros2bagpath: /path/to/csv## Timeseparate_time_stamp: false # [true]:Set secs_stamp_column and nsecs_stamp_column / [false]:Set stamp_column#--------true--------#secs_stamp_column: 2nsecs_stamp_column: 3#--------false-------#stamp_column: 0## Positionx_column: 1y_column: 2z_column: 3## Rotationuse_quaternion: false # [true]:Set Quaternion / [false]:Set Euler#--------true--------## Quaternionori_x_column: 8ori_y_column: 9ori_z_column: 10ori_w_column: 11#--------false-------## Euleruse_radian: true # [true]:radian / [false]:degreeroll_column: 4pitch_column: 5yaw_column: 6## TFtf_time: 0# [s]tf_x: 0# [m]tf_y: 0# [m]tf_z: 0# [m]tf_roll: 0# [rad]tf_pitch: 0# [rad]tf_yaw: 0# [rad]inv_roll: 1# 1 or -1inv_pitch: 1# 1 or -1inv_yaw: 1# 1 or -1# Display ellipse (put 2D covariance in result file)display_ellipse: falsecovariance_xx_column: 10covariance_xy_column: 11covariance_yx_column: 12covariance_yy_column: 13

Template of ros2 bag (click to expand)

Reference/Result:
## Auxiliary infolabel: anytype: 1# [0]: csv, [1]: ros2bagpath: /path/to/ros2bag## Rosbag infotopic_name: /localization/pose_estimator/pose_with_covariancestorage_id: sqlite3serialization_format: cdr## TFtf_time: 0# [s]tf_x: 0# [m]tf_y: 0# [m]tf_z: 0# [m]tf_roll: 0# [rad]tf_pitch: 0# [rad]tf_yaw: 0# [rad]inv_roll: 1# 1 or -1inv_pitch: 1# 1 or -1inv_yaw: 1# 1 or -1# Display ellipse (put 2D covariance in result file)display_ellipse: false # use PoseWithCovarianceStamped tyep topic in result data

Template of optional settings (click to expand)

# Horizontal axisaxis_type: 0# [0]:time, [1]:distancedegree_type: 0# [0]:radian, [1]:degree# Trajectory graph dilutiondilution_step: 10# at least 1, the larger the sparser for better performance# Trajectory graph numberingprogress_info: 0# [0]:off, [1]:number, [2]:time, [3]:ros time, [4]:distanceinterval: 0# progress_info is [2]:second, [3]:second, [4]:meter# Fonttitle_font_size: 14label_font_size: 10ticks_font_size: 8# Savesave_figures: truesave_extension_type: png # without "."save_dataframe: trueoutput_directory: /path/to/output/directoryuse_lerp: false

and run with

$ cd localization_evaluation_toolkit
$ python3 scripts/pose_main.py config/evaluation_pose.yaml

For example, you may output the following graphs with the sample data.

$ cd localization_evaluation_toolkit
$ python3 scripts/pose_main.py sample_data/config/sasashima_evaluation.yaml

2D Trajectory

When you zoom in on the graph, you can see the correspondence.

3D Trajectory

X Y Z

X Y Z error

2D Error

3D Error

Longitudinal Error

Lateral Error

Roll Pitch Yaw

You can choose the unit of the vertical axis between radian and degree.

Roll Pitch Yaw Error

You can choose the unit of the vertical axis between radian and degree.

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,

Template of Reference (click to expand)

Reference:
## Auxiliary infolabel: referencepath: /path/to/reference/csv## Timeseparate_time_stamp: false # [true]:Set secs_stamp_column and nsecs_stamp_column / [false]:Set stamp_column#--------true--------#secs_stamp_column: 1nsecs_stamp_column: 2#--------false-------#stamp_column: 2## Positionx_column: 4y_column: 5z_column: 6## Rotationuse_quaternion: true # [true]:Set Quaternion / [false]:Set Euler#--------true--------## Quaternionori_x_column: 7ori_y_column: 8ori_z_column: 9ori_w_column: 10#--------false-------## Euleruse_radian: true # [true]:radian / [false]:degreeroll_column: 7pitch_column: 8yaw_column: 9## Enu velocityuse_enu_vel: false # [true]: use enu velocity / [false]: ignoredvel_x_column: 0vel_y_column: 0vel_z_column: 0## Angularuse_angular: false # [true]: use angular / [false]: ignoredangular_x_column: 0angular_y_column: 0angular_z_column: 0# GNSS qualityuse_gnss_qual: false # [true]: use GNSS quality / [false]: ignoredgnss_qual: 10

Template of Twist (click to expand)

Twist:
## Auxiliary infolabel: twistpath: /path/to/twist/csv## Timeseparate_time_stamp: false # [true]:Set secs_stamp_column and nsecs_stamp_column / [false]:Set stamp_column#--------true--------#secs_stamp_column: 1nsecs_stamp_column: 2#--------false-------#stamp_column: 2## Linearlinear_x_column: 4linear_y_column: 5linear_z_column: 6## Angularangular_x_column: 7angular_y_column: 8angular_z_column: 9

Template of optional settings (click to expand)

# Trajectory graph numberingprogress_info: 0# [0]:off, [1]:number, [2]:time, [3]:ros time, [4]:distanceinterval: 0# progress_info is [2]:second, [3]:second, [4]:meter# Miscsync_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]# Fonttitle_font_size: 14label_font_size: 10ticks_font_size: 8# Savesave_figures: truesave_extension_type: png # without "."save_dataframe: trueoutput_directory: /path/to/output/directory

and run with

$ cd localization_evaluation_toolkit
$ python3 scripts/twist_main.py config/evaluation_twist.yaml

3. Sub-evaluation

3.1 NDT performance

Evaluation of TP, NVTL, execution time, iteration and error ellipse with ros2 bag files.

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

$ cd localization_evaluation_toolkit/sub_scripts
$ source~/xxxxxx/install/setup.bash
$ python3 sub_ndt_evaluation.py [bag_path] [output_folder_path]

3.2 Eagleye log evaluation

Currently, refer to the original doc page for details.

3.3 Covariance file generation

Create a CSV file with covariance columns from rosbag file. In detail, refer to qiita article for details.

$ cd localization_evaluation_toolkit/sub_scripts
$ python covariance_to_csv.py [input_bag_path] [output_folder_path]