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 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_SPLINE_FITTING_H
11 #define EIGEN_SPLINE_FITTING_H
12 
13 #include <numeric>
14 
15 #include "SplineFwd.h"
16 
17 #include <Eigen/QR>
18 
19 namespace Eigen
20 {
40  template <typename KnotVectorType>
41  void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
42  {
43  typedef typename KnotVectorType::Scalar Scalar;
44 
45  knots.resize(parameters.size()+degree+1);
46 
47  for (DenseIndex j=1; j<parameters.size()-degree; ++j)
48  knots(j+degree) = parameters.segment(j,degree).mean();
49 
50  knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
51  knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
52  }
53 
63  template <typename PointArrayType, typename KnotVectorType>
64  void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
65  {
66  typedef typename KnotVectorType::Scalar Scalar;
67 
68  const DenseIndex n = pts.cols();
69 
70  // 1. compute the column-wise norms
71  chord_lengths.resize(pts.cols());
72  chord_lengths[0] = 0;
73  chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
74 
75  // 2. compute the partial sums
76  std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
77 
78  // 3. normalize the data
79  chord_lengths /= chord_lengths(n-1);
80  chord_lengths(n-1) = Scalar(1);
81  }
82 
87  template <typename SplineType>
89  {
90  typedef typename SplineType::KnotVectorType KnotVectorType;
91 
100  template <typename PointArrayType>
101  static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
102 
112  template <typename PointArrayType>
113  static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
114  };
115 
116  template <typename SplineType>
117  template <typename PointArrayType>
118  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
119  {
120  typedef typename SplineType::KnotVectorType::Scalar Scalar;
121  typedef typename SplineType::BasisVectorType BasisVectorType;
122  typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
123 
124  typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
125 
126  KnotVectorType knots;
127  KnotAveraging(knot_parameters, degree, knots);
128 
129  DenseIndex n = pts.cols();
130  MatrixType A = MatrixType::Zero(n,n);
131  for (DenseIndex i=1; i<n-1; ++i)
132  {
133  const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
134 
135  // The segment call should somehow be told the spline order at compile time.
136  A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
137  }
138  A(0,0) = 1.0;
139  A(n-1,n-1) = 1.0;
140 
141  HouseholderQR<MatrixType> qr(A);
142 
143  // Here, we are creating a temporary due to an Eigen issue.
144  ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
145 
146  return SplineType(knots, ctrls);
147  }
148 
149  template <typename SplineType>
150  template <typename PointArrayType>
151  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
152  {
153  KnotVectorType chord_lengths; // knot parameters
154  ChordLengths(pts, chord_lengths);
155  return Interpolate(pts, degree, chord_lengths);
156  }
157 }
158 
159 #endif // EIGEN_SPLINE_FITTING_H