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 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #ifndef EIGEN_SPLINE_FITTING_H
00026 #define EIGEN_SPLINE_FITTING_H
00027 
00028 #include <numeric>
00029 
00030 #include "SplineFwd.h"
00031 
00032 #include <Eigen/QR>
00033 
00034 namespace Eigen
00035 {
00055   template <typename KnotVectorType>
00056   void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
00057   {
00058     typedef typename KnotVectorType::Scalar Scalar;
00059 
00060     knots.resize(parameters.size()+degree+1);      
00061 
00062     for (DenseIndex j=1; j<parameters.size()-degree; ++j)
00063       knots(j+degree) = parameters.segment(j,degree).mean();
00064 
00065     knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
00066     knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
00067   }
00068 
00078   template <typename PointArrayType, typename KnotVectorType>
00079   void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
00080   {
00081     typedef typename KnotVectorType::Scalar Scalar;
00082 
00083     const DenseIndex n = pts.cols();
00084 
00085     // 1. compute the column-wise norms
00086     chord_lengths.resize(pts.cols());
00087     chord_lengths[0] = 0;
00088     chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
00089 
00090     // 2. compute the partial sums
00091     std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
00092 
00093     // 3. normalize the data
00094     chord_lengths /= chord_lengths(n-1);
00095     chord_lengths(n-1) = Scalar(1);
00096   }
00097 
00102   template <typename SplineType>
00103   struct SplineFitting
00104   {
00105     typedef typename SplineType::KnotVectorType KnotVectorType;
00106 
00115     template <typename PointArrayType>
00116     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
00117 
00127     template <typename PointArrayType>
00128     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
00129   };
00130 
00131   template <typename SplineType>
00132   template <typename PointArrayType>
00133   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
00134   {
00135     typedef typename SplineType::KnotVectorType::Scalar Scalar;      
00136     typedef typename SplineType::BasisVectorType BasisVectorType;
00137     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;      
00138 
00139     typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
00140 
00141     KnotVectorType knots;
00142     KnotAveraging(knot_parameters, degree, knots);
00143 
00144     DenseIndex n = pts.cols();
00145     MatrixType A = MatrixType::Zero(n,n);
00146     for (DenseIndex i=1; i<n-1; ++i)
00147     {
00148       const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
00149 
00150       // The segment call should somehow be told the spline order at compile time.
00151       A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
00152     }
00153     A(0,0) = 1.0;
00154     A(n-1,n-1) = 1.0;
00155 
00156     HouseholderQR<MatrixType> qr(A);
00157 
00158     // Here, we are creating a temporary due to an Eigen issue.
00159     ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
00160 
00161     return SplineType(knots, ctrls);
00162   }
00163 
00164   template <typename SplineType>
00165   template <typename PointArrayType>
00166   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
00167   {
00168     KnotVectorType chord_lengths; // knot parameters
00169     ChordLengths(pts, chord_lengths);
00170     return Interpolate(pts, degree, chord_lengths);
00171   }
00172 }
00173 
00174 #endif // EIGEN_SPLINE_FITTING_H