SplineFitting.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_SPLINE_FITTING_H
00011 #define EIGEN_SPLINE_FITTING_H
00012 
00013 #include <algorithm>
00014 #include <functional>
00015 #include <numeric>
00016 #include <vector>
00017 
00018 #include "SplineFwd.h"
00019 
00020 #include <Eigen/LU>
00021 #include <Eigen/QR>
00022 
00023 namespace Eigen
00024 {
00044   template <typename KnotVectorType>
00045   void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
00046   {
00047     knots.resize(parameters.size()+degree+1);      
00048 
00049     for (DenseIndex j=1; j<parameters.size()-degree; ++j)
00050       knots(j+degree) = parameters.segment(j,degree).mean();
00051 
00052     knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
00053     knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
00054   }
00055 
00077   template <typename KnotVectorType, typename ParameterVectorType, typename IndexArray>
00078   void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
00079                                     const unsigned int degree,
00080                                     const IndexArray& derivativeIndices,
00081                                     KnotVectorType& knots)
00082   {
00083     typedef typename ParameterVectorType::Scalar Scalar;
00084 
00085     DenseIndex numParameters = parameters.size();
00086     DenseIndex numDerivatives = derivativeIndices.size();
00087 
00088     if (numDerivatives < 1)
00089     {
00090       KnotAveraging(parameters, degree, knots);
00091       return;
00092     }
00093 
00094     DenseIndex startIndex;
00095     DenseIndex endIndex;
00096   
00097     DenseIndex numInternalDerivatives = numDerivatives;
00098     
00099     if (derivativeIndices[0] == 0)
00100     {
00101       startIndex = 0;
00102       --numInternalDerivatives;
00103     }
00104     else
00105     {
00106       startIndex = 1;
00107     }
00108     if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
00109     {
00110       endIndex = numParameters - degree;
00111       --numInternalDerivatives;
00112     }
00113     else
00114     {
00115       endIndex = numParameters - degree - 1;
00116     }
00117 
00118     // There are (endIndex - startIndex + 1) knots obtained from the averaging
00119     // and 2 for the first and last parameters.
00120     DenseIndex numAverageKnots = endIndex - startIndex + 3;
00121     KnotVectorType averageKnots(numAverageKnots);
00122     averageKnots[0] = parameters[0];
00123 
00124     int newKnotIndex = 0;
00125     for (DenseIndex i = startIndex; i <= endIndex; ++i)
00126       averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
00127     averageKnots[++newKnotIndex] = parameters[numParameters - 1];
00128 
00129     newKnotIndex = -1;
00130   
00131     ParameterVectorType temporaryParameters(numParameters + 1);
00132     KnotVectorType derivativeKnots(numInternalDerivatives);
00133     for (DenseIndex i = 0; i < numAverageKnots - 1; ++i)
00134     {
00135       temporaryParameters[0] = averageKnots[i];
00136       ParameterVectorType parameterIndices(numParameters);
00137       int temporaryParameterIndex = 1;
00138       for (DenseIndex j = 0; j < numParameters; ++j)
00139       {
00140         Scalar parameter = parameters[j];
00141         if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
00142         {
00143           parameterIndices[temporaryParameterIndex] = j;
00144           temporaryParameters[temporaryParameterIndex++] = parameter;
00145         }
00146       }
00147       temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];
00148 
00149       for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
00150       {
00151         for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
00152         {
00153           if (parameterIndices[j + 1] == derivativeIndices[k]
00154               && parameterIndices[j + 1] != 0
00155               && parameterIndices[j + 1] != numParameters - 1)
00156           {
00157             derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
00158             break;
00159           }
00160         }
00161       }
00162     }
00163     
00164     KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());
00165 
00166     std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
00167                derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
00168                temporaryKnots.data());
00169 
00170     // Number of knots (one for each point and derivative) plus spline order.
00171     DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
00172     knots.resize(numKnots);
00173 
00174     knots.head(degree).fill(temporaryKnots[0]);
00175     knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
00176     knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
00177   }
00178 
00188   template <typename PointArrayType, typename KnotVectorType>
00189   void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
00190   {
00191     typedef typename KnotVectorType::Scalar Scalar;
00192 
00193     const DenseIndex n = pts.cols();
00194 
00195     // 1. compute the column-wise norms
00196     chord_lengths.resize(pts.cols());
00197     chord_lengths[0] = 0;
00198     chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
00199 
00200     // 2. compute the partial sums
00201     std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
00202 
00203     // 3. normalize the data
00204     chord_lengths /= chord_lengths(n-1);
00205     chord_lengths(n-1) = Scalar(1);
00206   }
00207 
00212   template <typename SplineType>
00213   struct SplineFitting
00214   {
00215     typedef typename SplineType::KnotVectorType KnotVectorType;
00216     typedef typename SplineType::ParameterVectorType ParameterVectorType;
00217 
00226     template <typename PointArrayType>
00227     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
00228 
00238     template <typename PointArrayType>
00239     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
00240 
00258     template <typename PointArrayType, typename IndexArray>
00259     static SplineType InterpolateWithDerivatives(const PointArrayType& points,
00260                                                  const PointArrayType& derivatives,
00261                                                  const IndexArray& derivativeIndices,
00262                                                  const unsigned int degree);
00263 
00280     template <typename PointArrayType, typename IndexArray>
00281     static SplineType InterpolateWithDerivatives(const PointArrayType& points,
00282                                                  const PointArrayType& derivatives,
00283                                                  const IndexArray& derivativeIndices,
00284                                                  const unsigned int degree,
00285                                                  const ParameterVectorType& parameters);
00286   };
00287 
00288   template <typename SplineType>
00289   template <typename PointArrayType>
00290   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
00291   {
00292     typedef typename SplineType::KnotVectorType::Scalar Scalar;      
00293     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;      
00294 
00295     typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
00296 
00297     KnotVectorType knots;
00298     KnotAveraging(knot_parameters, degree, knots);
00299 
00300     DenseIndex n = pts.cols();
00301     MatrixType A = MatrixType::Zero(n,n);
00302     for (DenseIndex i=1; i<n-1; ++i)
00303     {
00304       const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
00305 
00306       // The segment call should somehow be told the spline order at compile time.
00307       A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
00308     }
00309     A(0,0) = 1.0;
00310     A(n-1,n-1) = 1.0;
00311 
00312     HouseholderQR<MatrixType> qr(A);
00313 
00314     // Here, we are creating a temporary due to an Eigen issue.
00315     ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
00316 
00317     return SplineType(knots, ctrls);
00318   }
00319 
00320   template <typename SplineType>
00321   template <typename PointArrayType>
00322   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
00323   {
00324     KnotVectorType chord_lengths; // knot parameters
00325     ChordLengths(pts, chord_lengths);
00326     return Interpolate(pts, degree, chord_lengths);
00327   }
00328   
00329   template <typename SplineType>
00330   template <typename PointArrayType, typename IndexArray>
00331   SplineType 
00332   SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
00333                                                         const PointArrayType& derivatives,
00334                                                         const IndexArray& derivativeIndices,
00335                                                         const unsigned int degree,
00336                                                         const ParameterVectorType& parameters)
00337   {
00338     typedef typename SplineType::KnotVectorType::Scalar Scalar;      
00339     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
00340 
00341     typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;
00342 
00343     const DenseIndex n = points.cols() + derivatives.cols();
00344     
00345     KnotVectorType knots;
00346 
00347     KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);
00348     
00349     // fill matrix
00350     MatrixType A = MatrixType::Zero(n, n);
00351 
00352     // Use these dimensions for quicker populating, then transpose for solving.
00353     MatrixType b(points.rows(), n);
00354 
00355     DenseIndex startRow;
00356     DenseIndex derivativeStart;
00357 
00358     // End derivatives.
00359     if (derivativeIndices[0] == 0)
00360     {
00361       A.template block<1, 2>(1, 0) << -1, 1;
00362       
00363       Scalar y = (knots(degree + 1) - knots(0)) / degree;
00364       b.col(1) = y*derivatives.col(0);
00365       
00366       startRow = 2;
00367       derivativeStart = 1;
00368     }
00369     else
00370     {
00371       startRow = 1;
00372       derivativeStart = 0;
00373     }
00374     if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
00375     {
00376       A.template block<1, 2>(n - 2, n - 2) << -1, 1;
00377 
00378       Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
00379       b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
00380     }
00381     
00382     DenseIndex row = startRow;
00383     DenseIndex derivativeIndex = derivativeStart;
00384     for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
00385     {
00386       const DenseIndex span = SplineType::Span(parameters[i], degree, knots);
00387 
00388       if (derivativeIndices[derivativeIndex] == i)
00389       {
00390         A.block(row, span - degree, 2, degree + 1)
00391           = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);
00392 
00393         b.col(row++) = points.col(i);
00394         b.col(row++) = derivatives.col(derivativeIndex++);
00395       }
00396       else
00397       {
00398         A.row(row++).segment(span - degree, degree + 1)
00399           = SplineType::BasisFunctions(parameters[i], degree, knots);
00400       }
00401     }
00402     b.col(0) = points.col(0);
00403     b.col(b.cols() - 1) = points.col(points.cols() - 1);
00404     A(0,0) = 1;
00405     A(n - 1, n - 1) = 1;
00406     
00407     // Solve
00408     FullPivLU<MatrixType> lu(A);
00409     ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose();
00410 
00411     SplineType spline(knots, controlPoints);
00412     
00413     return spline;
00414   }
00415   
00416   template <typename SplineType>
00417   template <typename PointArrayType, typename IndexArray>
00418   SplineType
00419   SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
00420                                                         const PointArrayType& derivatives,
00421                                                         const IndexArray& derivativeIndices,
00422                                                         const unsigned int degree)
00423   {
00424     ParameterVectorType parameters;
00425     ChordLengths(points, parameters);
00426     return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
00427   }
00428 }
00429 
00430 #endif // EIGEN_SPLINE_FITTING_H
 All Classes Functions Variables Typedefs Enumerator