Skip to content

Commit 5c26a7c

Browse files
mpowelsonmpowelson
committed
Add spline interpolator to tesseract_common
This function uses Eigen to interpolate each column of a TrajArray using a cubic spline.
1 parent dbc284d commit 5c26a7c

File tree

2 files changed

+260
-0
lines changed

2 files changed

+260
-0
lines changed

‎tesseract/tesseract_common/include/tesseract_common/interpolators.h‎

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
/**
2+
* @file utils.h
3+
* @brief Common Tesseract Utilities for Interpolating
4+
*
5+
* @date January 7, 2020
6+
* @version TODO
7+
* @bug No known bugs
8+
*
9+
* @copyright Copyright (c) 2019, Southwest Research Institute
10+
*
11+
* @par License
12+
* Software License Agreement (Apache License)
13+
* @par
14+
* Licensed under the Apache License, Version 2.0 (the "License");
15+
* you may not use this file except in compliance with the License.
16+
* You may obtain a copy of the License at
17+
* http://www.apache.org/licenses/LICENSE-2.0
18+
* @par
19+
* Unless required by applicable law or agreed to in writing, software
20+
* distributed under the License is distributed on an "AS IS" BASIS,
21+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22+
* See the License for the specific language governing permissions and
23+
* limitations under the License.
24+
*/
25+
#ifndef TESSERACT_COMMON_INTERPOLATORS_H
26+
#define TESSERACT_COMMON_INTERPOLATORS_H
27+
28+
#include <tesseract_common/macros.h>
29+
#include <tesseract_common/types.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <Eigen/Core>
32+
#include <unsupported/Eigen/Splines>
33+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
34+
35+
namespace tesseract_common
36+
{
37+
/** @brief Uses Eigen/Splines to fit a 3rd order B-Spline to the input data and provides an operator for retrieving
38+
* intermediate points
39+
*
40+
* Note: B-Spline may be lower order than requested for short input vectors */
41+
class SplineFunction
42+
{
43+
public:
44+
/**
45+
* @brief SplineFunction Constructor for interpolating while applying restrictions on the derivates of some points
46+
*
47+
* Example: Start/end with 0 velocity,
48+
* Derivatives is a VectorXd of length 2 filled with zeros;
49+
* Indices is then a VectorXi of length 2 filled with 0 and x_vec.size()-1
50+
*
51+
* **Note: There is a known bug in Eigen as of 1/6/2020. This will give incorrect results unless you patch
52+
* SplineFitting.h.** See stackoverflow.com/questions/48382939 and https://gitlab.com/libeigen/eigen/merge_requests/41
53+
* @param x_vec Independent variable. Probably time for most motion planning problems
54+
* @param y_vec Dependent variable. Probably joint values for most motion planning problems.
55+
* @param derivatives This is a vector of derivative values.
56+
* @param indices This is a vector of indices of where those derivatives are applied
57+
*/
58+
SplineFunction(const Eigen::Ref<Eigen::VectorXd>& x_vec,
59+
const Eigen::Ref<Eigen::VectorXd>& y_vec,
60+
const Eigen::Ref<Eigen::VectorXd>& derivatives,
61+
const Eigen::Ref<Eigen::VectorXi>& indices)
62+
: x_min_(x_vec.minCoeff())
63+
, x_max_(x_vec.maxCoeff())
64+
,
65+
// Scale x values to [0:1] and curve fit with a 3rd order B-Spline.
66+
spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::InterpolateWithDerivatives(
67+
y_vec.transpose(),
68+
derivatives.transpose(),
69+
indices,
70+
std::min<unsigned>(static_cast<unsigned>(x_vec.rows()) - 1, 3),
71+
scaledValues(x_vec)))
72+
{
73+
assert(derivatives.size() == indices.size());
74+
assert(!(x_vec.array().isNaN().any()));
75+
assert(!(x_vec.array().isInf().any()));
76+
assert(!(y_vec.array().isNaN().any()));
77+
assert(!(y_vec.array().isInf().any()));
78+
}
79+
80+
/**
81+
* @brief Constructor for interpolating with no restrictions on derivatives
82+
* @param x_vec Independent variable. Probably time for most motion planning problems
83+
* @param y_vec Dependent variable. Probably joint values for most motion planning problems.
84+
*/
85+
SplineFunction(const Eigen::Ref<Eigen::VectorXd>& x_vec, const Eigen::Ref<Eigen::VectorXd>& y_vec)
86+
: x_min_(x_vec.minCoeff())
87+
, x_max_(x_vec.maxCoeff())
88+
,
89+
// Scale x values to [0:1] and curve fit with a 3rd order B-Spline.
90+
spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(y_vec.transpose(),
91+
std::min<long>(x_vec.rows() - 1, 3),
92+
scaledValues(x_vec)))
93+
{
94+
assert(!(x_vec.array().isNaN().any()));
95+
assert(!(x_vec.array().isInf().any()));
96+
assert(!(y_vec.array().isNaN().any()));
97+
assert(!(y_vec.array().isInf().any()));
98+
}
99+
100+
/** @brief Returns the y value at point x in the spline */
101+
double operator()(double x) const
102+
{
103+
// x values need to be scaled to [0:1] in extraction as well.
104+
return spline_(scaledValue(x))(0);
105+
}
106+
107+
/** @brief Returns a vector of interpolated y values for each x in the input vector */
108+
Eigen::VectorXd operator()(const Eigen::Ref<Eigen::VectorXd>& x_vec) const
109+
{
110+
return x_vec.unaryExpr([this](double x) { return spline_(scaledValue(x))(0); });
111+
}
112+
113+
private:
114+
/** @brief Scales value to [0:1] based on x_min and x_max */
115+
double scaledValue(double x) const { return (x - x_min_) / (x_max_ - x_min_); }
116+
117+
/** @brief Scales vector such that each value is [0:1] based on x_min and x_max
118+
*
119+
* It is a requirement for Interpolate that x be on the interval [0:1] */
120+
Eigen::RowVectorXd scaledValues(const Eigen::Ref<Eigen::VectorXd>& x_vec) const
121+
{
122+
return x_vec.unaryExpr([this](double x) { return scaledValue(x); }).transpose();
123+
}
124+
125+
/** @brief Minimum value in x_vec. Used to scale inputs to [0:1] */
126+
double x_min_;
127+
/** @brief Maximum value in x_vec. Used to scale inputs to [0:1] */
128+
double x_max_;
129+
130+
/** @brief One dimensional Spline that is used to interpolate the data
131+
*
132+
* Note: operator(double) returns an array of points. In this case the 0th element is the y value*/
133+
Eigen::Spline<double, 1> spline_;
134+
};
135+
136+
/**
137+
* @brief Interpolates each column in a TrajArray using a cubic spline. The resuls are an evenly spaced trajectory with
138+
* result_length size.
139+
*
140+
* Note: While the spline will hit these points, the resulting TrajArray is not guaranteed to include the original
141+
* points unless result_length is of size n * (input_traj.rows() - 1) + 1
142+
* @param input_traj Input TrajArray. Each column will be interpolated with a cubic spline.
143+
* @param result_length Number of rows in the resulting TrajArray. For best results this should be size n *
144+
* (input_traj.rows() - 1) + 1
145+
* @return Resulting TrajArray of size results_length x input_traj.cols()
146+
*/
147+
inline TrajArray interpolateCubicSpline(const Eigen::Ref<TrajArray>& input_traj, const int& result_length)
148+
{
149+
TrajArray results(result_length, input_traj.cols());
150+
for (long ind = 0; ind < input_traj.cols(); ind++)
151+
{
152+
// Fit the spline to the input data
153+
Eigen::VectorXd t_in =
154+
Eigen::VectorXd::LinSpaced(input_traj.rows(), 0.0, static_cast<double>(input_traj.rows() - 1));
155+
Eigen::VectorXd y_in = input_traj.col(ind);
156+
SplineFunction spline(t_in, y_in);
157+
158+
// Extract evenly spaced points from spline
159+
Eigen::VectorXd t_out = Eigen::VectorXd::LinSpaced(result_length, 0.0, static_cast<double>(input_traj.rows() - 1));
160+
Eigen::VectorXd y_out = spline(t_out);
161+
results.col(ind) = y_out;
162+
}
163+
164+
return results;
165+
}
166+
167+
} // namespace tesseract_common
168+
169+
#endif

