SplineFitting.h
Go to the documentation of this file.
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 <algorithm>
14 #include <functional>
15 #include <numeric>
16 #include <vector>
17 
18 #include "SplineFwd.h"
19 
20 #include "../../../../Eigen/LU"
21 #include "../../../../Eigen/QR"
22 
23 namespace Eigen
24 {
44  template <typename KnotVectorType>
45  void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
46  {
47  knots.resize(parameters.size()+degree+1);
48 
49  for (DenseIndex j=1; j<parameters.size()-degree; ++j)
50  knots(j+degree) = parameters.segment(j,degree).mean();
51 
52  knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
53  knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
54  }
55 
77  template <typename KnotVectorType, typename ParameterVectorType, typename IndexArray>
78  void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
79  const unsigned int degree,
80  const IndexArray& derivativeIndices,
81  KnotVectorType& knots)
82  {
83  typedef typename ParameterVectorType::Scalar Scalar;
84 
85  DenseIndex numParameters = parameters.size();
86  DenseIndex numDerivatives = derivativeIndices.size();
87 
88  if (numDerivatives < 1)
89  {
90  KnotAveraging(parameters, degree, knots);
91  return;
92  }
93 
94  DenseIndex startIndex;
95  DenseIndex endIndex;
96 
97  DenseIndex numInternalDerivatives = numDerivatives;
98 
99  if (derivativeIndices[0] == 0)
100  {
101  startIndex = 0;
102  --numInternalDerivatives;
103  }
104  else
105  {
106  startIndex = 1;
107  }
108  if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
109  {
110  endIndex = numParameters - degree;
111  --numInternalDerivatives;
112  }
113  else
114  {
115  endIndex = numParameters - degree - 1;
116  }
117 
118  // There are (endIndex - startIndex + 1) knots obtained from the averaging
119  // and 2 for the first and last parameters.
120  DenseIndex numAverageKnots = endIndex - startIndex + 3;
121  KnotVectorType averageKnots(numAverageKnots);
122  averageKnots[0] = parameters[0];
123 
124  int newKnotIndex = 0;
125  for (DenseIndex i = startIndex; i <= endIndex; ++i)
126  averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
127  averageKnots[++newKnotIndex] = parameters[numParameters - 1];
128 
129  newKnotIndex = -1;
130 
131  ParameterVectorType temporaryParameters(numParameters + 1);
132  KnotVectorType derivativeKnots(numInternalDerivatives);
133  for (DenseIndex i = 0; i < numAverageKnots - 1; ++i)
134  {
135  temporaryParameters[0] = averageKnots[i];
136  ParameterVectorType parameterIndices(numParameters);
137  int temporaryParameterIndex = 1;
138  for (DenseIndex j = 0; j < numParameters; ++j)
139  {
140  Scalar parameter = parameters[j];
141  if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
142  {
143  parameterIndices[temporaryParameterIndex] = j;
144  temporaryParameters[temporaryParameterIndex++] = parameter;
145  }
146  }
147  temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];
148 
149  for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
150  {
151  for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
152  {
153  if (parameterIndices[j + 1] == derivativeIndices[k]
154  && parameterIndices[j + 1] != 0
155  && parameterIndices[j + 1] != numParameters - 1)
156  {
157  derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
158  break;
159  }
160  }
161  }
162  }
163 
164  KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());
165 
166  std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
167  derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
168  temporaryKnots.data());
169 
170  // Number of knots (one for each point and derivative) plus spline order.
171  DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
172  knots.resize(numKnots);
173 
174  knots.head(degree).fill(temporaryKnots[0]);
175  knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
176  knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
177  }
178 
188  template <typename PointArrayType, typename KnotVectorType>
189  void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
190  {
191  typedef typename KnotVectorType::Scalar Scalar;
192 
193  const DenseIndex n = pts.cols();
194 
195  // 1. compute the column-wise norms
196  chord_lengths.resize(pts.cols());
197  chord_lengths[0] = 0;
198  chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
199 
200  // 2. compute the partial sums
201  std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
202 
203  // 3. normalize the data
204  chord_lengths /= chord_lengths(n-1);
205  chord_lengths(n-1) = Scalar(1);
206  }
207 
212  template <typename SplineType>
214  {
215  typedef typename SplineType::KnotVectorType KnotVectorType;
216  typedef typename SplineType::ParameterVectorType ParameterVectorType;
217 
226  template <typename PointArrayType>
227  static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
228 
238  template <typename PointArrayType>
239  static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
240 
258  template <typename PointArrayType, typename IndexArray>
259  static SplineType InterpolateWithDerivatives(const PointArrayType& points,
260  const PointArrayType& derivatives,
261  const IndexArray& derivativeIndices,
262  const unsigned int degree);
263 
280  template <typename PointArrayType, typename IndexArray>
281  static SplineType InterpolateWithDerivatives(const PointArrayType& points,
282  const PointArrayType& derivatives,
283  const IndexArray& derivativeIndices,
284  const unsigned int degree,
285  const ParameterVectorType& parameters);
286  };
287 
288  template <typename SplineType>
289  template <typename PointArrayType>
290  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
291  {
292  typedef typename SplineType::KnotVectorType::Scalar Scalar;
293  typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
294 
296 
297  KnotVectorType knots;
298  KnotAveraging(knot_parameters, degree, knots);
299 
300  DenseIndex n = pts.cols();
301  MatrixType A = MatrixType::Zero(n,n);
302  for (DenseIndex i=1; i<n-1; ++i)
303  {
304  const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
305 
306  // The segment call should somehow be told the spline order at compile time.
307  A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
308  }
309  A(0,0) = 1.0;
310  A(n-1,n-1) = 1.0;
311 
313 
314  // Here, we are creating a temporary due to an Eigen issue.
315  ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
316 
317  return SplineType(knots, ctrls);
318  }
319 
320  template <typename SplineType>
321  template <typename PointArrayType>
322  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
323  {
324  KnotVectorType chord_lengths; // knot parameters
325  ChordLengths(pts, chord_lengths);
326  return Interpolate(pts, degree, chord_lengths);
327  }
328 
329  template <typename SplineType>
330  template <typename PointArrayType, typename IndexArray>
331  SplineType
333  const PointArrayType& derivatives,
334  const IndexArray& derivativeIndices,
335  const unsigned int degree,
337  {
338  typedef typename SplineType::KnotVectorType::Scalar Scalar;
339  typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
340 
342 
343  const DenseIndex n = points.cols() + derivatives.cols();
344 
345  KnotVectorType knots;
346 
347  KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);
348 
349  // fill matrix
350  MatrixType A = MatrixType::Zero(n, n);
351 
352  // Use these dimensions for quicker populating, then transpose for solving.
353  MatrixType b(points.rows(), n);
354 
355  DenseIndex startRow;
356  DenseIndex derivativeStart;
357 
358  // End derivatives.
359  if (derivativeIndices[0] == 0)
360  {
361  A.template block<1, 2>(1, 0) << -1, 1;
362 
363  Scalar y = (knots(degree + 1) - knots(0)) / degree;
364  b.col(1) = y*derivatives.col(0);
365 
366  startRow = 2;
367  derivativeStart = 1;
368  }
369  else
370  {
371  startRow = 1;
372  derivativeStart = 0;
373  }
374  if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
375  {
376  A.template block<1, 2>(n - 2, n - 2) << -1, 1;
377 
378  Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
379  b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
380  }
381 
382  DenseIndex row = startRow;
383  DenseIndex derivativeIndex = derivativeStart;
384  for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
385  {
386  const DenseIndex span = SplineType::Span(parameters[i], degree, knots);
387 
388  if (derivativeIndex < derivativeIndices.size() && derivativeIndices[derivativeIndex] == i)
389  {
390  A.block(row, span - degree, 2, degree + 1)
391  = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);
392 
393  b.col(row++) = points.col(i);
394  b.col(row++) = derivatives.col(derivativeIndex++);
395  }
396  else
397  {
398  A.row(row).segment(span - degree, degree + 1)
399  = SplineType::BasisFunctions(parameters[i], degree, knots);
400  b.col(row++) = points.col(i);
401  }
402  }
403  b.col(0) = points.col(0);
404  b.col(b.cols() - 1) = points.col(points.cols() - 1);
405  A(0,0) = 1;
406  A(n - 1, n - 1) = 1;
407 
408  // Solve
410  ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose();
411 
412  SplineType spline(knots, controlPoints);
413 
414  return spline;
415  }
416 
417  template <typename SplineType>
418  template <typename PointArrayType, typename IndexArray>
419  SplineType
421  const PointArrayType& derivatives,
422  const IndexArray& derivativeIndices,
423  const unsigned int degree)
424  {
426  ChordLengths(points, parameters);
427  return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
428  }
429 }
430 
431 #endif // EIGEN_SPLINE_FITTING_H
const Solve< Derived, Rhs > solve(const MatrixBase< Rhs > &b) const
Definition: SolverBase.h:106
SCALAR Scalar
Definition: bench_gemm.cpp:46
Scalar * y
Scalar * b
Definition: benchVecAdd.cpp:17
int n
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
MatrixXf MatrixType
Spline fitting methods.
void ChordLengths(const PointArrayType &pts, KnotVectorType &chord_lengths)
Computes chord length parameters which are required for spline interpolation.
HouseholderQR< MatrixXf > qr(A)
void KnotAveragingWithDerivatives(const ParameterVectorType &parameters, const unsigned int degree, const IndexArray &derivativeIndices, KnotVectorType &knots)
Computes knot averages when derivative constraints are present. Note that this is a technical interpr...
Definition: SplineFitting.h:78
const double degree
static SplineType InterpolateWithDerivatives(const PointArrayType &points, const PointArrayType &derivatives, const IndexArray &derivativeIndices, const unsigned int degree)
Fits an interpolating spline to the given data points and derivatives.
void KnotAveraging(const KnotVectorType &parameters, DenseIndex degree, KnotVectorType &knots)
Computes knot averages.The knots are computed as where is the degree and the number knots of the d...
Definition: SplineFitting.h:45
m row(1)
cout<< "Here is the matrix m:"<< endl<< m<< endl;Eigen::FullPivLU< Matrix5x3 > lu(m)
static ConjugateGradientParameters parameters
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: Meta.h:66
static SplineType Interpolate(const PointArrayType &pts, DenseIndex degree)
Fits an interpolating Spline to the given data points.
SplineType::ParameterVectorType ParameterVectorType
LU decomposition of a matrix with complete pivoting, and related features.
SplineType::KnotVectorType KnotVectorType
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:36:19