From 0ef8a10f717f273a1f16a6affb2bd9b77f4e5f64 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 14 Nov 2023 16:10:11 +0900 Subject: [PATCH 1/9] fix(deviation_evaluator): fix bug in map launch and plot Signed-off-by: kminoda --- .../launch/deviation_evaluator.launch.xml | 6 +++--- .../deviation_evaluator/scripts/plot_utils.py | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) 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/plot_utils.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py index df2bc499..6ac7b708 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -44,7 +44,8 @@ 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 + THRESHOLD_FOR_INITIALIZED_ERROR = 1.0 + ignore_index = np.where(results.long_radius.expected_error < THRESHOLD_FOR_INITIALIZED_ERROR)[0][0] error_maximum = np.max( np.hstack( [ From 9395e094559973d95c8895d9c757ef25a1fce252 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Nov 2023 07:12:08 +0000 Subject: [PATCH 2/9] ci(pre-commit): autofix --- .../deviation_evaluator/scripts/plot_utils.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 6ac7b708..82a1c46f 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -45,7 +45,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 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( [ From d9bc3ef31e1f0b9571523af4533ab57717f3e055 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 14 Nov 2023 16:13:12 +0900 Subject: [PATCH 3/9] update comment Signed-off-by: kminoda --- .../deviation_evaluator/scripts/plot_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 82a1c46f..329ba1f2 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -43,7 +43,7 @@ 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 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 From 0e90285ac10471f4a86221374e6b8bd27a04d571 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 14 Nov 2023 16:13:59 +0900 Subject: [PATCH 4/9] update Signed-off-by: kminoda --- .../deviation_evaluator/scripts/plot_utils.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 329ba1f2..8396c47d 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -45,9 +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( [ From 93b04aee1d4bdd6f5d923a83e4f89f165a271471 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Nov 2023 07:15:16 +0000 Subject: [PATCH 5/9] ci(pre-commit): autofix --- .../deviation_evaluator/scripts/plot_utils.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 8396c47d..329ba1f2 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -45,8 +45,9 @@ 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( [ From ae7abb39f1896e036f92ae0b35429d81fab9093a Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 14 Nov 2023 16:23:56 +0900 Subject: [PATCH 6/9] add constants Signed-off-by: kminoda --- .../deviation_evaluator/scripts/bag_load_utils.py | 8 ++++++-- .../deviation_evaluator/scripts/constants.py | 3 +++ .../deviation_evaluator/scripts/plot_utils.py | 7 +++---- 3 files changed, 12 insertions(+), 6 deletions(-) create mode 100644 localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py 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..5853c8f7 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 @@ -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", 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..dc846d65 --- /dev/null +++ b/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] 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 329ba1f2..547a00d5 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/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( [ From d1a3552e7f5e83f4ba84a164ba32c650ef076f1d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Nov 2023 07:24:50 +0000 Subject: [PATCH 7/9] ci(pre-commit): autofix --- .../deviation_evaluator/scripts/bag_load_utils.py | 5 ++--- .../deviation_evaluator/scripts/constants.py | 2 +- .../deviation_evaluator/scripts/plot_utils.py | 7 ++++--- 3 files changed, 7 insertions(+), 7 deletions(-) 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 5853c8f7..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 @@ -29,7 +30,6 @@ from scipy.spatial.transform import Rotation from tqdm import tqdm -from constants import THRESHOLD_FOR_INITIALIZED_ERROR @dataclasses.dataclass class ErrorResults: @@ -209,8 +209,7 @@ 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] + ignore_index = np.where(stddev_long_2d_gt < THRESHOLD_FOR_INITIALIZED_ERROR)[0][0] long_radius_results = ErrorResults( "long_radius", diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py index dc846d65..844b521a 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py @@ -1,3 +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] +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 547a00d5..5713ce94 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -15,10 +15,10 @@ # 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 -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)) @@ -45,8 +45,9 @@ 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. - 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( [ From 055f2428fdcd8c837bb3fc245590c8fc150ed1b9 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 14 Nov 2023 16:26:22 +0900 Subject: [PATCH 8/9] update for better readability Signed-off-by: kminoda --- .../deviation_evaluator/scripts/plot_utils.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 5713ce94..4ef3752e 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/plot_utils.py @@ -45,9 +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. - ignore_index = np.where(results.long_radius.expected_error < THRESHOLD_FOR_INITIALIZED_ERROR)[ - 0 - ][0] + 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( [ From 7b9f40c18c554b231a4f8fbc09460d6c83eaac55 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 15 Nov 2023 09:06:21 +0900 Subject: [PATCH 9/9] add copyright Signed-off-by: kminoda --- .../deviation_evaluator/scripts/constants.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py index 844b521a..523b8b8d 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py +++ b/localization/deviation_estimation_tools/deviation_evaluator/scripts/constants.py @@ -1,3 +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]