src/Splines/SplineFitting.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
0005 //
0006 // This Source Code Form is subject to the terms of the Mozilla
0007 // Public License v. 2.0. If a copy of the MPL was not distributed
0008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0009
0010 #ifndef EIGEN_SPLINE_FITTING_H
0011 #define EIGEN_SPLINE_FITTING_H
0012
0013 #include <algorithm>
0014 #include <functional>
0015 #include <numeric>
0016 #include <vector>
0017
0018 #include "SplineFwd.h"
0019
0020 #include "../../../../Eigen/LU"
0021 #include "../../../../Eigen/QR"
0022
0023 namespace Eigen
0024 {
0025   /**
0026    * \brief Computes knot averages.
0027    * \ingroup Splines_Module
0028    *
0029    * The knots are computed as
0030    * \f{align*}
0031    *  u_0 & = \hdots = u_p = 0 \\
0032    *  u_{m-p} & = \hdots = u_{m} = 1 \\
0033    *  u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p
0034    * \f}
0035    * where \f$p\f$ is the degree and \f$m+1\f$ the number knots
0036    * of the desired interpolating spline.
0037    *
0038    * \param[in] parameters The input parameters. During interpolation one for each data point.
0039    * \param[in] degree The spline degree which is used during the interpolation.
0040    * \param[out] knots The output knot vector.
0041    *
0042    * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
0043    **/
0044   template <typename KnotVectorType>
0045   void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
0046   {
0047     knots.resize(parameters.size()+degree+1);
0048
0049     for (DenseIndex j=1; j<parameters.size()-degree; ++j)
0050       knots(j+degree) = parameters.segment(j,degree).mean();
0051
0052     knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
0053     knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
0054   }
0055
0056   /**
0057    * \brief Computes knot averages when derivative constraints are present.
0058    * Note that this is a technical interpretation of the referenced article
0059    * since the algorithm contained therein is incorrect as written.
0060    * \ingroup Splines_Module
0061    *
0062    * \param[in] parameters The parameters at which the interpolation B-Spline
0063    *            will intersect the given interpolation points. The parameters
0064    *            are assumed to be a non-decreasing sequence.
0065    * \param[in] degree The degree of the interpolating B-Spline. This must be
0066    *            greater than zero.
0067    * \param[in] derivativeIndices The indices corresponding to parameters at
0068    *            which there are derivative constraints. The indices are assumed
0069    *            to be a non-decreasing sequence.
0070    * \param[out] knots The calculated knot vector. These will be returned as a
0071    *             non-decreasing sequence
0072    *
0073    * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
0074    * Curve interpolation with directional constraints for engineering design.
0075    * Engineering with Computers
0076    **/
0077   template <typename KnotVectorType, typename ParameterVectorType, typename IndexArray>
0078   void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
0079                                     const unsigned int degree,
0080                                     const IndexArray& derivativeIndices,
0081                                     KnotVectorType& knots)
0082   {
0083     typedef typename ParameterVectorType::Scalar Scalar;
0084
0085     DenseIndex numParameters = parameters.size();
0086     DenseIndex numDerivatives = derivativeIndices.size();
0087
0088     if (numDerivatives < 1)
0089     {
0090       KnotAveraging(parameters, degree, knots);
0091       return;
0092     }
0093
0094     DenseIndex startIndex;
0095     DenseIndex endIndex;
0096
0097     DenseIndex numInternalDerivatives = numDerivatives;
0098
0099     if (derivativeIndices[0] == 0)
0100     {
0101       startIndex = 0;
0102       --numInternalDerivatives;
0103     }
0104     else
0105     {
0106       startIndex = 1;
0107     }
0108     if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
0109     {
0110       endIndex = numParameters - degree;
0111       --numInternalDerivatives;
0112     }
0113     else
0114     {
0115       endIndex = numParameters - degree - 1;
0116     }
0117
0118     // There are (endIndex - startIndex + 1) knots obtained from the averaging
0119     // and 2 for the first and last parameters.
0120     DenseIndex numAverageKnots = endIndex - startIndex + 3;
0121     KnotVectorType averageKnots(numAverageKnots);
0122     averageKnots[0] = parameters[0];
0123
0124     int newKnotIndex = 0;
0125     for (DenseIndex i = startIndex; i <= endIndex; ++i)
0126       averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
0127     averageKnots[++newKnotIndex] = parameters[numParameters - 1];
0128
0129     newKnotIndex = -1;
0130
0131     ParameterVectorType temporaryParameters(numParameters + 1);
0132     KnotVectorType derivativeKnots(numInternalDerivatives);
0133     for (DenseIndex i = 0; i < numAverageKnots - 1; ++i)
0134     {
0135       temporaryParameters[0] = averageKnots[i];
0136       ParameterVectorType parameterIndices(numParameters);
0137       int temporaryParameterIndex = 1;
0138       for (DenseIndex j = 0; j < numParameters; ++j)
0139       {
0140         Scalar parameter = parameters[j];
0141         if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
0142         {
0143           parameterIndices[temporaryParameterIndex] = j;
0144           temporaryParameters[temporaryParameterIndex++] = parameter;
0145         }
0146       }
0147       temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];
0148
0149       for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
0150       {
0151         for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
0152         {
0153           if (parameterIndices[j + 1] == derivativeIndices[k]
0154               && parameterIndices[j + 1] != 0
0155               && parameterIndices[j + 1] != numParameters - 1)
0156           {
0157             derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
0158             break;
0159           }
0160         }
0161       }
0162     }
0163
0164     KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());
0165
0166     std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
0167                derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
0168                temporaryKnots.data());
0169
0170     // Number of knots (one for each point and derivative) plus spline order.
0171     DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
0172     knots.resize(numKnots);
0173
0174     knots.head(degree).fill(temporaryKnots[0]);
0175     knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
0176     knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
0177   }
0178
0179   /**
0180    * \brief Computes chord length parameters which are required for spline interpolation.
0181    * \ingroup Splines_Module
0182    *
0183    * \param[in] pts The data points to which a spline should be fit.
0184    * \param[out] chord_lengths The resulting chord length vector.
0185    *
0186    * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
0187    **/
0188   template <typename PointArrayType, typename KnotVectorType>
0189   void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
0190   {
0191     typedef typename KnotVectorType::Scalar Scalar;
0192
0193     const DenseIndex n = pts.cols();
0194
0195     // 1. compute the column-wise norms
0196     chord_lengths.resize(pts.cols());
0197     chord_lengths[0] = 0;
0198     chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
0199
0200     // 2. compute the partial sums
0201     std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
0202
0203     // 3. normalize the data
0204     chord_lengths /= chord_lengths(n-1);
0205     chord_lengths(n-1) = Scalar(1);
0206   }
0207
0208   /**
0209    * \brief Spline fitting methods.
0210    * \ingroup Splines_Module
0211    **/
0212   template <typename SplineType>
0213   struct SplineFitting
0214   {
0215     typedef typename SplineType::KnotVectorType KnotVectorType;
0216     typedef typename SplineType::ParameterVectorType ParameterVectorType;
0217
0218     /**
0219      * \brief Fits an interpolating Spline to the given data points.
0220      *
0221      * \param pts The points for which an interpolating spline will be computed.
0222      * \param degree The degree of the interpolating spline.
0223      *
0224      * \returns A spline interpolating the initially provided points.
0225      **/
0226     template <typename PointArrayType>
0227     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
0228
0229     /**
0230      * \brief Fits an interpolating Spline to the given data points.
0231      *
0232      * \param pts The points for which an interpolating spline will be computed.
0233      * \param degree The degree of the interpolating spline.
0234      * \param knot_parameters The knot parameters for the interpolation.
0235      *
0236      * \returns A spline interpolating the initially provided points.
0237      **/
0238     template <typename PointArrayType>
0239     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
0240
0241     /**
0242      * \brief Fits an interpolating spline to the given data points and
0243      * derivatives.
0244      *
0245      * \param points The points for which an interpolating spline will be computed.
0246      * \param derivatives The desired derivatives of the interpolating spline at interpolation
0247      *                    points.
0248      * \param derivativeIndices An array indicating which point each derivative belongs to. This
0249      *                          must be the same size as @a derivatives.
0250      * \param degree The degree of the interpolating spline.
0251      *
0252      * \returns A spline interpolating @a points with @a derivatives at those points.
0253      *
0254      * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
0255      * Curve interpolation with directional constraints for engineering design.
0256      * Engineering with Computers
0257      **/
0258     template <typename PointArrayType, typename IndexArray>
0259     static SplineType InterpolateWithDerivatives(const PointArrayType& points,
0260                                                  const PointArrayType& derivatives,
0261                                                  const IndexArray& derivativeIndices,
0262                                                  const unsigned int degree);
0263
0264     /**
0265      * \brief Fits an interpolating spline to the given data points and derivatives.
0266      *
0267      * \param points The points for which an interpolating spline will be computed.
0268      * \param derivatives The desired derivatives of the interpolating spline at interpolation points.
0269      * \param derivativeIndices An array indicating which point each derivative belongs to. This
0270      *                          must be the same size as @a derivatives.
0271      * \param degree The degree of the interpolating spline.
0272      * \param parameters The parameters corresponding to the interpolation points.
0273      *
0274      * \returns A spline interpolating @a points with @a derivatives at those points.
0275      *
0276      * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
0277      * Curve interpolation with directional constraints for engineering design.
0278      * Engineering with Computers
0279      */
0280     template <typename PointArrayType, typename IndexArray>
0281     static SplineType InterpolateWithDerivatives(const PointArrayType& points,
0282                                                  const PointArrayType& derivatives,
0283                                                  const IndexArray& derivativeIndices,
0284                                                  const unsigned int degree,
0285                                                  const ParameterVectorType& parameters);
0286   };
0287
0288   template <typename SplineType>
0289   template <typename PointArrayType>
0290   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
0291   {
0292     typedef typename SplineType::KnotVectorType::Scalar Scalar;
0293     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
0294
0295     typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
0296
0297     KnotVectorType knots;
0298     KnotAveraging(knot_parameters, degree, knots);
0299
0300     DenseIndex n = pts.cols();
0301     MatrixType A = MatrixType::Zero(n,n);
0302     for (DenseIndex i=1; i<n-1; ++i)
0303     {
0304       const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
0305
0306       // The segment call should somehow be told the spline order at compile time.
0307       A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
0308     }
0309     A(0,0) = 1.0;
0310     A(n-1,n-1) = 1.0;
0311
0312     HouseholderQR<MatrixType> qr(A);
0313
0314     // Here, we are creating a temporary due to an Eigen issue.
0315     ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
0316
0317     return SplineType(knots, ctrls);
0318   }
0319
0320   template <typename SplineType>
0321   template <typename PointArrayType>
0322   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
0323   {
0324     KnotVectorType chord_lengths; // knot parameters
0325     ChordLengths(pts, chord_lengths);
0326     return Interpolate(pts, degree, chord_lengths);
0327   }
0328
0329   template <typename SplineType>
0330   template <typename PointArrayType, typename IndexArray>
0331   SplineType
0332   SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
0333                                                         const PointArrayType& derivatives,
0334                                                         const IndexArray& derivativeIndices,
0335                                                         const unsigned int degree,
0336                                                         const ParameterVectorType& parameters)
0337   {
0338     typedef typename SplineType::KnotVectorType::Scalar Scalar;
0339     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
0340
0341     typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;
0342
0343     const DenseIndex n = points.cols() + derivatives.cols();
0344
0345     KnotVectorType knots;
0346
0347     KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);
0348
0349     // fill matrix
0350     MatrixType A = MatrixType::Zero(n, n);
0351
0352     // Use these dimensions for quicker populating, then transpose for solving.
0353     MatrixType b(points.rows(), n);
0354
0355     DenseIndex startRow;
0356     DenseIndex derivativeStart;
0357
0358     // End derivatives.
0359     if (derivativeIndices[0] == 0)
0360     {
0361       A.template block<1, 2>(1, 0) << -1, 1;
0362
0363       Scalar y = (knots(degree + 1) - knots(0)) / degree;
0364       b.col(1) = y*derivatives.col(0);
0365
0366       startRow = 2;
0367       derivativeStart = 1;
0368     }
0369     else
0370     {
0371       startRow = 1;
0372       derivativeStart = 0;
0373     }
0374     if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
0375     {
0376       A.template block<1, 2>(n - 2, n - 2) << -1, 1;
0377
0378       Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
0379       b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
0380     }
0381
0382     DenseIndex row = startRow;
0383     DenseIndex derivativeIndex = derivativeStart;
0384     for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
0385     {
0386       const DenseIndex span = SplineType::Span(parameters[i], degree, knots);
0387
0388       if (derivativeIndex < derivativeIndices.size() && derivativeIndices[derivativeIndex] == i)
0389       {
0390         A.block(row, span - degree, 2, degree + 1)
0391           = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);
0392
0393         b.col(row++) = points.col(i);
0394         b.col(row++) = derivatives.col(derivativeIndex++);
0395       }
0396       else
0397       {
0398         A.row(row).segment(span - degree, degree + 1)
0399           = SplineType::BasisFunctions(parameters[i], degree, knots);
0400         b.col(row++) = points.col(i);
0401       }
0402     }
0403     b.col(0) = points.col(0);
0404     b.col(b.cols() - 1) = points.col(points.cols() - 1);
0405     A(0,0) = 1;
0406     A(n - 1, n - 1) = 1;
0407
0408     // Solve
0409     FullPivLU<MatrixType> lu(A);
0410     ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose();
0411
0412     SplineType spline(knots, controlPoints);
0413
0414     return spline;
0415   }
0416
0417   template <typename SplineType>
0418   template <typename PointArrayType, typename IndexArray>
0419   SplineType
0420   SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
0421                                                         const PointArrayType& derivatives,
0422                                                         const IndexArray& derivativeIndices,
0423                                                         const unsigned int degree)
0424   {
0425     ParameterVectorType parameters;
0426     ChordLengths(points, parameters);
0427     return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
0428   }
0429 }
0430
0431 #endif // EIGEN_SPLINE_FITTING_H