From c76e0b9bf9037d9950001e5c89f2084e8e3de9bf Mon Sep 17 00:00:00 2001
From: Yuxuan Liu <619684051@qq.com>
Date: Mon, 11 Dec 2023 17:03:37 +0800
Subject: [PATCH] perf(motion_utils): faster removeOverlapPoints and
 calcLateralOffset functions (awf#5385)  (#1066)

* faster removeOverlapPoints and calcLateralOffset functions

Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp>
Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../motion_utils/trajectory/trajectory.hpp    |  1 +
 .../benchmark_calcLateralOffset.cpp           | 77 +++++++++++++++++++
 2 files changed, 78 insertions(+)
 create mode 100644 common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp

diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
index db06564d299f7..edb4ec21dc6e9 100644
--- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
+++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -132,6 +132,7 @@ T removeOverlapPoints(const T & points, const size_t & start_idx = 0)
   }
 
   T dst;
+  dst.reserve(points.size());
 
   for (size_t i = 0; i <= start_idx; ++i) {
     dst.push_back(points.at(i));
diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp
new file mode 100644
index 0000000000000..549ca4c0ae5bb
--- /dev/null
+++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp
@@ -0,0 +1,77 @@
+// 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.
+
+#include "motion_utils/trajectory/trajectory.hpp"
+
+#include <gtest/gtest.h>
+#include <gtest/internal/gtest-port.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <random>
+
+namespace
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using tier4_autoware_utils::createPoint;
+using tier4_autoware_utils::createQuaternionFromRPY;
+
+constexpr double epsilon = 1e-6;
+
+geometry_msgs::msg::Pose createPose(
+  double x, double y, double z, double roll, double pitch, double yaw)
+{
+  geometry_msgs::msg::Pose p;
+  p.position = createPoint(x, y, z);
+  p.orientation = createQuaternionFromRPY(roll, pitch, yaw);
+  return p;
+}
+
+template <class T>
+T generateTestTrajectory(
+  const size_t num_points, const double point_interval, const double vel = 0.0,
+  const double init_theta = 0.0, const double delta_theta = 0.0)
+{
+  using Point = typename T::_points_type::value_type;
+
+  T traj;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double theta = init_theta + i * delta_theta;
+    const double x = i * point_interval * std::cos(theta);
+    const double y = i * point_interval * std::sin(theta);
+
+    Point p;
+    p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta);
+    p.longitudinal_velocity_mps = vel;
+    traj.points.push_back(p);
+  }
+
+  return traj;
+}
+}  // namespace
+
+TEST(trajectory_benchmark, DISABLED_calcLateralOffset)
+{
+  std::random_device r;
+  std::default_random_engine e1(r());
+  std::uniform_real_distribution<double> uniform_dist(0.0, 1000.0);
+
+  using motion_utils::calcLateralOffset;
+
+  const auto traj = generateTestTrajectory<Trajectory>(1000, 1.0, 0.0, 0.0, 0.1);
+  constexpr auto nb_iteration = 10000;
+  for (auto i = 0; i < nb_iteration; ++i) {
+    const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0);
+    calcLateralOffset(traj.points, point);
+  }
+}