Add spline interpolator to tesseract_common

This function uses Eigen to interpolate each column of a TrajArray using a cubic spline.

tesseract/tesseract_common/include/tesseract_common/interpolators.h

@@ -0,0 +1,169 @@
+/**
+ * @file utils.h
+ * @brief Common Tesseract Utilities for Interpolating
+ *
+ * @date January 7, 2020
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2019, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * 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
+ * @par
+ * 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 TESSERACT_COMMON_INTERPOLATORS_H
+#define TESSERACT_COMMON_INTERPOLATORS_H
+
+#include <tesseract_common/macros.h>
+#include <tesseract_common/types.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <Eigen/Core>
+#include <unsupported/Eigen/Splines>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+namespace tesseract_common
+{
+/** @brief Uses Eigen/Splines to fit a 3rd order B-Spline to the input data and provides an operator for retrieving
+ * intermediate points
+ *
+ * Note: B-Spline may be lower order than requested for short input vectors */
+class SplineFunction
+{
+public:
+  /**
+   * @brief SplineFunction Constructor for interpolating while applying restrictions on the derivates of some points
+   *
+   * Example: Start/end with 0 velocity,
+   * Derivatives is a VectorXd of length 2 filled with zeros;
+   * Indices is then a VectorXi of length 2 filled with 0 and x_vec.size()-1
+   *
+   * **Note: There is a known bug in Eigen as of 1/6/2020. This will give incorrect results unless you patch
+   * SplineFitting.h.** See stackoverflow.com/questions/48382939 and https://gitlab.com/libeigen/eigen/merge_requests/41
+   * @param x_vec Independent variable. Probably time for most motion planning problems
+   * @param y_vec Dependent variable. Probably joint values for most motion planning problems.
+   * @param derivatives This is a vector of derivative values.
+   * @param indices This is a vector of indices of where those derivatives are applied
+   */
+  SplineFunction(const Eigen::Ref<Eigen::VectorXd>& x_vec,
+                 const Eigen::Ref<Eigen::VectorXd>& y_vec,
+                 const Eigen::Ref<Eigen::VectorXd>& derivatives,
+                 const Eigen::Ref<Eigen::VectorXi>& indices)
+    : x_min_(x_vec.minCoeff())
+    , x_max_(x_vec.maxCoeff())
+    ,
+    // Scale x values to [0:1] and curve fit with a 3rd order B-Spline.
+    spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::InterpolateWithDerivatives(
+        y_vec.transpose(),
+        derivatives.transpose(),
+        indices,
+        std::min<unsigned>(static_cast<unsigned>(x_vec.rows()) - 1, 3),
+        scaledValues(x_vec)))
+  {
+    assert(derivatives.size() == indices.size());
+    assert(!(x_vec.array().isNaN().any()));
+    assert(!(x_vec.array().isInf().any()));
+    assert(!(y_vec.array().isNaN().any()));
+    assert(!(y_vec.array().isInf().any()));
+  }
+
+  /**
+   * @brief Constructor for interpolating with no restrictions on derivatives
+   * @param x_vec Independent variable. Probably time for most motion planning problems
+   * @param y_vec Dependent variable. Probably joint values for most motion planning problems.
+   */
+  SplineFunction(const Eigen::Ref<Eigen::VectorXd>& x_vec, const Eigen::Ref<Eigen::VectorXd>& y_vec)
+    : x_min_(x_vec.minCoeff())
+    , x_max_(x_vec.maxCoeff())
+    ,
+    // Scale x values to [0:1] and curve fit with a 3rd order B-Spline.
+    spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(y_vec.transpose(),
+                                                                        std::min<long>(x_vec.rows() - 1, 3),
+                                                                        scaledValues(x_vec)))
+  {
+    assert(!(x_vec.array().isNaN().any()));
+    assert(!(x_vec.array().isInf().any()));
+    assert(!(y_vec.array().isNaN().any()));
+    assert(!(y_vec.array().isInf().any()));
+  }
+
+  /** @brief Returns the y value at point x in the spline */
+  double operator()(double x) const
+  {
+    // x values need to be scaled to [0:1] in extraction as well.
+    return spline_(scaledValue(x))(0);
+  }
+
+  /** @brief Returns a vector of interpolated y values for each x in the input vector */
+  Eigen::VectorXd operator()(const Eigen::Ref<Eigen::VectorXd>& x_vec) const
+  {
+    return x_vec.unaryExpr([this](double x) { return spline_(scaledValue(x))(0); });
+  }
+
+private:
+  /** @brief Scales value to [0:1] based on x_min and x_max */
+  double scaledValue(double x) const { return (x - x_min_) / (x_max_ - x_min_); }
+
+  /** @brief Scales vector such that each value is [0:1] based on x_min and x_max
+   *
+   * It is a requirement for Interpolate that x be on the interval [0:1] */
+  Eigen::RowVectorXd scaledValues(const Eigen::Ref<Eigen::VectorXd>& x_vec) const
+  {
+    return x_vec.unaryExpr([this](double x) { return scaledValue(x); }).transpose();
+  }
+
+  /** @brief Minimum value in x_vec. Used to scale inputs to [0:1] */
+  double x_min_;
+  /** @brief Maximum value in x_vec. Used to scale inputs to [0:1] */
+  double x_max_;
+
+  /** @brief One dimensional Spline that is used to interpolate the data
+   *
+   * Note: operator(double) returns an array of points. In this case the 0th element is the y value*/
+  Eigen::Spline<double, 1> spline_;
+};
+
+/**
+ * @brief Interpolates each column in a TrajArray using a cubic spline. The resuls are an evenly spaced trajectory with
+ * result_length size.
+ *
+ * Note: While the spline will hit these points, the resulting TrajArray is not guaranteed to include the original
+ * points unless result_length is of size n * (input_traj.rows() - 1) + 1
+ * @param input_traj Input TrajArray. Each column will be interpolated with a cubic spline.
+ * @param result_length Number of rows in the resulting TrajArray. For best results this should be size n *
+ * (input_traj.rows() - 1) + 1
+ * @return Resulting TrajArray of size results_length x input_traj.cols()
+ */
+inline TrajArray interpolateCubicSpline(const Eigen::Ref<TrajArray>& input_traj, const int& result_length)
+{
+  TrajArray results(result_length, input_traj.cols());
+  for (long ind = 0; ind < input_traj.cols(); ind++)
+  {
+    // Fit the spline to the input data
+    Eigen::VectorXd t_in =
+        Eigen::VectorXd::LinSpaced(input_traj.rows(), 0.0, static_cast<double>(input_traj.rows() - 1));
+    Eigen::VectorXd y_in = input_traj.col(ind);
+    SplineFunction spline(t_in, y_in);
+
+    // Extract evenly spaced points from spline
+    Eigen::VectorXd t_out = Eigen::VectorXd::LinSpaced(result_length, 0.0, static_cast<double>(input_traj.rows() - 1));
+    Eigen::VectorXd y_out = spline(t_out);
+    results.col(ind) = y_out;
+  }
+
+  return results;
+}
+
+}  // namespace tesseract_common
+
+#endif

