From 98c24b824c68605c28513de47a36aaf5a4a89817 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 21 Oct 2023 11:00:34 +0900 Subject: [PATCH] perf(motion_utils): faster removeOverlapPoints (#5360) Signed-off-by: Takayuki Murooka --- .../include/motion_utils/trajectory/trajectory.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index dcdefe61e4000..5d773c5be32d9 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -177,8 +177,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) for (size_t i = start_idx + 1; i < points.size(); ++i) { const auto prev_p = tier4_autoware_utils::getPoint(dst.back()); const auto curr_p = tier4_autoware_utils::getPoint(points.at(i)); - const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p); - if (dist < eps) { + if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } dst.push_back(points.at(i));