fix(interpolation): use slerp for spherical_linear_interpolation (aut…

…owarefoundation#1769) * fix(interpolation): use slerp for spherical_linear_interpolation Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Oct 3, 2022 · 09757f0 · 09757f0
1 parent 1235738
commit 09757f0
Show file tree

Hide file tree

Showing 20 changed files with 101 additions and 101 deletions.
diff --git 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
@@ -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<geometry_msgs::msg::Quaternion> spherical_linear_interpolation(
+std::vector<geometry_msgs::msg::Quaternion> slerp(
   const std::vector<double> & base_keys,
   const std::vector<geometry_msgs::msg::Quaternion> & base_values,
   const std::vector<double> & query_keys);

diff --git 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<double> slerp(
+std::vector<double> spline(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys);
-std::vector<double> slerpByAkima(
+std::vector<double> splineByAkima(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & 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
@@ -22,7 +22,7 @@
 namespace interpolation
 {
 template <typename T>
-std::vector<double> slerpYawFromPoints(const std::vector<T> & points);
+std::vector<double> splineYawFromPoints(const std::vector<T> & points);
 }  // namespace interpolation
 
 // non-static points spline interpolation
@@ -66,8 +66,8 @@ class SplineInterpolationPoints2d
 
 private:
   void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
-  SplineInterpolation slerp_x_;
-  SplineInterpolation slerp_y_;
+  SplineInterpolation spline_x_;
+  SplineInterpolation spline_y_;
 
   std::vector<double> base_s_vec_;
 };

diff --git 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<geometry_msgs::msg::Quaternion> spherical_linear_interpolation(
+std::vector<geometry_msgs::msg::Quaternion> slerp(
   const std::vector<double> & base_keys,
   const std::vector<geometry_msgs::msg::Quaternion> & base_values,
   const std::vector<double> & query_keys)
@@ -50,7 +50,7 @@ std::vector<geometry_msgs::msg::Quaternion> 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
@@ -81,7 +81,7 @@ inline std::vector<double> solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma
 
 namespace interpolation
 {
-std::vector<double> slerp(
+std::vector<double> spline(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys)
 {
@@ -94,7 +94,7 @@ std::vector<double> slerp(
   return interpolator.getSplineInterpolatedValues(query_keys);
 }
 
-std::vector<double> slerpByAkima(
+std::vector<double> splineByAkima(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys)
 {

diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp
@@ -69,7 +69,7 @@ std::array<std::vector<double>, 3> getBaseValues(
 namespace interpolation
 {
 template <typename T>
-std::vector<double> slerpYawFromPoints(const std::vector<T> & points)
+std::vector<double> splineYawFromPoints(const std::vector<T> & points)
 {
   SplineInterpolationPoints2d interpolator;
 
@@ -84,7 +84,7 @@ std::vector<double> slerpYawFromPoints(const std::vector<T> & points)
   }
   return yaw_vec;
 }
-template std::vector<double> slerpYawFromPoints(
+template std::vector<double> splineYawFromPoints(
   const std::vector<geometry_msgs::msg::Point> & 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
@@ -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<double> 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);