Skip to content

Commit b9b0fa9

Browse files
yhisakiyhisaki
authored
feat(autoware_motion_utils): add interpolator (#8517)
* feat(autoware_motion_utils): add interpolator Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * use int32_t instead of int Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * use int32_t instead of int Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * use int32_t instead of int Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * add const as much as possible and use `at()` in `vector` Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * fix directory name Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * refactor code and add example Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * update Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * remove unused include Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * refactor code Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> --------- Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
1 parent 18b5ee1 commit b9b0fa9

File tree

17 files changed

+1418
-0
lines changed

17 files changed

+1418
-0
lines changed

‎common/autoware_motion_utils/CMakeLists.txt

+30
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,22 @@
11
cmake_minimum_required(VERSION 3.14)
22
project(autoware_motion_utils)
33

4+
option(BUILD_EXAMPLES "Build examples" OFF)
5+
46
find_package(autoware_cmake REQUIRED)
57
autoware_package()
68

79
find_package(Boost REQUIRED)
10+
find_package(fmt REQUIRED)
811

912
ament_auto_add_library(autoware_motion_utils SHARED
1013
DIRECTORY src
1114
)
1215

16+
target_link_libraries(autoware_motion_utils
17+
fmt::fmt
18+
)
19+
1320
if(BUILD_TESTING)
1421
find_package(ament_cmake_ros REQUIRED)
1522

@@ -22,4 +29,27 @@ if(BUILD_TESTING)
2229
)
2330
endif()
2431

