feat: port autoware_interpolation from autoware.universe

common/autoware_interpolation/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_interpolation)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(autoware_interpolation SHARED
+  src/linear_interpolation.cpp
+  src/spline_interpolation.cpp
+  src/spline_interpolation_points_2d.cpp
+  src/spherical_linear_interpolation.cpp
+)
+
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_interpolation ${test_files})
+
+  target_link_libraries(test_interpolation
+    autoware_interpolation
+  )
+endif()
+
+ament_auto_package()

common/autoware_interpolation/README.md

@@ -0,0 +1,109 @@
+# Interpolation package
+
+This package supplies linear and spline interpolation functions.
+
+## Linear Interpolation
+
+`lerp(src_val, dst_val, ratio)` (for scalar interpolation) interpolates `src_val` and `dst_val` with `ratio`.
+This will be replaced with `std::lerp(src_val, dst_val, ratio)` in `C++20`.
+
+`lerp(base_keys, base_values, query_keys)` (for vector interpolation) applies linear 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.
+
+## Spline Interpolation
+
+`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
+
+We evaluated calculation cost of spline interpolation for 100 points, and adopted the best one which is tridiagonal matrix algorithm.
+Methods except for tridiagonal matrix algorithm exists in `spline_interpolation` package, which has been removed from Autoware.
+
+| Method                            | Calculation time |
+| --------------------------------- | ---------------- |
+| Tridiagonal Matrix Algorithm      | 0.007 [ms]       |
+| Preconditioned Conjugate Gradient | 0.024 [ms]       |
+| Successive Over-Relaxation        | 0.074 [ms]       |
+
+### Spline Interpolation Algorithm
+
+Assuming that the size of `base_keys` ($x_i$) and `base_values` ($y_i$) are $N + 1$, we aim to calculate spline interpolation with the following equation to interpolate between $y_i$ and $y_{i+1}$.
+
+$$
+Y_i(x) = a_i (x - x_i)^3 + b_i (x - x_i)^2 + c_i (x - x_i) + d_i \ \ \ (i = 0, \dots, N-1)
+$$
+
+Constraints on spline interpolation are as follows.
+The number of constraints is $4N$, which is equal to the number of variables of spline interpolation.
+
+$$
+\begin{align}
+Y_i (x_i) & = y_i \ \ \ (i = 0, \dots, N-1) \\
+Y_i (x_{i+1}) & = y_{i+1} \ \ \ (i = 0, \dots, N-1) \\
+Y_i '(x_{i+1}) & = Y_{i+1}' (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\
+Y_i (x_{i+1})'' & = Y_{i+1}'' (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\
+Y_0 ''(x_0) & = 0 \\
+Y_{N-1}'' (x_N) & = 0
+\end{align}
+$$
+
+According to [this article](https://www.mk-mode.com/rails/docs/INTERPOLATION_SPLINE.pdf), spline interpolation is formulated as the following linear equation.
+
+$$
+\begin{align}
+ \begin{pmatrix}
+    2(h_0 + h_1) & h_1 \\
+    h_0 & 2 (h_1 + h_2) & h_2 & & O \\
+        &     &     & \ddots \\
+    O &     &     &       & h_{N-2} & 2 (h_{N-2} + h_{N-1})
+ \end{pmatrix}
+ \begin{pmatrix}
+    v_1 \\ v_2 \\ v_3 \\ \vdots \\ v_{N-1}
+ \end{pmatrix}=
+ \begin{pmatrix}
+    w_1 \\ w_2 \\ w_3 \\ \vdots \\ w_{N-1}
+ \end{pmatrix}
+\end{align}
+$$
+
+where
+
+$$
+\begin{align}
+h_i & = x_{i+1} - x_i \ \ \ (i = 0, \dots, N-1) \\
+w_i & = 6 \left(\frac{y_{i+1} - y_{i+1}}{h_i} - \frac{y_i - y_{i-1}}{h_{i-1}}\right) \ \ \ (i = 1, \dots, N-1)
+\end{align}
+$$
+
+The coefficient matrix of this linear equation is tridiagonal matrix. Therefore, it can be solve with tridiagonal matrix algorithm, which can solve linear equations without gradient descent methods.
+
+Solving this linear equation with tridiagonal matrix algorithm, we can calculate coefficients of spline interpolation as follows.
+
+$$
+\begin{align}
+a_i & = \frac{v_{i+1} - v_i}{6 (x_{i+1} - x_i)} \ \ \ (i = 0, \dots, N-1) \\
+b_i & = \frac{v_i}{2} \ \ \ (i = 0, \dots, N-1) \\
+c_i & = \frac{y_{i+1} - y_i}{x_{i+1} - x_i} - \frac{1}{6}(x_{i+1} - x_i)(2 v_i + v_{i+1}) \ \ \ (i = 0, \dots, N-1) \\
+d_i & = y_i \ \ \ (i = 0, \dots, N-1)
+\end{align}
+$$
+
+### Tridiagonal Matrix Algorithm
+
+We solve tridiagonal linear equation according to [this article](https://www.iist.ac.in/sites/default/files/people/tdma.pdf) where variables of linear equation are expressed as follows in the implementation.
+
+$$
+\begin{align}
+ \begin{pmatrix}
+    b_0 & c_0 &     & \\
+    a_0 & b_1 & c_2 & O \\
+        &     & \ddots \\
+    O &     & a_{N-2} &  b_{N-1}
+ \end{pmatrix}
+x =
+\begin{pmatrix}
+    d_0 \\ d_2 \\ d_3 \\ \vdots \\ d_{N-1}
+ \end{pmatrix}
+\end{align}
+$$

common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp

@@ -0,0 +1,114 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
+#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
+
+#include <algorithm>
+#include <array>
+#include <stdexcept>
+#include <vector>
+
+namespace autoware::interpolation
+{
+inline bool isIncreasing(const std::vector<double> & x)
+{
+  if (x.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    if (x.at(i) >= x.at(i + 1)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+inline bool isNotDecreasing(const std::vector<double> & x)
+{
+  if (x.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    if (x.at(i) > x.at(i + 1)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+inline std::vector<double> validateKeys(
+  const std::vector<double> & base_keys, const std::vector<double> & query_keys)
+{
+  // when vectors are empty
+  if (base_keys.empty() || query_keys.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()));
+  }
+
+  // when indices are not sorted
+  if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) {
+    throw std::invalid_argument("Either base_keys or query_keys is not sorted.");
+  }
+
+  // when query_keys is out of base_keys (This function does not allow exterior division.)
+  constexpr double epsilon = 1e-3;
+  if (
+    query_keys.front() < base_keys.front() - epsilon ||
+    base_keys.back() + epsilon < query_keys.back()) {
+    throw std::invalid_argument("query_keys is out of base_keys");
+  }
+
+  // NOTE: Due to calculation error of double, a query key may be slightly out of base keys.
+  //       Therefore, query keys are cropped here.
+  auto validated_query_keys = query_keys;
+  validated_query_keys.front() = std::max(validated_query_keys.front(), base_keys.front());
+  validated_query_keys.back() = std::min(validated_query_keys.back(), base_keys.back());
+
+  return validated_query_keys;
+}
+
+template <class T>
+void validateKeysAndValues(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values)
+{
+  // when vectors are empty
+  if (base_keys.empty() || base_values.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2 || base_values.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
+      ", base_values.size() = " + std::to_string(base_values.size()));
+  }
+
+  // when sizes of indices and values are not same
+  if (base_keys.size() != base_values.size()) {
+    throw std::invalid_argument("The size of base_keys and base_values are not the same.");
+  }
+}
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_

common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp

@@ -0,0 +1,35 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+double lerp(const double src_val, const double dst_val, const double ratio);
+
+std::vector<double> lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const double query_key);
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_

common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp

@@ -0,0 +1,48 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+
+#include <geometry_msgs/msg/quaternion.hpp>
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <vector>
+
+namespace autoware::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> slerp(
+  const std::vector<double> & base_keys,
+  const std::vector<geometry_msgs::msg::Quaternion> & base_values,
+  const std::vector<double> & query_keys);
+
+geometry_msgs::msg::Quaternion lerpOrientation(
+  const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
+  const double ratio);
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_