fix(deviation_evaluator): fix bug in map launch and plot

localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml

@@ -12,7 +12,7 @@
   <arg name="save_dir" default="$(env HOME)/deviation_evaluator_sample"/>
   <arg name="save_dir" default="$(env HOME)/deviation_evaluator_sample"/>
   <arg name="param_path" default="$(find-pkg-share deviation_evaluator)/config/deviation_evaluator.param.yaml"/>
   <!-- TODO before merge (koji minoda) any other way to get appropriate param? -->
   <arg name="error_monitor_param_path" default="$(find-pkg-share autoware_launch)/config/localization/localization_error_monitor.param.yaml"/>
   <arg name="imu_corrector_param_path" default="$(env HOME)/imu_corrector.param.yaml"/>
   <arg name="vehicle_velocity_converter_param_path" default="$(env HOME)/vehicle_velocity_converter.param.yaml"/>
@@ -104,10 +104,10 @@
 
   <!-- Map -->
   <group>
-    <include file="$(find-pkg-share map_loader)/launch/lanelet2_map_loader.launch.xml" if="$(var rviz)">
+    <include file="$(find-pkg-share tier4_map_launch)/launch/map.launch.py" if="$(var rviz)">
       <arg name="lanelet2_map_path" value="$(var lanelet2_map_file)"/>
-      <arg name="lanelet2_map_topic" value="/map/vector_map"/>
-      <arg name="lanelet2_map_marker_topic" value="/map/vector_map_marker"/>
+      <arg name="pointcloud_map_loader_param_path" value="$(find-pkg-share autoware_launch)/config/map/pointcloud_map_loader.param.yaml"/>
+      <arg name="lanelet2_map_loader_param_path" value="$(find-pkg-share autoware_launch)/config/map/lanelet2_map_loader.param.yaml"/>
     </include>
   </group>
 

localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py

@@ -20,6 +20,7 @@
 import dataclasses
 import sqlite3
 
+from constants import THRESHOLD_FOR_INITIALIZED_ERROR
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from geometry_msgs.msg import TwistWithCovarianceStamped
 from nav_msgs.msg import Odometry
@@ -191,7 +192,7 @@
             ekf_dr_pose_list[:, 0].tolist(),
         )
 
         long_axis_angles = get_long_axis_angles_from_covs(ekf_dr_pose_cov_list[:, :2, :2])
         errors = ekf_gt_pose_list_interpolated[:, 1:3] - ekf_dr_pose_list[:, 1:3]
         errors_along_elliptical_axis = transform_errors(errors, long_axis_angles)
         errors_along_body_frame = transform_errors(errors, ekf_dr_pose_list[:, 4])
@@ -208,17 +209,19 @@
         stddev_long_2d, stddev_short_2d = calc_long_short_radius(ekf_dr_pose_cov_list)
         stddev_long_2d_gt, stddev_short_2d_gt = calc_long_short_radius(ekf_gt_pose_cov_list)
 
+        ignore_index = np.where(stddev_long_2d_gt < THRESHOLD_FOR_INITIALIZED_ERROR)[0][0]
+
         long_radius_results = ErrorResults(
             "long_radius",
             np.linalg.norm(errors_along_elliptical_axis, axis=1)[valid_idxs],
             stddev_long_2d[valid_idxs] * params["scale"],
-            np.max(stddev_long_2d_gt[50:]) * params["scale"],
+            np.max(stddev_long_2d_gt[ignore_index:]) * params["scale"],
         )
         lateral_results = ErrorResults(
             "lateral",
             np.abs(errors_along_body_frame[valid_idxs, 1]),
             stddev_lateral_2d[valid_idxs] * params["scale"],
-            np.max(stddev_lateral_2d_gt[50:]) * params["scale"],
+            np.max(stddev_lateral_2d_gt[ignore_index:]) * params["scale"],
         )
         longitudinal_results = ErrorResults(
             "longitudinal",
@@ -284,9 +287,9 @@
     return poses_interpolated, valid_idxs
 
 
 def get_long_axis_angles_from_covs(covs):
     angles = []
     for cov in covs:
         eigen_values, eigen_vectors = np.linalg.eig(np.linalg.inv(cov))
 
         idx = eigen_values.argsort()[::-1]
@@ -361,7 +364,7 @@
     # Apply some test here as a temporary measure.
     # Ideally this test should be implemented as a rostest.
 
     # get_long_axis_angles_from_covs
     cov0 = np.array([[1, 0], [0, 2]])
     cov1 = np.array([[2, 0], [0, 1]])
     cov2 = np.linalg.inv(np.array([[2, -1], [-1, 2]]))

localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py

@@ -0,0 +1,20 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Threshold used for detecting invalid values for expected errors which tends to have significantly large values at the beginning of the bag file
+# which is around 1e8 [m]. This value is used to ignore the initial part of the bag file.
+THRESHOLD_FOR_INITIALIZED_ERROR = 100.0  # [m]

localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py

@@ -15,6 +15,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from constants import THRESHOLD_FOR_INITIALIZED_ERROR
 import matplotlib.pyplot as plt
 import numpy as np
 
@@ -43,8 +44,9 @@ def plot_thresholds(recall_list, lower_bound, threshold, scale, save_path=None):
 
 
 def plot_bag_compare(save_path, results):
-    # Ignore the first 20 steps (=1 sec in 20 Hz) as this part may be noisy
-    ignore_index = 50
+    # Ignore the initial part larger than this value, since the values right after launch may diverge.
+    is_smaller_than_thres = results.long_radius.expected_error < THRESHOLD_FOR_INITIALIZED_ERROR
+    ignore_index = np.where(is_smaller_than_thres)[0][0]
     error_maximum = np.max(
         np.hstack(
             [