refactor(autoware_motion_utils): refactor interpolator (autowarefound…

…ation#8931) * refactor interpolator Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update cmake Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * rename Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * Update CMakeLists.txt --------- Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
1222-takeshi · Sep 27, 2024 · f77acfe · f77acfe
1 parent 71cfaaf
commit f77acfe
Show file tree

Hide file tree

Showing 18 changed files with 218 additions and 288 deletions.
diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt
@@ -7,16 +7,11 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(Boost REQUIRED)
-find_package(fmt REQUIRED)
 
 ament_auto_add_library(autoware_motion_utils SHARED
   DIRECTORY src
 )
 
-target_link_libraries(autoware_motion_utils
-  fmt::fmt
-)
-
 if(BUILD_TESTING)
   find_package(ament_cmake_ros REQUIRED)
 
@@ -45,6 +40,7 @@ if(BUILD_EXAMPLES)
   foreach(example_file ${example_files})
     get_filename_component(example_name ${example_file} NAME_WE)
     ament_auto_add_executable(${example_name} ${example_file})
+    set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter)
     target_link_libraries(${example_name}
       autoware_motion_utils
       matplotlibcpp17::matplotlibcpp17

diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp
@@ -12,7 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
 #include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp"
 
 #include <autoware/motion_utils/trajectory_container/interpolator.hpp>
 
@@ -27,26 +31,25 @@ int main()
   auto plt = matplotlibcpp17::pyplot::import();
 
   // create random values
-  std::vector<double> axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
+  std::vector<double> bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
   std::vector<double> values;
   std::random_device seed_gen;
   std::mt19937 engine(seed_gen());
   std::uniform_real_distribution<> dist(-1.0, 1.0);
-  for (size_t i = 0; i < axis.size(); ++i) {
+  for (size_t i = 0; i < bases.size(); ++i) {
     values.push_back(dist(engine));
   }
   // Scatter Data
-  plt.scatter(Args(axis, values));
+  plt.scatter(Args(bases, values));
 
-  using autoware::motion_utils::trajectory_container::interpolator::Interpolator;
-  using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator;
+  using autoware::motion_utils::trajectory_container::interpolator::InterpolatorInterface;
   // Linear Interpolator
   {
     using autoware::motion_utils::trajectory_container::interpolator::Linear;
-    auto interpolator = *InterpolatorCreator<Linear>().set_axis(axis).set_values(values).create();
+    auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> x;
     std::vector<double> y;
-    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
       x.push_back(i);
       y.push_back(interpolator.compute(i));
     }
@@ -56,11 +59,11 @@ int main()
   // AkimaSpline Interpolator
   {
     using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline;
-    auto interpolator =
-      *InterpolatorCreator<AkimaSpline>().set_axis(axis).set_values(values).create();
+
+    auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> x;
     std::vector<double> y;
-    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
       x.push_back(i);
       y.push_back(interpolator.compute(i));
     }
@@ -70,11 +73,10 @@ int main()
   // CubicSpline Interpolator
   {
     using autoware::motion_utils::trajectory_container::interpolator::CubicSpline;
-    auto interpolator =
-      *InterpolatorCreator<CubicSpline>().set_axis(axis).set_values(values).create();
+    auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> x;
     std::vector<double> y;
-    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
       x.push_back(i);
       y.push_back(interpolator.compute(i));
     }
@@ -85,10 +87,10 @@ int main()
   {
     using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor;
     auto interpolator =
-      *InterpolatorCreator<NearestNeighbor<double>>().set_axis(axis).set_values(values).create();
+      *NearestNeighbor<double>::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> x;
     std::vector<double> y;
-    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
       x.push_back(i);
       y.push_back(interpolator.compute(i));
     }
@@ -98,11 +100,10 @@ int main()
   // Stairstep Interpolator
   {
     using autoware::motion_utils::trajectory_container::interpolator::Stairstep;
-    auto interpolator =
-      *InterpolatorCreator<Stairstep<double>>().set_axis(axis).set_values(values).create();
+    auto interpolator = *Stairstep<double>::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> x;
     std::vector<double> y;
-    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
       x.push_back(i);
       y.push_back(interpolator.compute(i));
     }

diff --git a/...autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/...autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp
@@ -16,7 +16,6 @@
 #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_
 #include <autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp>
-#include <autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/linear.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp>
 #include <autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp>

diff --git a/...on_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp b/...on_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp
@@ -15,11 +15,10 @@
 #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_
 #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_
 
-#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
 
 #include <Eigen/Dense>
 
-#include <memory>
 #include <vector>
 
 namespace autoware::motion_utils::trajectory_container::interpolator
@@ -30,7 +29,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator
  *
  * This class provides methods to perform Akima spline interpolation on a set of data points.
  */
-class AkimaSpline : public Interpolator<double>
+class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
 {
 private:
   Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the Akima spline.
@@ -40,21 +39,20 @@ class AkimaSpline : public Interpolator<double>
    *
    * This method computes the coefficients for the Akima spline.
    *
-   * @param axis The axis values.
+   * @param bases The bases values.
    * @param values The values to interpolate.
    */
   void compute_parameters(
-    const Eigen::Ref<const Eigen::VectorXd> & axis,
+    const Eigen::Ref<const Eigen::VectorXd> & bases,
     const Eigen::Ref<const Eigen::VectorXd> & values);
 
   /**
    * @brief Build the interpolator with the given values.
    *
-   * @param axis The axis values.
+   * @param bases The bases values.
    * @param values The values to interpolate.
    */
-  void build_impl(
-    const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<double> & values) override;
+  void build_impl(const std::vector<double> & bases, const std::vector<double> & values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -89,13 +87,6 @@ class AkimaSpline : public Interpolator<double>
    * @return The minimum number of required points.
    */
   [[nodiscard]] size_t minimum_required_points() const override { return 5; }
-
-  /**
-   * @brief Clone the interpolator.
-   *
-   * @return A shared pointer to a new instance of the interpolator.
-   */
-  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override;
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

diff --git a/...on_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp b/...on_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp
@@ -15,7 +15,7 @@
 #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_
 #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_
 
-#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
 
 #include <Eigen/Dense>
 
@@ -29,33 +29,32 @@ namespace autoware::motion_utils::trajectory_container::interpolator
  *
  * This class provides methods to perform cubic spline interpolation on a set of data points.
  */
-class CubicSpline : public Interpolator<double>
+class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
 {
 private:
   Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the cubic spline.
-  Eigen::VectorXd h_;              ///< Interval sizes between axis points.
+  Eigen::VectorXd h_;              ///< Interval sizes between bases points.
 
   /**
    * @brief Compute the spline parameters.
    *
    * This method computes the coefficients for the cubic spline.
    *
-   * @param axis The axis values.
+   * @param bases The bases values.
    * @param values The values to interpolate.
    */
   void compute_parameters(
-    const Eigen::Ref<const Eigen::VectorXd> & axis,
+    const Eigen::Ref<const Eigen::VectorXd> & bases,
     const Eigen::Ref<const Eigen::VectorXd> & values);
 
   /**
    * @brief Build the interpolator with the given values.
    *
-   * @param axis The axis values.
+   * @param bases The bases values.
    * @param values The values to interpolate.
    * @return True if the interpolator was built successfully, false otherwise.
    */
-  void build_impl(
-    const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<double> & values) override;
+  void build_impl(const std::vector<double> & bases, const std::vector<double> & values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -90,13 +89,6 @@ class CubicSpline : public Interpolator<double>
    * @return The minimum number of required points.
    */
   [[nodiscard]] size_t minimum_required_points() const override { return 4; }
-
-  /**
-   * @brief Clone the interpolator.
-   *
-   * @return A shared pointer to a new instance of the interpolator.
-   */
-  [[nodiscard]] std::shared_ptr<Interpolator<double>> clone() const override;
 };
 
 }  // namespace autoware::motion_utils::trajectory_container::interpolator

diff --git a/...lator/detail/interpolator_common_impl.hpp → .../detail/interpolator_common_interface.hpp b/...lator/detail/interpolator_common_impl.hpp → .../detail/interpolator_common_interface.hpp
@@ -13,15 +13,13 @@
 // limitations under the License.
 
 // clang-format off
-#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_  // NOLINT
-#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_  // NOLINT
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
 // clang-format on
 
 #include <Eigen/Dense>
 #include <rclcpp/logging.hpp>
 
-#include <fmt/format.h>
-
 #include <vector>
 
 namespace autoware::motion_utils::trajectory_container::interpolator::detail
@@ -35,20 +33,20 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail
  * @tparam T The type of the values being interpolated.
  */
 template <typename T>
-class InterpolatorCommonImpl
+class InterpolatorCommonInterface
 {
 protected:
-  Eigen::VectorXd axis_;  ///< Axis values for the interpolation.
+  std::vector<double> bases_;  ///< bases values for the interpolation.
 
   /**
    * @brief Get the start of the interpolation range.
    */
-  [[nodiscard]] double start() const { return axis_(0); }
+  [[nodiscard]] double start() const { return bases_.front(); }
 
   /**
    * @brief Get the end of the interpolation range.
    */
-  [[nodiscard]] double end() const { return axis_(axis_.size() - 1); }
+  [[nodiscard]] double end() const { return bases_.back(); }
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -65,11 +63,10 @@ class InterpolatorCommonImpl
    *
    * This method should be overridden by subclasses to provide the specific build logic.
    *
-   * @param axis The axis values.
+   * @param bases The bases values.
    * @param values The values to interpolate.
    */
-  virtual void build_impl(
-    const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<T> & values) = 0;
+  virtual void build_impl(const std::vector<double> & bases, const std::vector<T> & values) = 0;
 
   /**
    * @brief Validate the input to the compute method.
@@ -91,29 +88,30 @@ class InterpolatorCommonImpl
   [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const
   {
     if (end_inclusive && s == end()) {
-      return static_cast<int32_t>(axis_.size()) - 2;
+      return static_cast<int32_t>(bases_.size()) - 2;
     }
     auto comp = [](const double & a, const double & b) { return a <= b; };
-    return std::distance(axis_.begin(), std::lower_bound(axis_.begin(), axis_.end(), s, comp)) - 1;
+    return std::distance(bases_.begin(), std::lower_bound(bases_.begin(), bases_.end(), s, comp)) -
+           1;
   }
 
 public:
   /**
-   * @brief Build the interpolator with the given axis and values.
+   * @brief Build the interpolator with the given bases and values.
    *
-   * @param axis The axis values.
+   * @param bases The bases values.
    * @param values The values to interpolate.
    * @return True if the interpolator was built successfully, false otherwise.
    */
-  bool build(const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<T> & values)
+  bool build(const std::vector<double> & bases, const std::vector<T> & values)
   {
-    if (axis.size() != static_cast<Eigen::Index>(values.size())) {
+    if (bases.size() != values.size()) {
       return false;
     }
-    if (axis.size() < static_cast<Eigen::Index>(minimum_required_points())) {
+    if (bases.size() < minimum_required_points()) {
       return false;
     }
-    build_impl(axis, values);
+    build_impl(bases, values);
     return true;
   }
 
@@ -141,5 +139,5 @@ class InterpolatorCommonImpl
 }  // namespace autoware::motion_utils::trajectory_container::interpolator::detail
 
 // clang-format off
-#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_  // NOLINT
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
 // clang-format on