tesseract/tesseract_common/test/tesseract_common_unit.cpp

@@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/utils.h>
+#include <tesseract_common/interpolators.h>
 
 TEST(TesseractCommonUnit, isNumeric)  // NOLINT
 {
@@ -49,6 +50,96 @@ TEST(TesseractCommonUnit, toNumeric)  // NOLINT
   }
 }
 
+TEST(TesseractCommonUnit, splineFunctionNoDerivatives)  // NOLINT
+{
+  Eigen::VectorXd xvals(10);
+  Eigen::VectorXd yvals(xvals.rows());
+
+  xvals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  yvals = xvals.array().square();
+
+  // Fit the spline
+  tesseract_common::SplineFunction spline(xvals, yvals);
+
+  // Check that input knots are hit exactly
+  for (long ind = 0; ind < xvals.size(); ind++)
+    EXPECT_NEAR(spline(xvals[ind]), yvals[ind], 1e-8);
+
+  Eigen::VectorXd xvals_interp(8);
+  xvals_interp << 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5;
+  // Check that the intermediate points are within 1% for a quadratic (they should be really close)
+  for (long ind = 0; ind < xvals_interp.size(); ind++)
+  {
+    double gt = xvals_interp[ind] * xvals_interp[ind];
+    EXPECT_NEAR(spline(xvals_interp[ind]), gt, gt * 0.01 + 1e-8);
+  }
+}
+
+TEST(TesseractCommonUnit, splineFunctionWithDerivatives)  // NOLINT
+{
+  Eigen::VectorXd xvals(10);
+  Eigen::VectorXd yvals(xvals.rows());
+
+  xvals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  yvals = xvals.array().cube();
+
+  Eigen::VectorXd derivatives(2);
+  derivatives << 0., 0.;
+  Eigen::VectorXi indices(2);
+  indices << 0, static_cast<int>(xvals.size() - 1);
+
+  // Fit the spline
+  tesseract_common::SplineFunction spline(xvals, yvals, derivatives, indices);
+
+  // Check that input knots are hit exactly
+  for (long ind = 0; ind < xvals.size(); ind++)
+    EXPECT_NEAR(spline(xvals[ind]), yvals[ind], 1e-8);
+
+  // Note: Interpolating using derivatives is not tested here because it requires patches to Eigen. See note in class
+  // documentation
+}
+
+TEST(TesseractCommonUnit, interpolateCubicSpline)  // NOLINT
+{
+  tesseract_common::TrajArray input_traj(10, 5);
+  Eigen::VectorXd t_in = Eigen::VectorXd::LinSpaced(input_traj.rows(), 0.0, static_cast<double>(input_traj.rows() - 1));
+  input_traj.col(0) = t_in.array().square();
+  input_traj.col(1) = t_in.array().cube();
+  input_traj.col(2) = t_in.array().sin();
+  input_traj.col(3) = t_in.array().cos();
+  input_traj.col(4) = t_in.array().exp();
+  std::cout << "Input Trajectory: \n" << input_traj << std::endl;
+
+  const int result_length = 46;
+  tesseract_common::TrajArray results = tesseract_common::interpolateCubicSpline(input_traj, result_length);
+  std::cout << "Spline Results: \n" << results << std::endl;
+
+  EXPECT_EQ(results.rows(), result_length);
+  EXPECT_EQ(results.cols(), input_traj.cols());
+
+  // Check that all of the knots are hit exactly
+  for (long ind = 0; ind < input_traj.rows(); ind++)
+  {
+    EXPECT_NEAR(input_traj.col(0)[ind], results.col(0)[ind * 5], 1e-8);
+    EXPECT_NEAR(input_traj.col(1)[ind], results.col(1)[ind * 5], 1e-8);
+    EXPECT_NEAR(input_traj.col(2)[ind], results.col(2)[ind * 5], 1e-8);
+    EXPECT_NEAR(input_traj.col(3)[ind], results.col(3)[ind * 5], 1e-8);
+    EXPECT_NEAR(input_traj.col(4)[ind], results.col(4)[ind * 5], 1e-8);
+  }
+
+  // Check that the intermediate points are within a small percentage. The polynomials should be really close
+  Eigen::VectorXd t_interp =
+      Eigen::VectorXd::LinSpaced(results.rows(), 0.0, static_cast<double>(input_traj.rows() - 1));
+  for (long ind = 0; ind < results.rows(); ind++)
+  {
+    EXPECT_NEAR(t_interp.array().square()[ind], results.col(0)[ind], t_interp.array().square()[ind] * 0.01 + 1e-8);
+    EXPECT_NEAR(t_interp.array().cube()[ind], results.col(1)[ind], t_interp.array().cube()[ind] * 0.01 + 1e-8);
+    EXPECT_NEAR(t_interp.array().sin()[ind], results.col(2)[ind], 1.0 * 0.05 + 1e-8);
+    EXPECT_NEAR(t_interp.array().cos()[ind], results.col(3)[ind], 1.0 * 0.05 + 1e-8);
+    EXPECT_NEAR(t_interp.array().exp()[ind], results.col(4)[ind], t_interp.array().exp()[ind] * 0.10 + 1e-8);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);