From f77acfe909378565e9f961e91f9c54f2097095a4 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 27 Sep 2024 19:31:49 +0900 Subject: [PATCH] refactor(autoware_motion_utils): refactor interpolator (#8931) * refactor interpolator Signed-off-by: Y.Hisaki * update cmake Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * rename Signed-off-by: Y.Hisaki * Update CMakeLists.txt --------- Signed-off-by: Y.Hisaki --- common/autoware_motion_utils/CMakeLists.txt | 6 +- .../examples/example_interpolator.cpp | 37 +++---- .../trajectory_container/interpolator.hpp | 1 - .../interpolator/akima_spline.hpp | 21 ++-- .../interpolator/cubic_spline.hpp | 22 ++--- ....hpp => interpolator_common_interface.hpp} | 38 ++++---- .../detail/interpolator_mixin.hpp | 96 +++++++++++++++++++ .../detail/nearest_neighbor_common_impl.hpp | 25 +++-- .../detail/stairstep_common_impl.hpp | 23 +++-- .../interpolator/interpolator.hpp | 10 +- .../interpolator/interpolator_creator.hpp | 77 --------------- .../interpolator/linear.hpp | 17 +--- .../interpolator/nearest_neighbor.hpp | 22 ----- .../interpolator/stairstep.hpp | 22 ----- .../interpolator/akima_spline.cpp | 25 ++--- .../interpolator/cubic_spline.cpp | 28 +++--- .../interpolator/linear.cpp | 20 ++-- .../test_interpolator.cpp | 16 ++-- 18 files changed, 218 insertions(+), 288 deletions(-) rename common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/{interpolator_common_impl.hpp => interpolator_common_interface.hpp} (77%) create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index e7f1f56a451f4..8cae2a8ac4110 100644 --- 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 index 8c98143af7e28..6ce81e4572276 100644 --- 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 @@ -27,26 +31,25 @@ int main() auto plt = matplotlibcpp17::pyplot::import(); // create random values - std::vector axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; + std::vector bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; std::vector 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().set_axis(axis).set_values(values).create(); + auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector 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().set_axis(axis).set_values(values).create(); + + auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector 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().set_axis(axis).set_values(values).create(); + auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector 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>().set_axis(axis).set_values(values).create(); + *NearestNeighbor::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector 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>().set_axis(axis).set_values(values).create(); + auto interpolator = *Stairstep::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector 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/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp index a6542d1fc212f..a6a0c69744301 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ #include #include -#include #include #include #include diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp index 363d046534399..f9a2ac1316a4e 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp +++ b/common/autoware_motion_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 -#include #include 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 +class AkimaSpline : public detail::InterpolatorMixin { private: Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. @@ -40,21 +39,20 @@ class AkimaSpline : public Interpolator * * 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 & axis, + const Eigen::Ref & bases, const Eigen::Ref & 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 & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -89,13 +87,6 @@ class AkimaSpline : public Interpolator * @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> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp index 50cff1dde3f35..437ddd727cc7d 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp +++ b/common/autoware_motion_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 @@ -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 +class CubicSpline : public detail::InterpolatorMixin { 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 & axis, + const Eigen::Ref & bases, const Eigen::Ref & 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 & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -90,13 +89,6 @@ class CubicSpline : public Interpolator * @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> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp similarity index 77% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp rename to common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp index f7de456b736a4..f0131e85e157c 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/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 #include -#include - #include 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 -class InterpolatorCommonImpl +class InterpolatorCommonInterface { protected: - Eigen::VectorXd axis_; ///< Axis values for the interpolation. + std::vector 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 & axis, const std::vector & values) = 0; + virtual void build_impl(const std::vector & bases, const std::vector & 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(axis_.size()) - 2; + return static_cast(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 & axis, const std::vector & values) + bool build(const std::vector & bases, const std::vector & values) { - if (axis.size() != static_cast(values.size())) { + if (bases.size() != values.size()) { return false; } - if (axis.size() < static_cast(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 diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp new file mode 100644 index 0000000000000..d3957567c49b5 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp @@ -0,0 +1,96 @@ +// 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ + +/** + * @brief Base class for interpolator implementations. + * + * This class implements the core functionality for interpolator implementations. + * + * @tparam InterpolatorType The type of the interpolator implementation. + * @tparam T The type of the values being interpolated. + */ +template +struct InterpolatorMixin : public InterpolatorInterface +{ + std::shared_ptr> clone() const override + { + return std::make_shared(static_cast(*this)); + } + + class Builder + { + private: + std::vector bases_; + std::vector values_; + + public: + [[nodiscard]] Builder & set_bases(const Eigen::Ref & bases) + { + bases_ = std::vector(bases.begin(), bases.end()); + return *this; + } + + [[nodiscard]] Builder & set_bases(const std::vector & bases) + { + bases_ = bases; + return *this; + } + + [[nodiscard]] Builder & set_values(const std::vector & values) + { + values_ = values; + return *this; + } + + [[nodiscard]] Builder & set_values(const Eigen::Ref & values) + { + values_ = std::vector(values.begin(), values.end()); + return *this; + } + + template + [[nodiscard]] std::optional build(Args &&... args) + { + auto interpolator = InterpolatorType(std::forward(args)...); + bool success = interpolator.build(bases_, values_); + if (!success) { + return std::nullopt; + } + return interpolator; + } + }; +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp index 9fd451bcdcf6e..ad0e2b98ef040 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -17,11 +17,17 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +template +class NearestNeighbor; + +namespace detail { /** * @brief Common Implementation of nearest neighbor. @@ -31,7 +37,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class NearestNeighborCommonImpl : public Interpolator +class NearestNeighborCommonImpl : public detail::InterpolatorMixin, T> { protected: std::vector values_; ///< Interpolation values. @@ -45,7 +51,7 @@ class NearestNeighborCommonImpl : public Interpolator [[nodiscard]] T compute_impl(const double & s) const override { const int32_t idx = this->get_index(s); - return (std::abs(s - this->axis_[idx]) <= std::abs(s - this->axis_[idx + 1])) + return (std::abs(s - this->bases_[idx]) <= std::abs(s - this->bases_[idx + 1])) ? this->values_.at(idx) : this->values_.at(idx + 1); } @@ -53,14 +59,13 @@ class NearestNeighborCommonImpl : public Interpolator /** * @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 & axis, const std::vector & values) override + void build_impl(const std::vector & bases, const std::vector & values) override { - this->axis_ = axis; + this->bases_ = bases; this->values_ = values; } @@ -73,8 +78,8 @@ class NearestNeighborCommonImpl : public Interpolator [[nodiscard]] size_t minimum_required_points() const override { return 1; } }; -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail - +} // namespace detail +} // namespace autoware::motion_utils::trajectory_container::interpolator // clang-format off #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp index 9ac5282429353..83b2275b0c121 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp @@ -17,11 +17,17 @@ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +template +class Stairstep; + +namespace detail { /** @@ -32,7 +38,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator::detail * @tparam T The type of the values being interpolated. */ template -class StairstepCommonImpl : public Interpolator +class StairstepCommonImpl : public detail::InterpolatorMixin, T> { protected: std::vector values_; ///< Interpolation values. @@ -51,13 +57,12 @@ class StairstepCommonImpl : public Interpolator /** * @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 & axis, const std::vector & values) override + void build_impl(const std::vector & bases, const std::vector & values) override { - this->axis_ = axis; + this->bases_ = bases; this->values_ = values; } @@ -72,8 +77,8 @@ class StairstepCommonImpl : public Interpolator */ [[nodiscard]] size_t minimum_required_points() const override { return 2; } }; - -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail +} // namespace detail +} // namespace autoware::motion_utils::trajectory_container::interpolator // clang-format off #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp index f0b12a47819e8..03827ded59cb6 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp" #include @@ -29,10 +29,10 @@ namespace autoware::motion_utils::trajectory_container::interpolator * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) */ template -class Interpolator : public detail::InterpolatorCommonImpl +class InterpolatorInterface : public detail::InterpolatorCommonInterface { public: - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; + [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; /** @@ -41,7 +41,7 @@ class Interpolator : public detail::InterpolatorCommonImpl * This class adds methods for computing first and second derivatives. */ template <> -class Interpolator : public detail::InterpolatorCommonImpl +class InterpolatorInterface : public detail::InterpolatorCommonInterface { protected: /** @@ -89,7 +89,7 @@ class Interpolator : public detail::InterpolatorCommonImpl return compute_second_derivative_impl(s); } - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; + [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp deleted file mode 100644 index 911adcb6545b6..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// 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__INTERPOLATOR_CREATOR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ - -#include - -#include -#include -#include - -namespace autoware::motion_utils::trajectory_container::interpolator -{ - -// Forward declaration -template -class Interpolator; - -template -class InterpolatorCreator -{ -private: - Eigen::VectorXd axis_; - std::vector values_; - -public: - [[nodiscard]] InterpolatorCreator & set_axis(const Eigen::Ref & axis) - { - axis_ = axis; - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_axis(const std::vector & axis) - { - axis_ = Eigen::Map(axis.data(), static_cast(axis.size())); - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_values(const std::vector & values) - { - values_ = values; - return *this; - } - - [[nodiscard]] InterpolatorCreator & set_values(const Eigen::Ref & values) - { - values_ = std::vector(values.begin(), values.end()); - return *this; - } - - template - [[nodiscard]] std::optional create(Args &&... args) - { - auto interpolator = InterpolatorType(std::forward(args)...); - bool success = interpolator.build(axis_, values_); - if (!success) { - return std::nullopt; - } - return interpolator; - } -}; - -} // namespace autoware::motion_utils::trajectory_container::interpolator - -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp index 9854b100f3742..99fcb469365b8 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator @@ -30,7 +29,7 @@ namespace autoware::motion_utils::trajectory_container::interpolator * * This class provides methods to perform linear interpolation on a set of data points. */ -class Linear : public Interpolator +class Linear : public detail::InterpolatorMixin { private: Eigen::VectorXd values_; ///< Interpolation values. @@ -38,12 +37,11 @@ class Linear : public Interpolator /** * @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 & axis, const std::vector & values) override; + void build_impl(const std::vector & bases, const std::vector & values) override; /** * @brief Compute the interpolated value at the given point. @@ -81,13 +79,6 @@ class Linear : public Interpolator * @return The minimum number of required points. */ [[nodiscard]] size_t minimum_required_points() const override; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to the cloned interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override; }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp index 32bff2d30b41d..72b61001b05e3 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp @@ -17,8 +17,6 @@ #include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" -#include - namespace autoware::motion_utils::trajectory_container::interpolator { @@ -44,16 +42,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl { public: NearestNeighbor() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; /** @@ -83,16 +71,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl public: NearestNeighbor() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp index 21bf57b46e3b7..801b207b25afc 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp @@ -17,8 +17,6 @@ #include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" -#include - namespace autoware::motion_utils::trajectory_container::interpolator { @@ -44,16 +42,6 @@ class Stairstep : public detail::StairstepCommonImpl { public: Stairstep() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; /** @@ -83,16 +71,6 @@ class Stairstep : public detail::StairstepCommonImpl public: Stairstep() = default; - - /** - * @brief Clone the interpolator. - * - * @return A shared pointer to a new instance of the interpolator. - */ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared>(*this); - } }; } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp index eb2fe2ab8deed..747362ac9167e 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp @@ -17,18 +17,17 @@ #include #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator { void AkimaSpline::compute_parameters( - const Eigen::Ref & axis, const Eigen::Ref & values) + const Eigen::Ref & bases, const Eigen::Ref & values) { - const auto n = static_cast(axis.size()); + const auto n = static_cast(bases.size()); - Eigen::VectorXd h = axis.tail(n - 1) - axis.head(n - 1); + Eigen::VectorXd h = bases.tail(n - 1) - bases.head(n - 1); Eigen::VectorXd m(n - 1); for (int32_t i = 0; i < n - 1; ++i) { @@ -61,39 +60,33 @@ void AkimaSpline::compute_parameters( } } -void AkimaSpline::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void AkimaSpline::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; compute_parameters( - this->axis_, + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); } double AkimaSpline::compute_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return a_[i] + b_[i] * dx + c_[i] * dx * dx + d_[i] * dx * dx * dx; } double AkimaSpline::compute_first_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return b_[i] + 2 * c_[i] * dx + 3 * d_[i] * dx * dx; } double AkimaSpline::compute_second_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_[i]; + const double dx = s - this->bases_[i]; return 2 * c_[i] + 6 * d_[i] * dx; } -std::shared_ptr> AkimaSpline::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp index db25816d1d457..e464fc7cd0511 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp @@ -20,15 +20,15 @@ namespace autoware::motion_utils::trajectory_container::interpolator { void CubicSpline::compute_parameters( - const Eigen::Ref & axis, const Eigen::Ref & values) + const Eigen::Ref & bases, const Eigen::Ref & values) { - const int32_t n = static_cast(axis.size()) - 1; + const int32_t n = static_cast(bases.size()) - 1; - h_ = axis.tail(n) - axis.head(n); + h_ = bases.tail(n) - bases.head(n); a_ = values.transpose(); for (int32_t i = 0; i < n; ++i) { - h_(i) = axis(i + 1) - axis(i); + h_(i) = bases(i + 1) - bases(i); } Eigen::VectorXd alpha(n - 1); @@ -43,7 +43,7 @@ void CubicSpline::compute_parameters( mu(0) = z(0) = 0.0; for (int32_t i = 1; i < n; ++i) { - l(i) = 2.0 * (axis(i + 1) - axis(i - 1)) - h_(i - 1) * mu(i - 1); + l(i) = 2.0 * (bases(i + 1) - bases(i - 1)) - h_(i - 1) * mu(i - 1); mu(i) = h_(i) / l(i); z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); } @@ -61,39 +61,33 @@ void CubicSpline::compute_parameters( } } -void CubicSpline::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void CubicSpline::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; compute_parameters( - this->axis_, + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); } double CubicSpline::compute_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return a_(i) + b_(i) * dx + c_(i) * dx * dx + d_(i) * dx * dx * dx; } double CubicSpline::compute_first_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return b_(i) + 2 * c_(i) * dx + 3 * d_(i) * dx * dx; } double CubicSpline::compute_second_derivative_impl(const double & s) const { const int32_t i = this->get_index(s); - const double dx = s - this->axis_(i); + const double dx = s - this->bases_.at(i); return 2 * c_(i) + 6 * d_(i) * dx; } -std::shared_ptr> CubicSpline::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp index c3e4ec99c4357..fb714b208772c 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp @@ -16,16 +16,14 @@ #include -#include #include namespace autoware::motion_utils::trajectory_container::interpolator { -void Linear::build_impl( - const Eigen::Ref & axis, const std::vector & values) +void Linear::build_impl(const std::vector & bases, const std::vector & values) { - this->axis_ = axis; + this->bases_ = bases; this->values_ = Eigen::Map(values.data(), static_cast(values.size())); } @@ -33,8 +31,8 @@ void Linear::build_impl( double Linear::compute_impl(const double & s) const { const int32_t idx = this->get_index(s); - const double x0 = this->axis_(idx); - const double x1 = this->axis_(idx + 1); + const double x0 = this->bases_.at(idx); + const double x1 = this->bases_.at(idx + 1); const double y0 = this->values_(idx); const double y1 = this->values_(idx + 1); return y0 + (y1 - y0) * (s - x0) / (x1 - x0); @@ -43,8 +41,8 @@ double Linear::compute_impl(const double & s) const double Linear::compute_first_derivative_impl(const double & s) const { const int32_t idx = this->get_index(s); - const double x0 = this->axis_(idx); - const double x1 = this->axis_(idx + 1); + const double x0 = this->bases_.at(idx); + const double x1 = this->bases_.at(idx + 1); const double y0 = this->values_(idx); const double y1 = this->values_(idx + 1); return (y1 - y0) / (x1 - x0); @@ -59,10 +57,4 @@ size_t Linear::minimum_required_points() const { return 2; } - -std::shared_ptr> Linear::clone() const -{ - return std::make_shared(*this); -} - } // namespace autoware::motion_utils::trajectory_container::interpolator diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp index 77f7af0eae93f..82c387c416436 100644 --- a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include @@ -26,7 +25,7 @@ class TestInterpolator : public ::testing::Test { public: std::optional interpolator; - std::vector axis; + std::vector bases; std::vector values; void SetUp() override @@ -35,10 +34,10 @@ class TestInterpolator : public ::testing::Test std::random_device rd; std::mt19937 gen(rd()); std::uniform_real_distribution<> dis(-1, 1); - axis.resize(10); + bases.resize(10); values.resize(10); - for (size_t i = 0; i < axis.size(); ++i) { - axis[i] = static_cast(i); + for (size_t i = 0; i < bases.size(); ++i) { + bases[i] = static_cast(i); values[i] = dis(gen); } } @@ -55,11 +54,10 @@ TYPED_TEST_SUITE(TestInterpolator, Interpolators, ); TYPED_TEST(TestInterpolator, compute) { - using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; this->interpolator = - InterpolatorCreator().set_axis(this->axis).set_values(this->values).create(); - for (size_t i = 0; i < this->axis.size(); ++i) { - EXPECT_NEAR(this->values[i], this->interpolator->compute(this->axis[i]), 1e-6); + typename TypeParam::Builder().set_bases(this->bases).set_values(this->values).build(); + for (size_t i = 0; i < this->bases.size(); ++i) { + EXPECT_NEAR(this->values[i], this->interpolator->compute(this->bases[i]), 1e-6); } }