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