SplineFitting.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_SPLINE_FITTING_H
26 #define EIGEN_SPLINE_FITTING_H
27 
28 #include <numeric>
29 
30 #include "SplineFwd.h"
31 
32 #include <Eigen/QR>
33 
34 namespace Eigen
35 {
55  template <typename KnotVectorType>
56  void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
57  {
58  typedef typename KnotVectorType::Scalar Scalar;
59 
60  knots.resize(parameters.size()+degree+1);
61 
62  for (DenseIndex j=1; j<parameters.size()-degree; ++j)
63  knots(j+degree) = parameters.segment(j,degree).mean();
64 
65  knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
66  knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
67  }
68 
78  template <typename PointArrayType, typename KnotVectorType>
79  void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
80  {
81  typedef typename KnotVectorType::Scalar Scalar;
82 
83  const DenseIndex n = pts.cols();
84 
85  // 1. compute the column-wise norms
86  chord_lengths.resize(pts.cols());
87  chord_lengths[0] = 0;
88  chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
89 
90  // 2. compute the partial sums
91  std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
92 
93  // 3. normalize the data
94  chord_lengths /= chord_lengths(n-1);
95  chord_lengths(n-1) = Scalar(1);
96  }
97 
102  template <typename SplineType>
104  {
105  typedef typename SplineType::KnotVectorType KnotVectorType;
106 
115  template <typename PointArrayType>
116  static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
117 
127  template <typename PointArrayType>
128  static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
129  };
130 
131  template <typename SplineType>
132  template <typename PointArrayType>
133  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
134  {
135  typedef typename SplineType::KnotVectorType::Scalar Scalar;
136  typedef typename SplineType::BasisVectorType BasisVectorType;
137  typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
138 
139  typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
140 
141  KnotVectorType knots;
142  KnotAveraging(knot_parameters, degree, knots);
143 
144  DenseIndex n = pts.cols();
145  MatrixType A = MatrixType::Zero(n,n);
146  for (DenseIndex i=1; i<n-1; ++i)
147  {
148  const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
149 
150  // The segment call should somehow be told the spline order at compile time.
151  A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
152  }
153  A(0,0) = 1.0;
154  A(n-1,n-1) = 1.0;
155 
156  HouseholderQR<MatrixType> qr(A);
157 
158  // Here, we are creating a temporary due to an Eigen issue.
159  ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
160 
161  return SplineType(knots, ctrls);
162  }
163 
164  template <typename SplineType>
165  template <typename PointArrayType>
166  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
167  {
168  KnotVectorType chord_lengths; // knot parameters
169  ChordLengths(pts, chord_lengths);
170  return Interpolate(pts, degree, chord_lengths);
171  }
172 }
173 
174 #endif // EIGEN_SPLINE_FITTING_H