Added Spline interpolation with derivatives.

This commit is contained in:
Jeff 2014-06-20 22:12:45 -06:00
parent 963d338922
commit 5dbbe6b400
4 changed files with 363 additions and 13 deletions

View File

@ -44,9 +44,15 @@ namespace Eigen
/** \brief The data type used to store knot vectors. */
typedef typename SplineTraits<Spline>::KnotVectorType KnotVectorType;
/** \brief The data type used to store parameter vectors. */
typedef typename SplineTraits<Spline>::ParameterVectorType ParameterVectorType;
/** \brief The data type used to store non-zero basis functions. */
typedef typename SplineTraits<Spline>::BasisVectorType BasisVectorType;
/** \brief The data type used to store the values of the basis function derivatives. */
typedef typename SplineTraits<Spline>::BasisDerivativeType BasisDerivativeType;
/** \brief The data type representing the spline's control points. */
typedef typename SplineTraits<Spline>::ControlPointVectorType ControlPointVectorType;
@ -203,10 +209,25 @@ namespace Eigen
**/
static BasisVectorType BasisFunctions(Scalar u, DenseIndex degree, const KnotVectorType& knots);
/**
* \copydoc Spline::basisFunctionDerivatives
* \param degree The degree of the underlying spline
* \param knots The underlying spline's knot vector.
**/
static BasisDerivativeType BasisFunctionDerivatives(
const Scalar u, const DenseIndex order, const DenseIndex degree, const KnotVectorType& knots);
private:
KnotVectorType m_knots; /*!< Knot vector. */
ControlPointVectorType m_ctrls; /*!< Control points. */
template <typename DerivativeType>
static void BasisFunctionDerivativesImpl(
const typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
const DenseIndex order,
const DenseIndex p,
const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& U,
DerivativeType& N_);
};
template <typename _Scalar, int _Dim, int _Degree>
@ -345,20 +366,24 @@ namespace Eigen
}
/* --------------------------------------------------------------------------------------------- */
template <typename SplineType, typename DerivativeType>
void basisFunctionDerivativesImpl(const SplineType& spline, typename SplineType::Scalar u, DenseIndex order, DerivativeType& N_)
template <typename _Scalar, int _Dim, int _Degree>
template <typename DerivativeType>
void Spline<_Scalar, _Dim, _Degree>::BasisFunctionDerivativesImpl(
const typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
const DenseIndex order,
const DenseIndex p,
const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& U,
DerivativeType& N_)
{
typedef Spline<_Scalar, _Dim, _Degree> SplineType;
enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
typedef typename SplineTraits<SplineType>::Scalar Scalar;
typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
typedef typename SplineTraits<SplineType>::KnotVectorType KnotVectorType;
const KnotVectorType& U = spline.knots();
const DenseIndex p = spline.degree();
const DenseIndex span = spline.span(u);
const DenseIndex span = SplineType::Span(u, p, U);
const DenseIndex n = (std::min)(p, order);
@ -455,8 +480,8 @@ namespace Eigen
typename SplineTraits< Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
{
typename SplineTraits< Spline >::BasisDerivativeType der;
basisFunctionDerivativesImpl(*this, u, order, der);
typename SplineTraits<Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType der;
BasisFunctionDerivativesImpl(u, order, degree(), knots(), der);
return der;
}
@ -465,8 +490,21 @@ namespace Eigen
typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType
Spline<_Scalar, _Dim, _Degree>::basisFunctionDerivatives(Scalar u, DenseIndex order) const
{
typename SplineTraits< Spline, DerivativeOrder >::BasisDerivativeType der;
basisFunctionDerivativesImpl(*this, u, order, der);
typename SplineTraits< Spline<_Scalar, _Dim, _Degree>, DerivativeOrder >::BasisDerivativeType der;
BasisFunctionDerivativesImpl(u, order, degree(), knots(), der);
return der;
}
template <typename _Scalar, int _Dim, int _Degree>
typename SplineTraits<Spline<_Scalar, _Dim, _Degree> >::BasisDerivativeType
Spline<_Scalar, _Dim, _Degree>::BasisFunctionDerivatives(
const typename Spline<_Scalar, _Dim, _Degree>::Scalar u,
const DenseIndex order,
const DenseIndex degree,
const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots)
{
typename SplineTraits<Spline>::BasisDerivativeType der;
BasisFunctionDerivativesImpl(u, order, degree, knots, der);
return der;
}
}

View File

@ -10,7 +10,10 @@
#ifndef EIGEN_SPLINE_FITTING_H
#define EIGEN_SPLINE_FITTING_H
#include <algorithm>
#include <functional>
#include <numeric>
#include <vector>
#include "SplineFwd.h"
@ -49,6 +52,129 @@ namespace Eigen
knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
}
/**
* \brief Computes knot averages when derivative constraints are present.
* Note that this is a technical interpretation of the referenced article
* since the algorithm contained therein is incorrect as written.
* \ingroup Splines_Module
*
* \param[in] parameters The parameters at which the interpolation B-Spline
* will intersect the given interpolation points. The parameters
* are assumed to be a non-decreasing sequence.
* \param[in] degree The degree of the interpolating B-Spline. This must be
* greater than zero.
* \param[in] derivativeIndices The indices corresponding to parameters at
* which there are derivative constraints. The indices are assumed
* to be a non-decreasing sequence.
* \param[out] knots The calculated knot vector. These will be returned as a
* non-decreasing sequence
*
* \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
* Curve interpolation with directional constraints for engineering design.
* Engineering with Computers
**/
template <typename KnotVectorType, typename ParameterVectorType>
void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
const unsigned int degree,
const IndexArray& derivativeIndices,
KnotVectorType& knots)
{
typedef typename ParameterVectorType::Scalar Scalar;
const unsigned int numParameters = parameters.size();
const unsigned int numDerivatives = derivativeIndices.size();
if (numDerivatives < 1)
{
KnotAveraging(parameters, degree, knots);
return;
}
uint startIndex;
uint endIndex;
unsigned int numInternalDerivatives = numDerivatives;
if (derivativeIndices[0] == 0)
{
startIndex = 0;
--numInternalDerivatives;
}
else
{
startIndex = 1;
}
if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
{
endIndex = numParameters - degree;
--numInternalDerivatives;
}
else
{
endIndex = numParameters - degree - 1;
}
// There are (endIndex - startIndex + 1) knots obtained from the averaging
// and 2 for the first and last parameters.
unsigned int numAverageKnots = endIndex - startIndex + 3;
KnotVectorType averageKnots(numAverageKnots);
averageKnots[0] = parameters[0];
int newKnotIndex = 0;
for (unsigned int i = startIndex; i <= endIndex; ++i)
averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
averageKnots[++newKnotIndex] = parameters[numParameters - 1];
newKnotIndex = -1;
ParameterVectorType temporaryParameters(numParameters);
KnotVectorType derivativeKnots(numInternalDerivatives);
for (unsigned int i = 0; i < numAverageKnots - 1; ++i)
{
temporaryParameters[0] = averageKnots[i];
ParameterVectorType parameterIndices(numParameters);
int temporaryParameterIndex = 1;
for (size_t j = 0; j < numParameters; ++j)
{
Scalar parameter = parameters[j];
if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
{
parameterIndices[temporaryParameterIndex] = j;
temporaryParameters[temporaryParameterIndex++] = parameter;
}
}
temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];
for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
{
for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
{
if (parameterIndices[j + 1] == derivativeIndices[k]
&& parameterIndices[j + 1] != 0
&& parameterIndices[j + 1] != numParameters - 1)
{
derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
break;
}
}
}
}
KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());
std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
temporaryKnots.data());
// Number of control points (one for each point and derivative) plus spline order.
DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
knots.resize(numKnots);
knots.head(degree).fill(temporaryKnots[0]);
knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
}
/**
* \brief Computes chord length parameters which are required for spline interpolation.
* \ingroup Splines_Module
@ -86,6 +212,7 @@ namespace Eigen
struct SplineFitting
{
typedef typename SplineType::KnotVectorType KnotVectorType;
typedef typename SplineType::ParameterVectorType ParameterVectorType;
/**
* \brief Fits an interpolating Spline to the given data points.
@ -109,6 +236,52 @@ namespace Eigen
**/
template <typename PointArrayType>
static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
/**
* \brief Fits an interpolating spline to the given data points and
* derivatives.
*
* \param points The points for which an interpolating spline will be computed.
* \param derivatives The desired derivatives of the interpolating spline at interpolation
* points.
* \param derivativeIndices An array indicating which point each derivative belongs to. This
* must be the same size as @a derivatives.
* \param degree The degree of the interpolating spline.
*
* \returns A spline interpolating @a points with @a derivatives at those points.
*
* \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
* Curve interpolation with directional constraints for engineering design.
* Engineering with Computers
**/
template <typename PointArrayType>
static SplineType InterpolateWithDerivatives(const PointArrayType& points,
const PointArrayType& derivatives,
const IndexArray& derivativeIndices,
const unsigned int degree);
/**
* \brief Fits an interpolating spline to the given data points and derivatives.
*
* \param points The points for which an interpolating spline will be computed.
* \param derivatives The desired derivatives of the interpolating spline at interpolation points.
* \param derivativeIndices An array indicating which point each derivative belongs to. This
* must be the same size as @a derivatives.
* \param degree The degree of the interpolating spline.
* \param parameters The parameters corresponding to the interpolation points.
*
* \returns A spline interpolating @a points with @a derivatives at those points.
*
* \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
* Curve interpolation with directional constraints for engineering design.
* Engineering with Computers
*/
template <typename PointArrayType>
static SplineType InterpolateWithDerivatives(const PointArrayType& points,
const PointArrayType& derivatives,
const IndexArray& derivativeIndices,
const unsigned int degree,
const ParameterVectorType& parameters);
};
template <typename SplineType>
@ -151,6 +324,106 @@ namespace Eigen
ChordLengths(pts, chord_lengths);
return Interpolate(pts, degree, chord_lengths);
}
template <typename SplineType>
template <typename PointArrayType>
SplineType
SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
const PointArrayType& derivatives,
const IndexArray& derivativeIndices,
const unsigned int degree,
const ParameterVectorType& parameters)
{
typedef typename SplineType::KnotVectorType::Scalar Scalar;
typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;
const DenseIndex n = points.cols() + derivatives.cols();
KnotVectorType knots;
KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);
// fill matrix
MatrixType A = MatrixType::Zero(n, n);
// Use these dimensions for quicker populating, then transpose for solving.
MatrixType b(points.rows(), n);
DenseIndex startRow;
DenseIndex derivativeStart;
// End derivatives.
if (derivativeIndices[0] == 0)
{
A.template block<1, 2>(1, 0) << -1, 1;
Scalar y = (knots(degree + 1) - knots(0)) / degree;
b.col(1) = y*derivatives.col(0);
startRow = 2;
derivativeStart = 1;
}
else
{
startRow = 1;
derivativeStart = 0;
}
if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
{
A.template block<1, 2>(n - 2, n - 2) << -1, 1;
Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
}
DenseIndex row = startRow;
DenseIndex derivativeIndex = derivativeStart;
for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
{
const DenseIndex span = SplineType::Span(parameters[i], degree, knots);
if (derivativeIndices[derivativeIndex] == i)
{
A.block(row, span - degree, 2, degree + 1)
= SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);
b.col(row++) = points.col(i);
b.col(row++) = derivatives.col(derivativeIndex++);
}
else
{
A.row(row++).segment(span - degree, degree + 1)
= SplineType::BasisFunctions(parameters[i], degree, knots);
}
}
b.col(0) = points.col(0);
b.col(b.cols() - 1) = points.col(points.cols() - 1);
A(0,0) = 1;
A(n - 1, n - 1) = 1;
// Solve
HouseholderQR<MatrixType> qr(A);
ControlPointVectorType controlPoints = qr.solve(MatrixType(b.transpose())).transpose();
SplineType spline(knots, controlPoints);
return spline;
}
template <typename SplineType>
template <typename PointArrayType>
SplineType
SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
const PointArrayType& derivatives,
const IndexArray& derivativeIndices,
const unsigned int degree)
{
ParameterVectorType parameters;
ChordLengths(points, parameters);
return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
}
}
#endif // EIGEN_SPLINE_FITTING_H

View File

@ -48,6 +48,9 @@ namespace Eigen
/** \brief The data type used to store knot vectors. */
typedef Array<Scalar,1,Dynamic> KnotVectorType;
/** \brief The data type used to store parameter vectors. */
typedef Array<Scalar,1,Dynamic> ParameterVectorType;
/** \brief The data type representing the spline's control points. */
typedef Array<Scalar,Dimension,Dynamic> ControlPointVectorType;
@ -85,6 +88,8 @@ namespace Eigen
/** \brief 3D double B-spline with dynamic degree. */
typedef Spline<double,3> Spline3d;
typedef Array<DenseIndex, 1, Dynamic> IndexArray;
}
#endif // EIGEN_SPLINES_FWD_H

View File

@ -234,6 +234,39 @@ void check_global_interpolation2d()
}
}
void check_global_interpolation_with_derivatives2d()
{
typedef Spline2d::PointType PointType;
typedef Spline2d::KnotVectorType KnotVectorType;
const unsigned int numPoints = 100;
const unsigned int dimension = 2;
const unsigned int degree = 3;
ArrayXXd points = ArrayXXd::Random(dimension, numPoints);
KnotVectorType knots;
Eigen::ChordLengths(points, knots);
ArrayXXd derivatives = ArrayXXd::Random(dimension, numPoints);
Eigen::IndexArray derivativeIndices(numPoints);
for (Eigen::DenseIndex i = 0; i < numPoints; ++i)
derivativeIndices(i) = i;
const Spline2d spline = SplineFitting<Spline2d>::InterpolateWithDerivatives(
points, derivatives, derivativeIndices, degree);
for (Eigen::DenseIndex i = 0; i < points.cols(); ++i)
{
PointType point = spline(knots(i));
PointType referencePoint = points.col(i);
VERIFY((point - referencePoint).matrix().norm() < 1e-12);
PointType derivative = spline.derivatives(knots(i), 1).col(1);
PointType referenceDerivative = derivatives.col(i);
VERIFY((derivative - referenceDerivative).matrix().norm() < 1e-10);
}
}
void test_splines()
{
@ -241,4 +274,5 @@ void test_splines()
CALL_SUBTEST( eval_spline3d_onbrks() );
CALL_SUBTEST( eval_closed_spline2d() );
CALL_SUBTEST( check_global_interpolation2d() );
CALL_SUBTEST( check_global_interpolation_with_derivatives2d() );
}