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

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

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* update comment

Signed-off-by: kminoda <[email protected]>

* update

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* add constants

Signed-off-by: kminoda <[email protected]>

* ci(pre-commit): autofix

* update for better readability

Signed-off-by: kminoda <[email protected]>

* add copyright

Signed-off-by: kminoda <[email protected]>

---------

Signed-off-by: kminoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

@@ -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
@@ -208,17 +209,19 @@ def __init__(self, bagfile, params, use_normal_ekf=False, bagfile_base=None):
         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",

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(
             [