32+
if(BUILD_EXAMPLES)
33+
message(STATUS "Building examples")
34+
35+
include(FetchContent)
36+
fetchcontent_declare(
37+
matplotlibcpp17
38+
GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git
39+
GIT_TAG master
40+
)
41+
fetchcontent_makeavailable(matplotlibcpp17)
42+
43+
file(GLOB_RECURSE example_files examples/*.cpp)
44+
45+
foreach(example_file ${example_files})
46+
get_filename_component(example_name ${example_file} NAME_WE)
47+
ament_auto_add_executable(${example_name} ${example_file})
48+
target_link_libraries(${example_name}
49+
autoware_motion_utils
50+
matplotlibcpp17::matplotlibcpp17
51+
)
52+
endforeach()
53+
endif()
54+
2555
ament_auto_package()

‎common/autoware_motion_utils/examples/example_interpolator.cpp

+115
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp"
16+
17+
#include <autoware/motion_utils/trajectory_container/interpolator.hpp>
18+
19+
#include <matplotlibcpp17/pyplot.h>
20+
21+
#include <random>
22+
#include <vector>
23+
24+
int main()
25+
{
26+
pybind11::scoped_interpreter guard{};
27+
auto plt = matplotlibcpp17::pyplot::import();
28+
29+
// create random values
30+
std::vector<double> axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
31+
std::vector<double> values;
32+
std::random_device seed_gen;
33+
std::mt19937 engine(seed_gen());
34+
std::uniform_real_distribution<> dist(-1.0, 1.0);
35+
for (size_t i = 0; i < axis.size(); ++i) {
36+
values.push_back(dist(engine));
37+
}
38+
// Scatter Data
39+
plt.scatter(Args(axis, values));
40+
41+
using autoware::motion_utils::trajectory_container::interpolator::Interpolator;
42+
using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator;
43+
// Linear Interpolator
44+
{
45+
using autoware::motion_utils::trajectory_container::interpolator::Linear;
46+
auto interpolator = *InterpolatorCreator<Linear>().set_axis(axis).set_values(values).create();
47+
std::vector<double> x;
48+
std::vector<double> y;
49+
for (double i = axis.front(); i < axis.back(); i += 0.01) {
50+
x.push_back(i);
51+
y.push_back(interpolator.compute(i));
52+
}
53+
plt.plot(Args(x, y), Kwargs("label"_a = "Linear"));
54+
}
55+
56+
// AkimaSpline Interpolator
57+
{
58+
using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline;
59+
auto interpolator =
60+
*InterpolatorCreator<AkimaSpline>().set_axis(axis).set_values(values).create();
61+
std::vector<double> x;
62+
std::vector<double> y;
63+
for (double i = axis.front(); i < axis.back(); i += 0.01) {
64+
x.push_back(i);
65+
y.push_back(interpolator.compute(i));
66+
}
67+
plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline"));
68+
}
69+
70+
// CubicSpline Interpolator
71+
{
72+
using autoware::motion_utils::trajectory_container::interpolator::CubicSpline;
73+
auto interpolator =
74+
*InterpolatorCreator<CubicSpline>().set_axis(axis).set_values(values).create();
75+
std::vector<double> x;
76+
std::vector<double> y;
77+
for (double i = axis.front(); i < axis.back(); i += 0.01) {
78+
x.push_back(i);
79+
y.push_back(interpolator.compute(i));
80+
}
81+
plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline"));
82+
}
83+
84+
// NearestNeighbor Interpolator
85+
{
86+
using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor;
87+
auto interpolator =
88+
*InterpolatorCreator<NearestNeighbor<double>>().set_axis(axis).set_values(values).create();
89+
std::vector<double> x;
90+
std::vector<double> y;
91+
for (double i = axis.front(); i < axis.back(); i += 0.01) {
92+
x.push_back(i);
93+
y.push_back(interpolator.compute(i));
94+
}
95+
plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor"));
96+
}
97+
98+
// Stairstep Interpolator
99+
{
100+
using autoware::motion_utils::trajectory_container::interpolator::Stairstep;
101+
auto interpolator =
102+
*InterpolatorCreator<Stairstep<double>>().set_axis(axis).set_values(values).create();
103+
std::vector<double> x;
104+
std::vector<double> y;
105+
for (double i = axis.front(); i < axis.back(); i += 0.01) {
106+
x.push_back(i);
107+
y.push_back(interpolator.compute(i));
108+
}
109+
plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep"));
110+
}
111+
112+
plt.legend();
113+
plt.show();
114+
return 0;
115+
}

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

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_
16+
#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_
17+
#include <autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp>
18+
#include <autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp>
19+
#include <autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp>
20+
#include <autoware/motion_utils/trajectory_container/interpolator/linear.hpp>
21+
#include <autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp>
22+
#include <autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp>
23+
#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_

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

+98
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_
16+
#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_
17+
18+
#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
19+
20+
#include <Eigen/Dense>
21+
22+
#include <vector>
23+
24+
namespace autoware::motion_utils::trajectory_container::interpolator
25+
{
26+
27+
/**
28+
* @brief Class for Akima spline interpolation.
29+
*
30+
* This class provides methods to perform Akima spline interpolation on a set of data points.
31+
*/
32+
class AkimaSpline : public Interpolator<double>
33+
{
34+
template <typename InterpolatorType>
35+
friend class InterpolatorCreator;
36+
37+
private:
38+
Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline.
39+
40+
AkimaSpline() = default;
41+
42+
/**
43+
* @brief Compute the spline parameters.
44+
*
45+
* This method computes the coefficients for the Akima spline.
46+
*
47+
* @param axis The axis values.
48+
* @param values The values to interpolate.
49+
*/
50+
void compute_parameters(
51+
const Eigen::Ref<const Eigen::VectorXd> & axis,
52+
const Eigen::Ref<const Eigen::VectorXd> & values);
53+
54+
/**
55+
* @brief Build the interpolator with the given values.
56+
*
57+
* @param axis The axis values.
58+
* @param values The values to interpolate.
59+
*/
60+
void build_impl(
61+
const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<double> & values) override;
62+
63+
/**
64+
* @brief Compute the interpolated value at the given point.
65+
*
66+
* @param s The point at which to compute the interpolated value.
67+
* @return The interpolated value.
68+
*/
69+
[[nodiscard]] double compute_impl(const double & s) const override;
70+
71+
/**
72+
* @brief Compute the first derivative at the given point.
73+
*
74+
* @param s The point at which to compute the first derivative.
75+
* @return The first derivative.
76+
*/
77+
[[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
78+
79+
/**
80+
* @brief Compute the second derivative at the given point.
81+
*
82+
* @param s The point at which to compute the second derivative.
83+
* @return The second derivative.
84+
*/
85+
[[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
86+
87+
public:
88+
/**
89+
* @brief Get the minimum number of required points for the interpolator.
90+
*
91+
* @return The minimum number of required points.
92+
*/
93+
[[nodiscard]] size_t minimum_required_points() const override { return 5; }
94+
};
95+
96+
} // namespace autoware::motion_utils::trajectory_container::interpolator
97+
98+
#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_

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

+100
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_
16+
#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_
17+
18+
#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
19+
20+
#include <Eigen/Dense>
21+
22+
#include <vector>
23+
24+
namespace autoware::motion_utils::trajectory_container::interpolator
25+
{
26+
27+
/**
28+
* @brief Class for cubic spline interpolation.
29+
*
30+
* This class provides methods to perform cubic spline interpolation on a set of data points.
31+
*/
32+
class CubicSpline : public Interpolator<double>
33+
{
34+
template <typename InterpolatorType>
35+
friend class InterpolatorCreator;
36+
37+
private:
38+
Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the cubic spline.
39+
Eigen::VectorXd h_; ///< Interval sizes between axis points.
40+
41+
CubicSpline() = default;
42+
43+
/**
44+
* @brief Compute the spline parameters.
45+
*
46+
* This method computes the coefficients for the cubic spline.
47+
*
48+
* @param axis The axis values.
49+
* @param values The values to interpolate.
50+
*/
51+
void compute_parameters(
52+
const Eigen::Ref<const Eigen::VectorXd> & axis,
53+
const Eigen::Ref<const Eigen::VectorXd> & values);
54+
55+
/**
56+
* @brief Build the interpolator with the given values.
57+
*
58+
* @param axis The axis values.
59+
* @param values The values to interpolate.
60+
* @return True if the interpolator was built successfully, false otherwise.
61+
*/
62+
void build_impl(
63+
const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<double> & values) override;
64+
65+
/**
66+
* @brief Compute the interpolated value at the given point.
67+
*
68+
* @param s The point at which to compute the interpolated value.
69+
* @return The interpolated value.
70+
*/
71+
[[nodiscard]] double compute_impl(const double & s) const override;
72+
73+
/**
74+
* @brief Compute the first derivative at the given point.
75+
*
76+
* @param s The point at which to compute the first derivative.
77+
* @return The first derivative.
78+
*/
79+
[[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
80+
81+
/**
82+
* @brief Compute the second derivative at the given point.
83+
*
84+
* @param s The point at which to compute the second derivative.
85+
* @return The second derivative.
86+
*/
87+
[[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
88+
89+
public:
90+
/**
91+
* @brief Get the minimum number of required points for the interpolator.
92+
*
93+
* @return The minimum number of required points.
94+
*/
95+
[[nodiscard]] size_t minimum_required_points() const override { return 4; }
96+
};
97+
98+
} // namespace autoware::motion_utils::trajectory_container::interpolator
99+
100+
#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_

0 commit comments

Comments
 (0)