From 09757f0d77923e5758db9de78c24e6f8c0dec0e2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 2 Sep 2022 20:38:12 +0900 Subject: [PATCH] fix(interpolation): use slerp for spherical_linear_interpolation (#1769) * fix(interpolation): use slerp for spherical_linear_interpolation Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- common/interpolation/README.md | 2 +- .../spherical_linear_interpolation.hpp | 4 +- .../interpolation/spline_interpolation.hpp | 4 +- .../spline_interpolation_points_2d.hpp | 6 +-- .../src/spherical_linear_interpolation.cpp | 6 +-- .../src/spline_interpolation.cpp | 4 +- .../src/spline_interpolation_points_2d.cpp | 16 +++---- .../test_spherical_linear_interpolation.cpp | 16 +++---- .../test/src/test_spline_interpolation.cpp | 48 +++++++++---------- .../include/motion_common/motion_common.hpp | 10 ++-- .../src/motion_common/motion_common.cpp | 6 +-- common/motion_common/test/interpolation.cpp | 8 ++-- common/motion_utils/src/resample/resample.cpp | 10 ++-- .../src/predicted_path_utils.cpp | 4 +- control/trajectory_follower/src/mpc_utils.cpp | 12 ++--- .../src/path_generator.cpp | 20 ++++---- .../src/scene_module/utils/path_shifter.cpp | 2 +- .../src/mpt_optimizer.cpp | 6 +-- .../obstacle_avoidance_planner/src/node.cpp | 2 +- .../obstacle_avoidance_planner/src/utils.cpp | 16 +++---- 20 files changed, 101 insertions(+), 101 deletions(-) diff --git a/common/interpolation/README.md b/common/interpolation/README.md index a56af1e864e32..5801c06dcb717 100644 --- a/common/interpolation/README.md +++ b/common/interpolation/README.md @@ -12,7 +12,7 @@ Then it calculates interpolated values on y-axis for `query_keys` on x-axis. ## Spline Interpolation -`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. Then it calculates interpolated values on y-axis for `query_keys` on x-axis. ### Evaluation of calculation cost diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 58cb65a2f198f..516b3ab09e8b7 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -31,11 +31,11 @@ namespace interpolation { -geometry_msgs::msg::Quaternion spherical_linear_interpolation( +geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, const double ratio); -std::vector spherical_linear_interpolation( +std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index c56498249e91f..9640bcd6c5120 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -46,10 +46,10 @@ struct MultiSplineCoef }; // static spline interpolation functions -std::vector slerp( +std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); -std::vector slerpByAkima( +std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); } // namespace interpolation diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index c1f08a6d937ae..e1a60c86f036a 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -22,7 +22,7 @@ namespace interpolation { template -std::vector slerpYawFromPoints(const std::vector & points); +std::vector splineYawFromPoints(const std::vector & points); } // namespace interpolation // non-static points spline interpolation @@ -66,8 +66,8 @@ class SplineInterpolationPoints2d private: void calcSplineCoefficientsInner(const std::vector & points); - SplineInterpolation slerp_x_; - SplineInterpolation slerp_y_; + SplineInterpolation spline_x_; + SplineInterpolation spline_y_; std::vector base_s_vec_; }; diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index 1fc562032a990..014e9011e2a61 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -16,7 +16,7 @@ namespace interpolation { -geometry_msgs::msg::Quaternion spherical_linear_interpolation( +geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, const double ratio) { @@ -28,7 +28,7 @@ geometry_msgs::msg::Quaternion spherical_linear_interpolation( return tf2::toMsg(interpolated_quat); } -std::vector spherical_linear_interpolation( +std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) @@ -50,7 +50,7 @@ std::vector spherical_linear_interpolation( const double ratio = (query_key - base_keys.at(key_index)) / (base_keys.at(key_index + 1) - base_keys.at(key_index)); - const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio); + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); query_values.push_back(interpolated_quat); } diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index ebf7c1ddb18bd..cf00452f1d850 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -81,7 +81,7 @@ inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma namespace interpolation { -std::vector slerp( +std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { @@ -94,7 +94,7 @@ std::vector slerp( return interpolator.getSplineInterpolatedValues(query_keys); } -std::vector slerpByAkima( +std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 2a34e53fc884e..2570e01790487 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -69,7 +69,7 @@ std::array, 3> getBaseValues( namespace interpolation { template -std::vector slerpYawFromPoints(const std::vector & points) +std::vector splineYawFromPoints(const std::vector & points) { SplineInterpolationPoints2d interpolator; @@ -84,7 +84,7 @@ std::vector slerpYawFromPoints(const std::vector & points) } return yaw_vec; } -template std::vector slerpYawFromPoints( +template std::vector splineYawFromPoints( const std::vector & points); } // namespace interpolation @@ -103,8 +103,8 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin whole_s = base_s_vec_.back(); } - const double x = slerp_x_.getSplineInterpolatedValues({whole_s}).at(0); - const double y = slerp_y_.getSplineInterpolatedValues({whole_s}).at(0); + const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0); + const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0); geometry_msgs::msg::Point geom_point; geom_point.x = x; @@ -126,8 +126,8 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c whole_s = base_s_vec_.back(); } - const double diff_x = slerp_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); - const double diff_y = slerp_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); return std::atan2(diff_y, diff_x); } @@ -150,6 +150,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( const auto & base_y_vec = base.at(2); // calculate spline coefficients - slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); + spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); + spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); } diff --git a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/interpolation/test/src/test_spherical_linear_interpolation.cpp index cfa881fb76648..0bb9ba8ef2ce8 100644 --- a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp +++ b/common/interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -32,9 +32,9 @@ inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( } } // namespace -TEST(spherical_linear_interpolation, slerp_scalar) +TEST(slerp, spline_scalar) { - using interpolation::spherical_linear_interpolation; + using interpolation::slerp; // Same value { @@ -46,7 +46,7 @@ TEST(spherical_linear_interpolation, slerp_scalar) const auto ans_quat = createQuaternionFromRPY(0.0, 0.0, 0.0); for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { - const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio); + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); @@ -62,7 +62,7 @@ TEST(spherical_linear_interpolation, slerp_scalar) const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw); for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { - const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio); + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); const double ans_yaw = M_PI * ratio; tf2::Quaternion ans; @@ -77,9 +77,9 @@ TEST(spherical_linear_interpolation, slerp_scalar) } } -TEST(spherical_linear_interpolation, slerp_vector) +TEST(slerp, spline_vector) { - using interpolation::spherical_linear_interpolation; + using interpolation::slerp; // query keys are same as base keys { @@ -92,7 +92,7 @@ TEST(spherical_linear_interpolation, slerp_vector) const std::vector query_keys = base_keys; const auto ans = base_values; - const auto results = spherical_linear_interpolation(base_keys, base_values, query_keys); + const auto results = slerp(base_keys, base_values, query_keys); for (size_t i = 0; i < results.size(); ++i) { const auto interpolated_quat = results.at(i); @@ -122,7 +122,7 @@ TEST(spherical_linear_interpolation, slerp_vector) ans.at(4) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0 + 3.0 * M_PI / 5.0); ans.at(5) = createQuaternionFromRPY(0.0, 0.0, 0.8 * M_PI / 5.0 + 3.0 * M_PI / 5.0); - const auto results = spherical_linear_interpolation(base_keys, base_values, query_keys); + const auto results = slerp(base_keys, base_values, query_keys); for (size_t i = 0; i < results.size(); ++i) { const auto interpolated_quat = results.at(i); diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index 132b0100ff3b5..c2bb8eb177ba1 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -23,7 +23,7 @@ constexpr double epsilon = 1e-6; -TEST(spline_interpolation, slerp) +TEST(spline_interpolation, spline) { { // straight: query_keys is same as base_keys const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; @@ -31,7 +31,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -43,7 +43,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -55,7 +55,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -67,7 +67,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -79,7 +79,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -91,7 +91,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -103,7 +103,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.808769, -0.077539, 1.035096}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -115,14 +115,14 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } } -TEST(spline_interpolation, slerpByAkima) +TEST(spline_interpolation, splineByAkima) { { // straight: query_keys is same as base_keys const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; @@ -130,7 +130,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -142,7 +142,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -154,7 +154,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -166,7 +166,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.0801, 1.110749, 1.4864}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -178,7 +178,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -190,7 +190,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -202,7 +202,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.8378, -0.0801, 0.927031}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -214,14 +214,14 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } } -TEST(spline_interpolation, slerpYawFromPoints) +TEST(spline_interpolation, splineYawFromPoints) { using tier4_autoware_utils::createPoint; @@ -235,7 +235,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -251,7 +251,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -261,7 +261,7 @@ TEST(spline_interpolation, slerpYawFromPoints) std::vector points; points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(interpolation::slerpYawFromPoints(points), std::logic_error); + EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) @@ -271,7 +271,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -285,7 +285,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } diff --git a/common/motion_common/include/motion_common/motion_common.hpp b/common/motion_common/include/motion_common/motion_common.hpp index e73cff3ed6e3f..2742b1c073bda 100644 --- a/common/motion_common/include/motion_common/motion_common.hpp +++ b/common/motion_common/include/motion_common/motion_common.hpp @@ -159,11 +159,11 @@ T interpolate(T a, T b, RealT t) } /// Spherical linear interpolation -MOTION_COMMON_PUBLIC Orientation slerp(const Orientation & a, const Orientation & b, const Real t); +MOTION_COMMON_PUBLIC Orientation spline(const Orientation & a, const Orientation & b, const Real t); /// Trajectory point interpolation template -Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) +Point interpolate(Point a, Point b, Real t, SlerpF spline_fn) { Point ret{rosidl_runtime_cpp::MessageInitialization::ALL}; { @@ -173,7 +173,7 @@ Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) } ret.pose.position.x = interpolate(a.pose.position.x, b.pose.position.x, t); ret.pose.position.y = interpolate(a.pose.position.y, b.pose.position.y, t); - ret.pose.orientation = slerp_fn(a.pose.orientation, b.pose.orientation, t); + ret.pose.orientation = spline_fn(a.pose.orientation, b.pose.orientation, t); ret.longitudinal_velocity_mps = interpolate(a.longitudinal_velocity_mps, b.longitudinal_velocity_mps, t); ret.lateral_velocity_mps = interpolate(a.lateral_velocity_mps, b.lateral_velocity_mps, t); @@ -191,7 +191,7 @@ MOTION_COMMON_PUBLIC Point interpolate(Point a, Point b, Real t); /// Sample a trajectory using interpolation; does not extrapolate template void sample( - const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF slerp_fn) + const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF spline_fn) { out.points.clear(); out.header = in.header; @@ -230,7 +230,7 @@ void sample( const auto dt_ = std::chrono::duration_cast>( ref_duration - from_message(curr_pt.time_from_start)); const auto t = dt_.count() / dt.count(); - out.points.push_back(interpolate(curr_pt, next_pt, t, slerp_fn)); + out.points.push_back(interpolate(curr_pt, next_pt, t, spline_fn)); } ref_duration += period; } diff --git a/common/motion_common/src/motion_common/motion_common.cpp b/common/motion_common/src/motion_common/motion_common.cpp index aaedabfd83b9a..7ad71096c53f8 100644 --- a/common/motion_common/src/motion_common/motion_common.cpp +++ b/common/motion_common/src/motion_common/motion_common.cpp @@ -153,7 +153,7 @@ Heading nlerp(Heading a, Heading b, Real t) } //////////////////////////////////////////////////////////////////////////////// -Orientation slerp(const Orientation & a, const Orientation & b, const Real t) +Orientation spline(const Orientation & a, const Orientation & b, const Real t) { tf2::Quaternion quat_a; tf2::Quaternion quat_b; @@ -163,12 +163,12 @@ Orientation slerp(const Orientation & a, const Orientation & b, const Real t) } //////////////////////////////////////////////////////////////////////////////// -Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, slerp); } +Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, spline); } //////////////////////////////////////////////////////////////////////////////// void sample(const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period) { - sample(in, out, period, slerp); + sample(in, out, period, spline); } //////////////////////////////////////////////////////////////////////////////// diff --git a/common/motion_common/test/interpolation.cpp b/common/motion_common/test/interpolation.cpp index e59d9e0af9346..ae57e5ea76ba6 100644 --- a/common/motion_common/test/interpolation.cpp +++ b/common/motion_common/test/interpolation.cpp @@ -109,8 +109,8 @@ TEST(Interpolation, Slerp2d) }; // Plain check const auto test_case = [=](Orientation a, Orientation b, Real t, Orientation res, Real tol) { - using motion::motion_common::slerp; - const auto ret = slerp(a, b, t); + using motion::motion_common::spline; + const auto ret = spline(a, b, t); EXPECT_LT(std::fabs(to_angle(ret) - to_angle(res)), tol) << to_angle(ret) << ", " << to_angle(res); }; @@ -123,10 +123,10 @@ TEST(Interpolation, Slerp2d) const auto t_ = motion::motion_common::clamp(t, 0.0F, 1.0F); const auto th_t = th_a + (t_ * ab); const auto res_th = from_angle(th_t); - using motion::motion_common::slerp; + using motion::motion_common::spline; test_case(a, b, t, res_th, tol); if (HasFailure()) { - const auto ret = slerp(a, b, t); + const auto ret = spline(a, b, t); std::cout << "Angles: " << th_a << ", " << th_b << "; " << th_t << ", " << to_angle(ret) << "\n"; } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index affe832f6b221..7f050a4454ffe 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -66,13 +66,13 @@ std::vector resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; - const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_arclength, input, resampled_arclength); + const auto spline = [&](const auto & input) { + return interpolation::spline(input_arclength, input, resampled_arclength); }; - const auto interpolated_x = use_lerp_for_xy ? lerp(x) : slerp(x); - const auto interpolated_y = use_lerp_for_xy ? lerp(y) : slerp(y); - const auto interpolated_z = use_lerp_for_z ? lerp(z) : slerp(z); + const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x); + const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); + const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); std::vector resampled_points; resampled_points.resize(interpolated_x.size()); diff --git a/common/perception_utils/src/predicted_path_utils.cpp b/common/perception_utils/src/predicted_path_utils.cpp index 0eeed0a71bba2..96ca74d2cb234 100644 --- a/common/perception_utils/src/predicted_path_utils.cpp +++ b/common/perception_utils/src/predicted_path_utils.cpp @@ -66,10 +66,10 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return interpolation::lerp(input_time, input, resampled_time); }; const auto spline = [&](const auto & input) { - return interpolation::slerp(input_time, input, resampled_time); + return interpolation::spline(input_time, input, resampled_time); }; const auto slerp = [&](const auto & input) { - return interpolation::spherical_linear_interpolation(input_time, input, resampled_time); + return interpolation::slerp(input_time, input, resampled_time); }; const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index a56dc36a007de..6fce85d88827c 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -100,13 +100,13 @@ bool8_t resampleMPCTrajectoryByDistance( std::vector input_yaw = input.yaw; convertEulerAngleToMonotonic(&input_yaw); - output->x = interpolation::slerp(input_arclength, input.x, output_arclength); - output->y = interpolation::slerp(input_arclength, input.y, output_arclength); - output->z = interpolation::slerp(input_arclength, input.z, output_arclength); - output->yaw = interpolation::slerp(input_arclength, input.yaw, output_arclength); + output->x = interpolation::spline(input_arclength, input.x, output_arclength); + output->y = interpolation::spline(input_arclength, input.y, output_arclength); + output->z = interpolation::spline(input_arclength, input.z, output_arclength); + output->yaw = interpolation::spline(input_arclength, input.yaw, output_arclength); output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength); - output->k = interpolation::slerp(input_arclength, input.k, output_arclength); - output->smooth_k = interpolation::slerp(input_arclength, input.smooth_k, output_arclength); + output->k = interpolation::spline(input_arclength, input.k, output_arclength); + output->smooth_k = interpolation::spline(input_arclength, input.smooth_k, output_arclength); output->relative_time = interpolation::lerp(input_arclength, input.relative_time, output_arclength); diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4d594f46c8af8..093bc5c76f3b8 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -309,28 +309,28 @@ PosePath PathGenerator::interpolateReferencePath( } // Spline Interpolation - std::vector slerp_ref_path_x = - interpolation::slerp(base_path_s, base_path_x, resampled_s); - std::vector slerp_ref_path_y = - interpolation::slerp(base_path_s, base_path_y, resampled_s); - std::vector slerp_ref_path_z = - interpolation::slerp(base_path_s, base_path_z, resampled_s); + std::vector spline_ref_path_x = + interpolation::spline(base_path_s, base_path_x, resampled_s); + std::vector spline_ref_path_y = + interpolation::spline(base_path_s, base_path_y, resampled_s); + std::vector spline_ref_path_z = + interpolation::spline(base_path_s, base_path_z, resampled_s); interpolated_path.resize(interpolate_num); for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - tier4_autoware_utils::createPoint(slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), 0.0); + tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); const auto next_point = tier4_autoware_utils::createPoint( - slerp_ref_path_x.at(i + 1), slerp_ref_path_y.at(i + 1), 0.0); + spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); interpolated_pose.position = tier4_autoware_utils::createPoint( - slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), slerp_ref_path_z.at(i)); + spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } interpolated_path.back().position = tier4_autoware_utils::createPoint( - slerp_ref_path_x.back(), slerp_ref_path_y.back(), slerp_ref_path_z.back()); + spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; return interpolated_path; diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index ffaf8312bcf91..d89cf8ade1e9d 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -203,7 +203,7 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs query_distance.push_back(dist_from_start); } if (!query_distance.empty()) { - query_length = interpolation::slerp(base_distance, base_length, query_distance); + query_length = interpolation::spline(base_distance, base_length, query_distance); } // Apply shifting. diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 3cb66bb7a5087..1dea0206252eb 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -178,7 +178,7 @@ Eigen::Vector2d getState( return kinematics; } -std::vector slerpYawFromReferencePoints(const std::vector & ref_points) +std::vector splineYawFromReferencePoints(const std::vector & ref_points) { if (ref_points.size() == 1) { return {ref_points.front().yaw}; @@ -188,7 +188,7 @@ std::vector slerpYawFromReferencePoints(const std::vector MPTOptimizer::get void MPTOptimizer::calcOrientation(std::vector & ref_points) const { - const auto yaw_angles = slerpYawFromReferencePoints(ref_points); + const auto yaw_angles = splineYawFromReferencePoints(ref_points); for (size_t i = 0; i < ref_points.size(); ++i) { if (ref_points.at(i).fix_kinematic_state) { continue; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 3802dde0e067a..0bd3bf97dd541 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -181,7 +181,7 @@ std::tuple, std::vector> calcVehicleCirclesInfo( for (const auto & traj_point : traj_points) { points.push_back(traj_point.pose.position); } - const auto yaw_vec = interpolation::slerpYawFromPoints(points); + const auto yaw_vec = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < traj_points.size(); ++i) { traj_points.at(i).pose.orientation = diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 90e9daccb0a89..c45d7c808586d 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -110,7 +110,7 @@ std::array, 2> validatePoints( } // only two points is supported -std::vector slerpTwoPoints( +std::vector splineTwoPoints( std::vector base_s, std::vector base_x, const double begin_diff, const double end_diff, std::vector new_s) { @@ -259,8 +259,8 @@ std::vector interpolate2DPoints( } // spline interpolation - const std::vector interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const std::vector interpolated_x = interpolation::spline(base_s, base_x, new_s); + const std::vector interpolated_y = interpolation::spline(base_s, base_y, new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { return std::vector{}; @@ -296,9 +296,9 @@ std::vector interpolateConnec // spline interpolation const auto interpolated_x = - slerpTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); + splineTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); const auto interpolated_y = - slerpTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); + splineTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); for (size_t i = 0; i < interpolated_x.size(); i++) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { @@ -343,9 +343,9 @@ std::vector interpolate2DTraj const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); // spline interpolation - const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); + const auto interpolated_x = interpolation::spline(base_s, base_x, new_s); + const auto interpolated_y = interpolation::spline(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::spline(base_s, monotonic_base_yaw, new_s); for (size_t i = 0; i < interpolated_x.size(); i++) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {