Please, help us to better know about our user community by answering the following short survey: https://forms.gle/wpyrxWi18ox9Z5ae9
 
Loading...
Searching...
No Matches
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 <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
23namespace 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
295 typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
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,
336 const ParameterVectorType& parameters)
337 {
338 typedef typename SplineType::KnotVectorType::Scalar Scalar;
339 typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
340
341 typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;
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 {
425 ParameterVectorType parameters;
426 ChordLengths(points, parameters);
427 return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
428 }
429}
430
431#endif // EIGEN_SPLINE_FITTING_H
const Solve< FullPivLU, Rhs > solve(const MatrixBase< Rhs > &b) const
const Solve< HouseholderQR, Rhs > solve(const MatrixBase< Rhs > &b) const
void ChordLengths(const PointArrayType &pts, KnotVectorType &chord_lengths)
Computes chord length parameters which are required for spline interpolation.
Definition: SplineFitting.h:189
void KnotAveraging(const KnotVectorType &parameters, DenseIndex degree, KnotVectorType &knots)
Computes knot averages.
Definition: SplineFitting.h:45
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
Namespace containing all symbols from the Eigen library.
Spline fitting methods.
Definition: SplineFitting.h:214
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.
Definition: SplineFitting.h:420
static SplineType Interpolate(const PointArrayType &pts, DenseIndex degree)
Fits an interpolating Spline to the given data points.
Definition: SplineFitting.h:322