perf(motion_utils): faster removeOverlapPoints (#5360)

Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Oct 21, 2023 · 98c24b8 · 98c24b8
1 parent 89ae3bb
commit 98c24b8
Showing 1 changed file with 1 addition and 2 deletions.
diff --git 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));