From fd67083d67206d0ea2e32fc8f1f2031a7b4017a8 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 14 Feb 2023 20:03:36 +0900 Subject: [PATCH] perf(map_based_prediction): performance improvement of interpolateReferencePath with cumulative sum (#2883) * perf(map_based_prediction): performance improvement of interpolateReferencePath with cumulative sum Signed-off-by: veqcc * style(pre-commit): autofix --------- Signed-off-by: veqcc Co-authored-by: veqcc Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/path_generator.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index d4306eee4431d..9b94ff1419f15 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -287,15 +287,18 @@ PosePath PathGenerator::interpolateReferencePath( return interpolated_path; } - std::vector base_path_x; - std::vector base_path_y; - std::vector base_path_z; - std::vector base_path_s; + std::vector base_path_x(base_path.size()); + std::vector base_path_y(base_path.size()); + std::vector base_path_z(base_path.size()); + std::vector base_path_s(base_path.size(), 0.0); for (size_t i = 0; i < base_path.size(); ++i) { - base_path_x.push_back(base_path.at(i).position.x); - base_path_y.push_back(base_path.at(i).position.y); - base_path_z.push_back(base_path.at(i).position.z); - base_path_s.push_back(motion_utils::calcSignedArcLength(base_path, 0, i)); + base_path_x.at(i) = base_path.at(i).position.x; + base_path_y.at(i) = base_path.at(i).position.y; + base_path_z.at(i) = base_path.at(i).position.z; + if (i > 0) { + base_path_s.at(i) = base_path_s.at(i - 1) + tier4_autoware_utils::calcDistance2d( + base_path.at(i - 1), base_path.at(i)); + } } std::vector resampled_s(frenet_predicted_path.size());