‎tesseract/tesseract_common/test/tesseract_common_unit.cpp‎

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
44
TESSERACT_COMMON_IGNORE_WARNINGS_POP
55

66
#include <tesseract_common/utils.h>
7+
#include <tesseract_common/interpolators.h>
78

89
TEST(TesseractCommonUnit, isNumeric) // NOLINT
910
{
@@ -49,6 +50,96 @@ TEST(TesseractCommonUnit, toNumeric) // NOLINT
4950
}
5051
}
5152

53+
TEST(TesseractCommonUnit, splineFunctionNoDerivatives) // NOLINT
54+
{
55+
Eigen::VectorXd xvals(10);
56+
Eigen::VectorXd yvals(xvals.rows());
57+
58+
xvals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;
59+
yvals = xvals.array().square();
60+
61+
// Fit the spline
62+
tesseract_common::SplineFunction spline(xvals, yvals);
63+
64+
// Check that input knots are hit exactly
65+
for (long ind = 0; ind < xvals.size(); ind++)
66+
EXPECT_NEAR(spline(xvals[ind]), yvals[ind], 1e-8);
67+
68+
Eigen::VectorXd xvals_interp(8);
69+
xvals_interp << 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5;
70+
// Check that the intermediate points are within 1% for a quadratic (they should be really close)
71+
for (long ind = 0; ind < xvals_interp.size(); ind++)
72+
{
73+
double gt = xvals_interp[ind] * xvals_interp[ind];
74+
EXPECT_NEAR(spline(xvals_interp[ind]), gt, gt * 0.01 + 1e-8);
75+
}
76+
}
77+
78+
TEST(TesseractCommonUnit, splineFunctionWithDerivatives) // NOLINT
79+
{
80+
Eigen::VectorXd xvals(10);
81+
Eigen::VectorXd yvals(xvals.rows());
82+
83+
xvals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9;
84+
yvals = xvals.array().cube();
85+
86+
Eigen::VectorXd derivatives(2);
87+
derivatives << 0., 0.;
88+
Eigen::VectorXi indices(2);
89+
indices << 0, static_cast<int>(xvals.size() - 1);
90+
91+
// Fit the spline
92+
tesseract_common::SplineFunction spline(xvals, yvals, derivatives, indices);
93+
94+
// Check that input knots are hit exactly
95+
for (long ind = 0; ind < xvals.size(); ind++)
96+
EXPECT_NEAR(spline(xvals[ind]), yvals[ind], 1e-8);
97+
98+
// Note: Interpolating using derivatives is not tested here because it requires patches to Eigen. See note in class
99+
// documentation
100+
}
101+
102+
TEST(TesseractCommonUnit, interpolateCubicSpline) // NOLINT
103+
{
104+
tesseract_common::TrajArray input_traj(10, 5);
105+
Eigen::VectorXd t_in = Eigen::VectorXd::LinSpaced(input_traj.rows(), 0.0, static_cast<double>(input_traj.rows() - 1));
106+
input_traj.col(0) = t_in.array().square();
107+
input_traj.col(1) = t_in.array().cube();
108+
input_traj.col(2) = t_in.array().sin();
109+
input_traj.col(3) = t_in.array().cos();
110+
input_traj.col(4) = t_in.array().exp();
111+
std::cout << "Input Trajectory: \n" << input_traj << std::endl;
112+
113+
const int result_length = 46;
114+
tesseract_common::TrajArray results = tesseract_common::interpolateCubicSpline(input_traj, result_length);
115+
std::cout << "Spline Results: \n" << results << std::endl;
116+
117+
EXPECT_EQ(results.rows(), result_length);
118+
EXPECT_EQ(results.cols(), input_traj.cols());
119+
120+
// Check that all of the knots are hit exactly
121+
for (long ind = 0; ind < input_traj.rows(); ind++)
122+
{
123+
EXPECT_NEAR(input_traj.col(0)[ind], results.col(0)[ind * 5], 1e-8);
124+
EXPECT_NEAR(input_traj.col(1)[ind], results.col(1)[ind * 5], 1e-8);
125+
EXPECT_NEAR(input_traj.col(2)[ind], results.col(2)[ind * 5], 1e-8);
126+
EXPECT_NEAR(input_traj.col(3)[ind], results.col(3)[ind * 5], 1e-8);
127+
EXPECT_NEAR(input_traj.col(4)[ind], results.col(4)[ind * 5], 1e-8);
128+
}
129+
130+
// Check that the intermediate points are within a small percentage. The polynomials should be really close
131+
Eigen::VectorXd t_interp =
132+
Eigen::VectorXd::LinSpaced(results.rows(), 0.0, static_cast<double>(input_traj.rows() - 1));
133+
for (long ind = 0; ind < results.rows(); ind++)
134+
{
135+
EXPECT_NEAR(t_interp.array().square()[ind], results.col(0)[ind], t_interp.array().square()[ind] * 0.01 + 1e-8);
136+
EXPECT_NEAR(t_interp.array().cube()[ind], results.col(1)[ind], t_interp.array().cube()[ind] * 0.01 + 1e-8);
137+
EXPECT_NEAR(t_interp.array().sin()[ind], results.col(2)[ind], 1.0 * 0.05 + 1e-8);
138+
EXPECT_NEAR(t_interp.array().cos()[ind], results.col(3)[ind], 1.0 * 0.05 + 1e-8);
139+
EXPECT_NEAR(t_interp.array().exp()[ind], results.col(4)[ind], t_interp.array().exp()[ind] * 0.10 + 1e-8);
140+
}
141+
}
142+
52143
int main(int argc, char** argv)
53144
{
54145
testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)