feat(autoware_motion_utils): add interpolator (#8517)

* feat(autoware_motion_utils): add interpolator

Signed-off-by: Y.Hisaki <[email protected]>

* use int32_t instead of int

Signed-off-by: Y.Hisaki <[email protected]>

* use int32_t instead of int

Signed-off-by: Y.Hisaki <[email protected]>

* use int32_t instead of int

Signed-off-by: Y.Hisaki <[email protected]>

* add const as much as possible and use `at()` in `vector`

Signed-off-by: Y.Hisaki <[email protected]>

* fix directory name

Signed-off-by: Y.Hisaki <[email protected]>

* refactor code and add example

Signed-off-by: Y.Hisaki <[email protected]>

* update

Signed-off-by: Y.Hisaki <[email protected]>

* remove unused include

Signed-off-by: Y.Hisaki <[email protected]>

* refactor code

Signed-off-by: Y.Hisaki <[email protected]>

---------

Signed-off-by: Y.Hisaki <[email protected]>

common/autoware_motion_utils/CMakeLists.txt

@@ -1,15 +1,22 @@
 cmake_minimum_required(VERSION 3.14)
 project(autoware_motion_utils)
 
+option(BUILD_EXAMPLES "Build examples" OFF)
+
 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)
 
@@ -22,4 +29,27 @@ if(BUILD_TESTING)
   )
 endif()
 
+if(BUILD_EXAMPLES)
+  message(STATUS "Building examples")
+
+  include(FetchContent)
+  fetchcontent_declare(
+    matplotlibcpp17
+    GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git
+    GIT_TAG master
+  )
+  fetchcontent_makeavailable(matplotlibcpp17)
+
+  file(GLOB_RECURSE example_files examples/*.cpp)
+
+  foreach(example_file ${example_files})
+    get_filename_component(example_name ${example_file} NAME_WE)
+    ament_auto_add_executable(${example_name} ${example_file})
+    target_link_libraries(${example_name}
+      autoware_motion_utils
+      matplotlibcpp17::matplotlibcpp17
+    )
+  endforeach()
+endif()
+
 ament_auto_package()

common/autoware_motion_utils/examples/example_interpolator.cpp

@@ -0,0 +1,115 @@
+// Copyright 2024 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 "autoware/motion_utils/trajectory_container/interpolator/linear.hpp"
+
+#include <autoware/motion_utils/trajectory_container/interpolator.hpp>
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <random>
+#include <vector>
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+  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> 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) {
+    values.push_back(dist(engine));
+  }
+  // Scatter Data
+  plt.scatter(Args(axis, values));
+
+  using autoware::motion_utils::trajectory_container::interpolator::Interpolator;
+  using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator;
+  // Linear Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::Linear;
+    auto interpolator = *InterpolatorCreator<Linear>().set_axis(axis).set_values(values).create();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "Linear"));
+  }
+
+  // AkimaSpline Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline;
+    auto interpolator =
+      *InterpolatorCreator<AkimaSpline>().set_axis(axis).set_values(values).create();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline"));
+  }
+
+  // CubicSpline Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::CubicSpline;
+    auto interpolator =
+      *InterpolatorCreator<CubicSpline>().set_axis(axis).set_values(values).create();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline"));
+  }
+
+  // NearestNeighbor Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor;
+    auto interpolator =
+      *InterpolatorCreator<NearestNeighbor<double>>().set_axis(axis).set_values(values).create();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor"));
+  }
+
+  // Stairstep Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::Stairstep;
+    auto interpolator =
+      *InterpolatorCreator<Stairstep<double>>().set_axis(axis).set_values(values).create();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = axis.front(); i < axis.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep"));
+  }
+
+  plt.legend();
+  plt.show();
+  return 0;
+}

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp

@@ -0,0 +1,23 @@
+// Copyright 2024 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_
+#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>
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp

@@ -0,0 +1,98 @@
+// Copyright 2024 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.
+
+#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 <Eigen/Dense>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for Akima spline interpolation.
+ *
+ * This class provides methods to perform Akima spline interpolation on a set of data points.
+ */
+class AkimaSpline : public Interpolator<double>
+{
+  template <typename InterpolatorType>
+  friend class InterpolatorCreator;
+
+private:
+  Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the Akima spline.
+
+  AkimaSpline() = default;
+
+  /**
+   * @brief Compute the spline parameters.
+   *
+   * This method computes the coefficients for the Akima spline.
+   *
+   * @param axis The axis values.
+   * @param values The values to interpolate.
+   */
+  void compute_parameters(
+    const Eigen::Ref<const Eigen::VectorXd> & axis,
+    const Eigen::Ref<const Eigen::VectorXd> & values);
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param axis The axis values.
+   * @param values The values to interpolate.
+   */
+  void build_impl(
+    const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<double> & values) override;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] double compute_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
+
+public:
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override { return 5; }
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_

common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp

@@ -0,0 +1,100 @@
+// Copyright 2024 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.
+
+#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 <Eigen/Dense>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for cubic spline interpolation.
+ *
+ * This class provides methods to perform cubic spline interpolation on a set of data points.
+ */
+class CubicSpline : public Interpolator<double>
+{
+  template <typename InterpolatorType>
+  friend class InterpolatorCreator;
+
+private:
+  Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the cubic spline.
+  Eigen::VectorXd h_;              ///< Interval sizes between axis points.
+
+  CubicSpline() = default;
+
+  /**
+   * @brief Compute the spline parameters.
+   *
+   * This method computes the coefficients for the cubic spline.
+   *
+   * @param axis The axis values.
+   * @param values The values to interpolate.
+   */
+  void compute_parameters(
+    const Eigen::Ref<const Eigen::VectorXd> & axis,
+    const Eigen::Ref<const Eigen::VectorXd> & values);
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param axis The axis 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;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] double compute_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
+
+public:
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override { return 4; }
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_