From b9b0fa9dbb4e141519dc8de13d37827763b481f2 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 23 Aug 2024 17:51:51 +0900 Subject: [PATCH] feat(autoware_motion_utils): add interpolator (#8517) * feat(autoware_motion_utils): add interpolator Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * add const as much as possible and use `at()` in `vector` Signed-off-by: Y.Hisaki * fix directory name Signed-off-by: Y.Hisaki * refactor code and add example Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * remove unused include Signed-off-by: Y.Hisaki * refactor code Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- common/autoware_motion_utils/CMakeLists.txt | 30 ++++ .../examples/example_interpolator.cpp | 115 ++++++++++++++ .../trajectory_container/interpolator.hpp | 23 +++ .../interpolator/akima_spline.hpp | 98 ++++++++++++ .../interpolator/cubic_spline.hpp | 100 ++++++++++++ .../detail/interpolator_common_impl.hpp | 145 ++++++++++++++++++ .../detail/nearest_neighbor_common_impl.hpp | 80 ++++++++++ .../detail/stairstep_common_impl.hpp | 80 ++++++++++ .../interpolator/interpolator.hpp | 91 +++++++++++ .../interpolator/interpolator_creator.hpp | 77 ++++++++++ .../interpolator/linear.hpp | 90 +++++++++++ .../interpolator/nearest_neighbor.hpp | 83 ++++++++++ .../interpolator/stairstep.hpp | 82 ++++++++++ .../interpolator/akima_spline.cpp | 93 +++++++++++ .../interpolator/cubic_spline.cpp | 94 ++++++++++++ .../interpolator/linear.cpp | 62 ++++++++ .../test_interpolator.cpp | 75 +++++++++ 17 files changed, 1418 insertions(+) create mode 100644 common/autoware_motion_utils/examples/example_interpolator.cpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp create mode 100644 common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index 4c36ef2f4e70d..e7f1f56a451f4 100644 --- a/common/autoware_motion_utils/CMakeLists.txt +++ b/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() diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp new file mode 100644 index 0000000000000..8c98143af7e28 --- /dev/null +++ b/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 + +#include + +#include +#include + +int main() +{ + pybind11::scoped_interpreter guard{}; + 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 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().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector 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().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector 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().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector 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>().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector 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>().set_axis(axis).set_values(values).create(); + std::vector x; + std::vector 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; +} 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 new file mode 100644 index 0000000000000..a6542d1fc212f --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ 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 new file mode 100644 index 0000000000000..42cd1aa6759a3 --- /dev/null +++ b/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 + +#include + +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 +{ + template + 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 & axis, + const Eigen::Ref & 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 & axis, const std::vector & 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_ 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 new file mode 100644 index 0000000000000..43f1102fd0f73 --- /dev/null +++ b/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 + +#include + +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 +{ + template + 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 & axis, + const Eigen::Ref & 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 & axis, const std::vector & 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_ 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_impl.hpp new file mode 100644 index 0000000000000..f7de456b736a4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp @@ -0,0 +1,145 @@ +// 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_COMMON_IMPL_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_HPP_ // NOLINT +// clang-format on + +#include +#include + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ +/** + * @brief Base class for interpolation implementations. + * + * This class provides the basic interface and common functionality for different types + * of interpolation. It is intended to be subclassed by specific interpolation algorithms. + * + * @tparam T The type of the values being interpolated. + */ +template +class InterpolatorCommonImpl +{ +protected: + Eigen::VectorXd axis_; ///< Axis values for the interpolation. + + /** + * @brief Get the start of the interpolation range. + */ + [[nodiscard]] double start() const { return axis_(0); } + + /** + * @brief Get the end of the interpolation range. + */ + [[nodiscard]] double end() const { return axis_(axis_.size() - 1); } + + /** + * @brief Compute the interpolated value at the given point. + * + * This method should be overridden by subclasses to provide the specific interpolation logic. + * + * @param s The point at which to compute the interpolated value. + * @return The interpolated value. + */ + [[nodiscard]] virtual T compute_impl(const double & s) const = 0; + + /** + * @brief Build the interpolator with the given values. + * + * This method should be overridden by subclasses to provide the specific build logic. + * + * @param axis The axis values. + * @param values The values to interpolate. + */ + virtual void build_impl( + const Eigen::Ref & axis, const std::vector & values) = 0; + + /** + * @brief Validate the input to the compute method. + * + * Checks that the interpolator has been built and that the input value is within range. + * + * @param s The input value. + * @throw std::runtime_error if the interpolator has not been built. + */ + void validate_compute_input(const double & s) const + { + if (s < start() || s > end()) { + RCLCPP_WARN( + rclcpp::get_logger("Interpolator"), + "Input value %f is outside the range of the interpolator [%f, %f].", s, start(), end()); + } + } + + [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const + { + if (end_inclusive && s == end()) { + return static_cast(axis_.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; + } + +public: + /** + * @brief Build the interpolator with the given axis and values. + * + * @param axis The axis 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) + { + if (axis.size() != static_cast(values.size())) { + return false; + } + if (axis.size() < static_cast(minimum_required_points())) { + return false; + } + build_impl(axis, values); + return true; + } + + /** + * @brief Get the minimum number of required points for the interpolator. + * + * This method should be overridden by subclasses to return the specific requirement. + * + * @return The minimum number of required points. + */ + [[nodiscard]] virtual size_t minimum_required_points() const = 0; + + /** + * @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]] T compute(const double & s) const + { + validate_compute_input(s); + return compute_impl(s); + } +}; +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_IMPL_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 new file mode 100644 index 0000000000000..9fd451bcdcf6e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -0,0 +1,80 @@ +// 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__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +#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 + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ +/** + * @brief Common Implementation of nearest neighbor. + * + * This class implements the core functionality for nearest neighbor interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighborCommonImpl : public Interpolator +{ +protected: + std::vector values_; ///< Interpolation values. + + /** + * @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]] 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])) + ? this->values_.at(idx) + : this->values_.at(idx + 1); + } + + /** + * @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 & axis, const std::vector & values) override + { + this->axis_ = axis; + this->values_ = values; + } + +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 1; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// 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 new file mode 100644 index 0000000000000..9ac5282429353 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp @@ -0,0 +1,80 @@ +// 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__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +#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 + +namespace autoware::motion_utils::trajectory_container::interpolator::detail +{ + +/** + * @brief Base class for stairstep interpolation. + * + * This class implements the core functionality for stairstep interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class StairstepCommonImpl : public Interpolator +{ +protected: + std::vector values_; ///< Interpolation values. + + /** + * @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]] T compute_impl(const double & s) const override + { + const int32_t idx = this->get_index(s, false); + return this->values_.at(idx); + } + /** + * @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 & axis, const std::vector & values) override + { + this->axis_ = axis; + this->values_ = values; + } + +public: + /** + * @brief Default constructor. + */ + StairstepCommonImpl() = default; + + /** + * @brief Get the minimum number of required points for the interpolator. + */ + [[nodiscard]] size_t minimum_required_points() const override { return 2; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +// clang-format on 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 new file mode 100644 index 0000000000000..18dce8b46c2c7 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp @@ -0,0 +1,91 @@ +// 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_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ +/** + * @brief Template class for interpolation. + * + * This class serves as the base class for specific interpolation types. + * + * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) + */ +template +class Interpolator : public detail::InterpolatorCommonImpl +{ +}; + +/** + * @brief Specialization of Interpolator for double values. + * + * This class adds methods for computing first and second derivatives. + */ +template <> +class Interpolator : public detail::InterpolatorCommonImpl +{ +protected: + /** + * @brief Compute the first derivative at the given point. + * + * This method should be overridden by subclasses to provide the specific logic. + * + * @param s The point at which to compute the first derivative. + * @return The first derivative. + */ + [[nodiscard]] virtual double compute_first_derivative_impl(const double & s) const = 0; + + /** + * @brief Compute the second derivative at the given point. + * + * This method should be overridden by subclasses to provide the specific logic. + * + * @param s The point at which to compute the second derivative. + * @return The second derivative. + */ + [[nodiscard]] virtual double compute_second_derivative_impl(const double & s) const = 0; + +public: + /** + * @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(const double & s) const + { + this->validate_compute_input(s); + return compute_first_derivative_impl(s); + } + + /** + * @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(const double & s) const + { + this->validate_compute_input(s); + return compute_second_derivative_impl(s); + } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ 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 new file mode 100644 index 0000000000000..911adcb6545b6 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp @@ -0,0 +1,77 @@ +// 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 new file mode 100644 index 0000000000000..df0806f285339 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp @@ -0,0 +1,90 @@ +// 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__LINEAR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Class for linear interpolation. + * + * This class provides methods to perform linear interpolation on a set of data points. + */ +class Linear : public Interpolator +{ + template + friend class InterpolatorCreator; + +private: + Eigen::VectorXd values_; ///< Interpolation values. + + /** + * @brief Default constructor. + */ + Linear() = default; + + /** + * @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 & axis, const std::vector & 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 &) 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; +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ 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 new file mode 100644 index 0000000000000..c433607902b38 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp @@ -0,0 +1,83 @@ +// 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__NEAREST_NEIGHBOR_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Template class for nearest neighbor interpolation. + * + * This class provides methods to perform nearest neighbor interpolation on a set of data points. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighbor; + +/** + * @brief Template class for nearest neighbor interpolation. + * + * This class provides the interface for nearest neighbor interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class NearestNeighbor : public detail::NearestNeighborCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + NearestNeighbor() = default; +}; + +/** + * @brief Specialization of NearestNeighbor for double values. + * + * This class provides methods to perform nearest neighbor interpolation on double values. + */ +template <> +class NearestNeighbor : public detail::NearestNeighborCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + NearestNeighbor() = default; + + /** + * @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 &) const override { return 0.0; } + + /** + * @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 &) const override { return 0.0; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ 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 new file mode 100644 index 0000000000000..e8c8f8c015b63 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp @@ -0,0 +1,82 @@ +// 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__STAIRSTEP_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +/** + * @brief Template class for stairstep interpolation. + * + * This class provides methods to perform stairstep interpolation on a set of data points. + * + * @tparam T The type of the values being interpolated. + */ +template +class Stairstep; + +/** + * @brief Template class for stairstep interpolation. + * + * This class provides the interface for stairstep interpolation. + * + * @tparam T The type of the values being interpolated. + */ +template +class Stairstep : public detail::StairstepCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + Stairstep() = default; +}; + +/** + * @brief Specialization of Stairstep for double values. + * + * This class provides methods to perform stairstep interpolation on double values. + */ +template <> +class Stairstep : public detail::StairstepCommonImpl +{ + template + friend class InterpolatorCreator; + +private: + Stairstep() = default; + /** + * @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 &) const override { return 0.0; } + + /** + * @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 &) const override { return 0.0; } +}; + +} // namespace autoware::motion_utils::trajectory_container::interpolator + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ 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 new file mode 100644 index 0000000000000..283f46dd5d3e9 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp @@ -0,0 +1,93 @@ +// 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/akima_spline.hpp" + +#include + +#include +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void AkimaSpline::compute_parameters( + const Eigen::Ref & axis, const Eigen::Ref & values) +{ + const auto n = static_cast(axis.size()); + + Eigen::VectorXd h = axis.tail(n - 1) - axis.head(n - 1); + + Eigen::VectorXd m(n - 1); + for (int32_t i = 0; i < n - 1; ++i) { + m[i] = (values[i + 1] - values[i]) / h[i]; + } + + Eigen::VectorXd s(n); + s[0] = m[0]; + s[1] = (m[0] + m[1]) / 2; + for (int32_t i = 2; i < n - 2; ++i) { + if ((std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])) == 0) { + s[i] = (m[i] + m[i - 1]) / 2; + } else { + s[i] = (std::abs(m[i + 1] - m[i]) * m[i - 1] + std::abs(m[i - 1] - m[i - 2]) * m[i]) / + (std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])); + } + } + s[n - 2] = (m[n - 2] + m[n - 3]) / 2; + s[n - 1] = m[n - 2]; + + a_.resize(n - 1); + b_.resize(n - 1); + c_.resize(n - 1); + d_.resize(n - 1); + for (int32_t i = 0; i < n - 1; ++i) { + a_[i] = values[i]; + b_[i] = s[i]; + c_[i] = (3 * m[i] - 2 * s[i] - s[i + 1]) / h[i]; + d_[i] = (s[i] + s[i + 1] - 2 * m[i]) / (h[i] * h[i]); + } +} + +void AkimaSpline::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + compute_parameters( + this->axis_, + 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]; + 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]; + 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]; + return 2 * c_[i] + 6 * d_[i] * dx; +} + +} // 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 new file mode 100644 index 0000000000000..9f262b6a702f9 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp @@ -0,0 +1,94 @@ +// 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/cubic_spline.hpp" + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void CubicSpline::compute_parameters( + const Eigen::Ref & axis, const Eigen::Ref & values) +{ + const int32_t n = static_cast(axis.size()) - 1; + + h_ = axis.tail(n) - axis.head(n); + a_ = values.transpose(); + + for (int32_t i = 0; i < n; ++i) { + h_(i) = axis(i + 1) - axis(i); + } + + Eigen::VectorXd alpha(n - 1); + for (int32_t i = 1; i < n; ++i) { + alpha(i - 1) = (3.0 / h_(i)) * (a_(i + 1) - a_(i)) - (3.0 / h_(i - 1)) * (a_(i) - a_(i - 1)); + } + + Eigen::VectorXd l(n + 1); + Eigen::VectorXd mu(n + 1); + Eigen::VectorXd z(n + 1); + l(0) = 1.0; + 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); + mu(i) = h_(i) / l(i); + z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); + } + b_.resize(n); + d_.resize(n); + c_.resize(n + 1); + + l(n) = 1.0; + z(n) = c_(n) = 0.0; + + for (int32_t j = n - 1; j >= 0; --j) { + c_(j) = z(j) - mu(j) * c_(j + 1); + b_(j) = (a_(j + 1) - a_(j)) / h_(j) - h_(j) * (c_(j + 1) + 2.0 * c_(j)) / 3.0; + d_(j) = (c_(j + 1) - c_(j)) / (3.0 * h_(j)); + } +} + +void CubicSpline::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + compute_parameters( + this->axis_, + 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); + 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); + 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); + return 2 * c_(i) + 6 * d_(i) * dx; +} + +} // 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 new file mode 100644 index 0000000000000..4897ba4d2a56c --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp @@ -0,0 +1,62 @@ +// 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 + +#include + +namespace autoware::motion_utils::trajectory_container::interpolator +{ + +void Linear::build_impl( + const Eigen::Ref & axis, const std::vector & values) +{ + this->axis_ = axis; + this->values_ = + Eigen::Map(values.data(), static_cast(values.size())); +} + +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 y0 = this->values_(idx); + const double y1 = this->values_(idx + 1); + return y0 + (y1 - y0) * (s - x0) / (x1 - x0); +} + +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 y0 = this->values_(idx); + const double y1 = this->values_(idx + 1); + return (y1 - y0) / (x1 - x0); +} + +double Linear::compute_second_derivative_impl(const double &) const +{ + return 0.0; +} + +size_t Linear::minimum_required_points() const +{ + return 2; +} + +} // 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 new file mode 100644 index 0000000000000..77f7af0eae93f --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp @@ -0,0 +1,75 @@ +// 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 + +#include +#include + +#include +#include +#include + +template +class TestInterpolator : public ::testing::Test +{ +public: + std::optional interpolator; + std::vector axis; + std::vector values; + + void SetUp() override + { + // generate random values -1 to 1 + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-1, 1); + axis.resize(10); + values.resize(10); + for (size_t i = 0; i < axis.size(); ++i) { + axis[i] = static_cast(i); + values[i] = dis(gen); + } + } +}; + +using Interpolators = testing::Types< + autoware::motion_utils::trajectory_container::interpolator::CubicSpline, + autoware::motion_utils::trajectory_container::interpolator::AkimaSpline, + autoware::motion_utils::trajectory_container::interpolator::Linear, + autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor, + autoware::motion_utils::trajectory_container::interpolator::Stairstep>; + +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); + } +} + +// Instantiate test cases for all interpolators +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::CubicSpline>; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::AkimaSpline>; +template class TestInterpolator; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor>; +template class TestInterpolator< + autoware::motion_utils::trajectory_container::interpolator::Stairstep>;