diff --git a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml index 2c0a0f6e..8a29da9a 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml +++ b/localization/deviation_estimation_tools/deviation_evaluator/launch/deviation_evaluator.launch.xml @@ -104,10 +104,10 @@ - + - - + + diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py index 49f0e37f..f0d82e9b 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/bag_load_utils.py +++ b/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", diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py new file mode 100644 index 00000000..523b8b8d --- /dev/null +++ b/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] diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py index df2bc499..4ef3752e 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/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( [