mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-01-24 14:45:14 +08:00
Added Spline interpolation with derivatives.
This commit is contained in:
parent
963d338922
commit
5dbbe6b400
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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() );
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user