add constants

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

localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py

@@ -29,6 +29,7 @@
 from scipy.spatial.transform import Rotation
 from tqdm import tqdm
 
+from constants import THRESHOLD_FOR_INITIALIZED_ERROR
 
 @dataclasses.dataclass
 class ErrorResults:
@@ -208,17 +209,20 @@ 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,3 @@
+# 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

@@ -18,6 +18,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 
+from constants import THRESHOLD_FOR_INITIALIZED_ERROR
 
 def plot_thresholds(recall_list, lower_bound, threshold, scale, save_path=None):
     fig = plt.figure(figsize=(8, 6))
@@ -44,10 +45,8 @@ def plot_thresholds(recall_list, lower_bound, threshold, scale, save_path=None):
 
 def plot_bag_compare(save_path, results):
     # Ignore the initial part larger than this value, since the values right after launch may diverge.
-    THRESHOLD_FOR_INITIALIZED_ERROR = 1.0
-    ignore_index = np.where(results.long_radius.expected_error < THRESHOLD_FOR_INITIALIZED_ERROR)[
-        0
-    ][0]
+    ignore_index = np.where(
+        results.long_radius.expected_error < THRESHOLD_FOR_INITIALIZED_ERROR)[0][0]
     error_maximum = np.max(
         np.hstack(
             [