From 4996cf7676443990ea3b7c5fbc67fa2f6c3423c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 20 Sep 2024 13:09:45 +0300 Subject: [PATCH] fix(interpolation): fix bug of interpolation (#8903) * fix(interpolation): fix bug of interpolation Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * auto -> int64_t Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki * add const Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../interpolation/spline_interpolation.hpp | 29 +-- common/autoware_interpolation/package.xml | 1 + .../src/spline_interpolation.cpp | 237 ++++++++---------- .../test/src/test_spline_interpolation.cpp | 10 +- .../test_spline_interpolation_points_2d.cpp | 19 +- 5 files changed, 123 insertions(+), 173 deletions(-) diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index cfc58d7fc7c6c..f7feada78ff4f 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -18,6 +18,8 @@ #include "autoware/interpolation/interpolation_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include + #include #include #include @@ -26,25 +28,6 @@ namespace autoware::interpolation { -// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) -struct MultiSplineCoef -{ - MultiSplineCoef() = default; - - explicit MultiSplineCoef(const size_t num_spline) - { - a.resize(num_spline); - b.resize(num_spline); - c.resize(num_spline); - d.resize(num_spline); - } - - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; - // static spline interpolation functions std::vector spline( const std::vector & base_keys, const std::vector & base_values, @@ -97,11 +80,17 @@ class SplineInterpolation size_t getSize() const { return base_keys_.size(); } private: + Eigen::VectorXd a_; + Eigen::VectorXd b_; + Eigen::VectorXd c_; + Eigen::VectorXd d_; + std::vector base_keys_; - MultiSplineCoef multi_spline_coef_; void calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values); + + Eigen::Index get_index(const double & key) const; }; } // namespace autoware::interpolation diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index c0df4b45acf65..d2538bd602f45 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_universe_utils + eigen ament_cmake_ros ament_lint_auto diff --git a/common/autoware_interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp index b3f92170f9d65..991421919b818 100644 --- a/common/autoware_interpolation/src/spline_interpolation.cpp +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -14,65 +14,40 @@ #include "autoware/interpolation/spline_interpolation.hpp" +#include #include namespace autoware::interpolation { -// solve Ax = d -// where A is tridiagonal matrix -// [b_0 c_0 ... ] -// [a_0 b_1 c_1 ... O ] -// A = [ ... ] -// [ O ... a_N-3 b_N-2 c_N-2] -// [ ... a_N-2 b_N-1] -struct TDMACoef +Eigen::VectorXd solve_tridiagonal_matrix_algorithm( + const Eigen::Ref & a, const Eigen::Ref & b, + const Eigen::Ref & c, const Eigen::Ref & d) { - explicit TDMACoef(const size_t num_row) - { - a.resize(num_row - 1); - b.resize(num_row); - c.resize(num_row - 1); - d.resize(num_row); + const auto n = d.size(); + + if (n == 1) { + return d.array() / b.array(); } - std::vector a; - std::vector b; - std::vector c; - std::vector d; -}; + Eigen::VectorXd c_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd d_prime = Eigen::VectorXd::Zero(n); + Eigen::VectorXd x = Eigen::VectorXd::Zero(n); -inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef) -{ - const auto & a = tdma_coef.a; - const auto & b = tdma_coef.b; - const auto & c = tdma_coef.c; - const auto & d = tdma_coef.d; - - const size_t num_row = b.size(); - - std::vector x(num_row); - if (num_row != 1) { - // calculate p and q - std::vector p; - std::vector q; - p.push_back(-c[0] / b[0]); - q.push_back(d[0] / b[0]); - - for (size_t i = 1; i < num_row; ++i) { - const double den = b[i] + a[i - 1] * p[i - 1]; - p.push_back(-c[i - 1] / den); - q.push_back((d[i] - a[i - 1] * q[i - 1]) / den); - } + // Forward sweep + c_prime(0) = c(0) / b(0); + d_prime(0) = d(0) / b(0); - // calculate solution - x[num_row - 1] = q[num_row - 1]; + for (auto i = 1; i < n; i++) { + const double m = 1.0 / (b(i) - a(i - 1) * c_prime(i - 1)); + c_prime(i) = i < n - 1 ? c(i) * m : 0; + d_prime(i) = (d(i) - a(i - 1) * d_prime(i - 1)) * m; + } - for (size_t i = 1; i < num_row; ++i) { - const size_t j = num_row - 1 - i; - x[j] = p[j] * x[j + 1] + q[j]; - } - } else { - x[0] = (d[0] / b[0]); + // Back substitution + x(n - 1) = d_prime(n - 1); + + for (int64_t i = n - 2; i >= 0; i--) { + x(i) = d_prime(i) - c_prime(i) * x(i + 1); } return x; @@ -162,53 +137,59 @@ std::vector splineByAkima( return res; } +Eigen::Index SplineInterpolation::get_index(const double & key) const +{ + const auto it = std::lower_bound(base_keys_.begin(), base_keys_.end(), key); + return std::clamp( + static_cast(std::distance(base_keys_.begin(), it)) - 1, 0, + static_cast(base_keys_.size()) - 2); +} + void SplineInterpolation::calcSplineCoefficients( const std::vector & base_keys, const std::vector & base_values) { // throw exceptions for invalid arguments - validateKeysAndValues(base_keys, base_values); - - const size_t num_base = base_keys.size(); // N+1 - - std::vector diff_keys; // N - std::vector diff_values; // N - for (size_t i = 0; i < num_base - 1; ++i) { - diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i)); - diff_values.push_back(base_values.at(i + 1) - base_values.at(i)); - } - - std::vector v = {0.0}; - if (num_base > 2) { - // solve tridiagonal matrix algorithm - TDMACoef tdma_coef(num_base - 2); // N-1 - - for (size_t i = 0; i < num_base - 2; ++i) { - tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]); - if (i != num_base - 3) { - tdma_coef.a[i] = diff_keys[i + 1]; - tdma_coef.c[i] = diff_keys[i + 1]; - } - tdma_coef.d[i] = - 6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]); - } - - const std::vector tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef); - - // calculate v - v.insert(v.end(), tdma_res.begin(), tdma_res.end()); - } - v.push_back(0.0); - - // calculate a, b, c, d of spline coefficients - multi_spline_coef_ = MultiSplineCoef{num_base - 1}; // N - for (size_t i = 0; i < num_base - 1; ++i) { - multi_spline_coef_.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; - multi_spline_coef_.b[i] = v[i] / 2.0; - multi_spline_coef_.c[i] = - diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0; - multi_spline_coef_.d[i] = base_values[i]; + interpolation_utils::validateKeysAndValues(base_keys, base_values); + const Eigen::VectorXd x = Eigen::Map( + base_keys.data(), static_cast(base_keys.size())); + const Eigen::VectorXd y = Eigen::Map( + base_values.data(), static_cast(base_values.size())); + + const auto n = x.size(); + + if (n == 2) { + a_ = Eigen::VectorXd::Zero(1); + b_ = Eigen::VectorXd::Zero(1); + c_ = Eigen::VectorXd::Zero(1); + d_ = Eigen::VectorXd::Zero(1); + c_[0] = (y[1] - y[0]) / (x[1] - x[0]); + d_[0] = y[0]; + base_keys_ = base_keys; + return; } + // Create Tridiagonal matrix + Eigen::VectorXd v(n); + const Eigen::VectorXd h = x.segment(1, n - 1) - x.segment(0, n - 1); + const Eigen::VectorXd a = h.segment(1, n - 3); + const Eigen::VectorXd b = 2 * (h.segment(0, n - 2) + h.segment(1, n - 2)); + const Eigen::VectorXd c = h.segment(1, n - 3); + const Eigen::VectorXd y_diff = y.segment(1, n - 1) - y.segment(0, n - 1); + const Eigen::VectorXd d = 6 * (y_diff.segment(1, n - 2).array() / h.tail(n - 2).array() - + y_diff.segment(0, n - 2).array() / h.head(n - 2).array()); + + // Solve tridiagonal matrix + v.segment(1, n - 2) = solve_tridiagonal_matrix_algorithm(a, b, c, d); + v[0] = 0; + v[n - 1] = 0; + + // Calculate spline coefficients + a_ = (v.tail(n - 1) - v.head(n - 1)).array() / 6.0 / (x.tail(n - 1) - x.head(n - 1)).array(); + b_ = v.segment(0, n - 1) / 2.0; + c_ = (y.tail(n - 1) - y.head(n - 1)).array() / (x.tail(n - 1) - x.head(n - 1)).array() - + (x.tail(n - 1) - x.head(n - 1)).array() * + (2 * v.segment(0, n - 1).array() + v.segment(1, n - 1).array()) / 6.0; + d_ = y.head(n - 1); base_keys_ = base_keys; } @@ -216,71 +197,51 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - const auto & d = multi_spline_coef_.d; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_values; + interpolated_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_values.emplace_back( + a_[idx] * dx * dx * dx + b_[idx] * dx * dx + c_[idx] * dx + d_[idx]); } - return res; + return interpolated_values; } std::vector SplineInterpolation::getSplineInterpolatedDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - const auto & c = multi_spline_coef_.c; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(c.at(j) + (2.0 * b.at(j) + 3.0 * a.at(j) * ds) * ds); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_diff_values; + interpolated_diff_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_diff_values.emplace_back(3 * a_[idx] * dx * dx + 2 * b_[idx] * dx + c_[idx]); } - return res; + return interpolated_diff_values; } std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - const auto validated_query_keys = validateKeys(base_keys_, query_keys); - - const auto & a = multi_spline_coef_.a; - const auto & b = multi_spline_coef_.b; - - std::vector res; - size_t j = 0; - for (const auto & query_key : validated_query_keys) { - while (base_keys_.at(j + 1) < query_key) { - ++j; - } - - const double ds = query_key - base_keys_.at(j); - res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + std::vector interpolated_quad_diff_values; + interpolated_quad_diff_values.reserve(query_keys.size()); + + for (const auto & key : query_keys) { + const auto idx = get_index(key); + const auto dx = key - base_keys_[idx]; + interpolated_quad_diff_values.emplace_back(6 * a_[idx] * dx + 2 * b_[idx]); } - return res; + return interpolated_quad_diff_values; } } // namespace autoware::interpolation diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp index 79edf9315ea44..766e94a47b968 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -66,7 +66,7 @@ TEST(spline_interpolation, spline) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -114,7 +114,7 @@ TEST(spline_interpolation, spline) const std::vector base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0}; const std::vector base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; 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 std::vector ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1}; const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { @@ -237,7 +237,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); @@ -252,7 +252,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; - const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; + const std::vector ans{0.671343, 0.049289, 0.209471, -0.253746}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); @@ -267,7 +267,7 @@ TEST(spline_interpolation, SplineInterpolation) const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; - const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; + const std::vector ans{-0.155829, 0.043097, -0.011143, -0.049611}; SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp index 097fc4e43cb7f..974ad606b978d 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -53,8 +53,7 @@ TEST(spline_interpolation, splineYawFromPoints) points.push_back(createPoint(5.0, 10.0, 0.0)); points.push_back(createPoint(10.0, 12.5, 0.0)); - const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - + const std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; const auto yaws = autoware::interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); @@ -123,8 +122,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // random const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.3036484, epsilon); - EXPECT_NEAR(random_point.y, 10.3343074, epsilon); + EXPECT_NEAR(random_point.x, 5.28974, epsilon); + EXPECT_NEAR(random_point.y, 10.3450319, epsilon); // out of range of total length const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); @@ -142,17 +141,17 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) { // yaw // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon); // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.808580, epsilon); // out of range of total length - EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.368174, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.278594, epsilon); // out of range of index EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); @@ -167,7 +166,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); // random - EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon); // out of range of total length EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon);