feat(interpolation): add curvature calculation (autowarefoundation#2801)

* feat(interpolation): add curvature calculation

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add test

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>

common/interpolation/include/interpolation/spline_interpolation.hpp

@@ -70,9 +70,11 @@ class SplineInterpolation
 {
 public:
   SplineInterpolation() = default;
-
-  void calcSplineCoefficients(
-    const std::vector<double> & base_keys, const std::vector<double> & base_values);
+  SplineInterpolation(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values)
+  {
+    calcSplineCoefficients(base_keys, base_values);
+  }
 
   //!< @brief get values of spline interpolation on designated sampling points.
   //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
@@ -86,9 +88,21 @@ class SplineInterpolation
   //            return value will be dx/dt(t) vector
   std::vector<double> getSplineInterpolatedDiffValues(const std::vector<double> & query_keys) const;
 
+  //!< @brief get 2nd differential values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be d^2/dt^2(t) vector
+  std::vector<double> getSplineInterpolatedQuadDiffValues(
+    const std::vector<double> & query_keys) const;
+
+  size_t getSize() const { return base_keys_.size(); }
+
 private:
   std::vector<double> base_keys_;
   interpolation::MultiSplineCoef multi_spline_coef_;
+
+  void calcSplineCoefficients(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values);
 };
 
 #endif  // INTERPOLATION__SPLINE_INTERPOLATION_HPP_

common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp

@@ -37,20 +37,19 @@ std::vector<double> splineYawFromPoints(const std::vector<T> & points);
 // SplineInterpolationPoints2d spline;
 // // memorize pre-interpolation result internally
 // spline.calcSplineCoefficients(base_keys, base_values);
-// const auto interpolation_result1 = spline.getSplineInterpolatedPoints(
+// const auto interpolation_result1 = spline.getSplineInterpolatedPoint(
 //   base_keys, query_keys1);
-// const auto interpolation_result2 = spline.getSplineInterpolatedPoints(
+// const auto interpolation_result2 = spline.getSplineInterpolatedPoint(
 //   base_keys, query_keys2);
-// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaws(
+// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw(
 //   base_keys, query_keys1);
 // ```
 class SplineInterpolationPoints2d
 {
 public:
   SplineInterpolationPoints2d() = default;
-
   template <typename T>
-  void calcSplineCoefficients(const std::vector<T> & points)
+  explicit SplineInterpolationPoints2d(const std::vector<T> & points)
   {
     std::vector<geometry_msgs::msg::Point> points_inner;
     for (const auto & p : points) {
@@ -63,9 +62,22 @@ class SplineInterpolationPoints2d
   // std::vector<geometry_msgs::msg::Point> getSplineInterpolatedPoints(const double width);
   // std::vector<geometry_msgs::msg::Pose> getSplineInterpolatedPoses(const double width);
 
+  // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw)
+  geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const;
+
+  // point
   geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const;
+
+  // yaw
   double getSplineInterpolatedYaw(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedYaws() const;
+
+  // curvature
+  double getSplineInterpolatedCurvature(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedCurvatures() const;
 
+  size_t getSize() const { return base_s_vec_.size(); }
+  size_t getOffsetIndex(const size_t idx, const double offset) const;
   double getAccumulatedLength(const size_t idx) const;
 
 private:

common/interpolation/src/spline_interpolation.cpp

@@ -85,10 +85,8 @@ std::vector<double> spline(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys)
 {
-  SplineInterpolation interpolator;
-
   // calculate spline coefficients
-  interpolator.calcSplineCoefficients(base_keys, base_values);
+  SplineInterpolation interpolator(base_keys, base_values);
 
   // interpolate base_keys at query_keys
   return interpolator.getSplineInterpolatedValues(query_keys);
@@ -266,3 +264,26 @@ std::vector<double> SplineInterpolation::getSplineInterpolatedDiffValues(
 
   return res;
 }
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedQuadDiffValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys);
+
+  const auto & a = multi_spline_coef_.a;
+  const auto & b = multi_spline_coef_.b;
+
+  std::vector<double> 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);
+  }
+
+  return res;
+}

common/interpolation/src/spline_interpolation_points_2d.cpp

@@ -73,14 +73,14 @@ std::array<std::vector<double>, 3> slerp2dFromXY(
   const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
   const std::vector<double> & base_y_values, const std::vector<double> & query_keys)
 {
-  SplineInterpolation interpolator_x, interpolator_y;
-  std::vector<double> yaw_vec;
-
   // calculate spline coefficients
-  interpolator_x.calcSplineCoefficients(base_keys, base_x_values);
-  interpolator_y.calcSplineCoefficients(base_keys, base_y_values);
+  SplineInterpolation interpolator_x(base_keys, base_x_values);
+  SplineInterpolation interpolator_y(base_keys, base_y_values);
   const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys);
   const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys);
+
+  // calculate yaw
+  std::vector<double> yaw_vec;
   for (size_t i = 0; i < diff_x.size(); i++) {
     double yaw = std::atan2(diff_y[i], diff_x[i]);
     yaw_vec.push_back(yaw);
@@ -94,10 +94,8 @@ std::array<std::vector<double>, 3> slerp2dFromXY(
 template <typename T>
 std::vector<double> splineYawFromPoints(const std::vector<T> & points)
 {
-  SplineInterpolationPoints2d interpolator;
-
   // calculate spline coefficients
-  interpolator.calcSplineCoefficients(points);
+  SplineInterpolationPoints2d interpolator(points);
 
   // interpolate base_keys at query_keys
   std::vector<double> yaw_vec;
@@ -112,6 +110,16 @@ template std::vector<double> splineYawFromPoints(
 
 }  // namespace interpolation
 
+geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose(
+  const size_t idx, const double s) const
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position = getSplineInterpolatedPoint(idx, s);
+  pose.orientation =
+    tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s));
+  return pose;
+}
+
 geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint(
   const size_t idx, const double s) const
 {
@@ -142,18 +150,64 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c
     throw std::out_of_range("idx is out of range.");
   }
 
-  double whole_s = base_s_vec_.at(idx) + s;
-  if (whole_s < base_s_vec_.front()) {
-    whole_s = base_s_vec_.front();
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
+  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);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedYaws() const
+{
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double yaw = getSplineInterpolatedYaw(i, 0.0);
+    yaw_vec.push_back(yaw);
   }
-  if (whole_s > base_s_vec_.back()) {
-    whole_s = base_s_vec_.back();
+  return yaw_vec;
+}
+
+double SplineInterpolationPoints2d::getSplineInterpolatedCurvature(
+  const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
   }
 
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
   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);
+  const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+  const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+
+  return (diff_x * quad_diff_y - quad_diff_x * diff_y) /
+         std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const
+{
+  std::vector<double> curvature_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double curvature = getSplineInterpolatedCurvature(i, 0.0);
+    curvature_vec.push_back(curvature);
+  }
+  return curvature_vec;
+}
+
+size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const
+{
+  const double whole_s = base_s_vec_.at(idx) + offset;
+  for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) {
+    if (whole_s < base_s_vec_.at(s_idx)) {
+      return s_idx;
+    }
+  }
+  return base_s_vec_.size() - 1;
 }
 
 double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const
@@ -174,6 +228,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
   const auto & base_y_vec = base.at(2);
 
   // calculate spline coefficients
-  spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec);
-  spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec);
+  spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec);
+  spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec);
 }