SplineFitting.h
Go to the documentation of this file.
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 <numeric>
00014 
00015 #include "SplineFwd.h"
00016 
00017 #include <Eigen/QR>
00018 
00019 namespace Eigen
00020 {
00040   template <typename KnotVectorType>
00041   void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
00042   {
00043     knots.resize(parameters.size()+degree+1);      
00044 
00045     for (DenseIndex j=1; j<parameters.size()-degree; ++j)
00046       knots(j+degree) = parameters.segment(j,degree).mean();
00047 
00048     knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
00049     knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
00050   }
00051 
00061   template <typename PointArrayType, typename KnotVectorType>
00062   void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
00063   {
00064     typedef typename KnotVectorType::Scalar Scalar;
00065 
00066     const DenseIndex n = pts.cols();
00067 
00068     // 1. compute the column-wise norms
00069     chord_lengths.resize(pts.cols());
00070     chord_lengths[0] = 0;
00071     chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
00072 
00073     // 2. compute the partial sums
00074     std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
00075 
00076     // 3. normalize the data
00077     chord_lengths /= chord_lengths(n-1);
00078     chord_lengths(n-1) = Scalar(1);
00079   }
00080 
00085   template <typename SplineType>
00086   struct SplineFitting
00087   {
00088     typedef typename SplineType::KnotVectorType KnotVectorType;
00089 
00098     template <typename PointArrayType>
00099     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
00100 
00110     template <typename PointArrayType>
00111     static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
00112   };
00113 
00114   template <typename SplineType>
00115   template <typename PointArrayType>
00116   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
00117   {
00118     typedef typename SplineType::KnotVectorType::Scalar Scalar;      
00119     typedef typename SplineType::ControlPointVectorType ControlPointVectorType;      
00120 
00121     typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
00122 
00123     KnotVectorType knots;
00124     KnotAveraging(knot_parameters, degree, knots);
00125 
00126     DenseIndex n = pts.cols();
00127     MatrixType A = MatrixType::Zero(n,n);
00128     for (DenseIndex i=1; i<n-1; ++i)
00129     {
00130       const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
00131 
00132       // The segment call should somehow be told the spline order at compile time.
00133       A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
00134     }
00135     A(0,0) = 1.0;
00136     A(n-1,n-1) = 1.0;
00137 
00138     HouseholderQR<MatrixType> qr(A);
00139 
00140     // Here, we are creating a temporary due to an Eigen issue.
00141     ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
00142 
00143     return SplineType(knots, ctrls);
00144   }
00145 
00146   template <typename SplineType>
00147   template <typename PointArrayType>
00148   SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
00149   {
00150     KnotVectorType chord_lengths; // knot parameters
00151     ChordLengths(pts, chord_lengths);
00152     return Interpolate(pts, degree, chord_lengths);
00153   }
00154 }
00155 
00156 #endif // EIGEN_SPLINE_FITTING_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:39:43