![]() |
Eigen-unsupported
3.3.3
|
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 <algorithm> 00014 #include <functional> 00015 #include <numeric> 00016 #include <vector> 00017 00018 #include "SplineFwd.h" 00019 00020 #include <Eigen/LU> 00021 #include <Eigen/QR> 00022 00023 namespace Eigen 00024 { 00044 template <typename KnotVectorType> 00045 void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots) 00046 { 00047 knots.resize(parameters.size()+degree+1); 00048 00049 for (DenseIndex j=1; j<parameters.size()-degree; ++j) 00050 knots(j+degree) = parameters.segment(j,degree).mean(); 00051 00052 knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1); 00053 knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1); 00054 } 00055 00077 template <typename KnotVectorType, typename ParameterVectorType, typename IndexArray> 00078 void KnotAveragingWithDerivatives(const ParameterVectorType& parameters, 00079 const unsigned int degree, 00080 const IndexArray& derivativeIndices, 00081 KnotVectorType& knots) 00082 { 00083 typedef typename ParameterVectorType::Scalar Scalar; 00084 00085 DenseIndex numParameters = parameters.size(); 00086 DenseIndex numDerivatives = derivativeIndices.size(); 00087 00088 if (numDerivatives < 1) 00089 { 00090 KnotAveraging(parameters, degree, knots); 00091 return; 00092 } 00093 00094 DenseIndex startIndex; 00095 DenseIndex endIndex; 00096 00097 DenseIndex numInternalDerivatives = numDerivatives; 00098 00099 if (derivativeIndices[0] == 0) 00100 { 00101 startIndex = 0; 00102 --numInternalDerivatives; 00103 } 00104 else 00105 { 00106 startIndex = 1; 00107 } 00108 if (derivativeIndices[numDerivatives - 1] == numParameters - 1) 00109 { 00110 endIndex = numParameters - degree; 00111 --numInternalDerivatives; 00112 } 00113 else 00114 { 00115 endIndex = numParameters - degree - 1; 00116 } 00117 00118 // There are (endIndex - startIndex + 1) knots obtained from the averaging 00119 // and 2 for the first and last parameters. 00120 DenseIndex numAverageKnots = endIndex - startIndex + 3; 00121 KnotVectorType averageKnots(numAverageKnots); 00122 averageKnots[0] = parameters[0]; 00123 00124 int newKnotIndex = 0; 00125 for (DenseIndex i = startIndex; i <= endIndex; ++i) 00126 averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean(); 00127 averageKnots[++newKnotIndex] = parameters[numParameters - 1]; 00128 00129 newKnotIndex = -1; 00130 00131 ParameterVectorType temporaryParameters(numParameters + 1); 00132 KnotVectorType derivativeKnots(numInternalDerivatives); 00133 for (DenseIndex i = 0; i < numAverageKnots - 1; ++i) 00134 { 00135 temporaryParameters[0] = averageKnots[i]; 00136 ParameterVectorType parameterIndices(numParameters); 00137 int temporaryParameterIndex = 1; 00138 for (DenseIndex j = 0; j < numParameters; ++j) 00139 { 00140 Scalar parameter = parameters[j]; 00141 if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1]) 00142 { 00143 parameterIndices[temporaryParameterIndex] = j; 00144 temporaryParameters[temporaryParameterIndex++] = parameter; 00145 } 00146 } 00147 temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1]; 00148 00149 for (int j = 0; j <= temporaryParameterIndex - 2; ++j) 00150 { 00151 for (DenseIndex k = 0; k < derivativeIndices.size(); ++k) 00152 { 00153 if (parameterIndices[j + 1] == derivativeIndices[k] 00154 && parameterIndices[j + 1] != 0 00155 && parameterIndices[j + 1] != numParameters - 1) 00156 { 00157 derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean(); 00158 break; 00159 } 00160 } 00161 } 00162 } 00163 00164 KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size()); 00165 00166 std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(), 00167 derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(), 00168 temporaryKnots.data()); 00169 00170 // Number of knots (one for each point and derivative) plus spline order. 00171 DenseIndex numKnots = numParameters + numDerivatives + degree + 1; 00172 knots.resize(numKnots); 00173 00174 knots.head(degree).fill(temporaryKnots[0]); 00175 knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]); 00176 knots.segment(degree, temporaryKnots.size()) = temporaryKnots; 00177 } 00178 00188 template <typename PointArrayType, typename KnotVectorType> 00189 void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths) 00190 { 00191 typedef typename KnotVectorType::Scalar Scalar; 00192 00193 const DenseIndex n = pts.cols(); 00194 00195 // 1. compute the column-wise norms 00196 chord_lengths.resize(pts.cols()); 00197 chord_lengths[0] = 0; 00198 chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm(); 00199 00200 // 2. compute the partial sums 00201 std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data()); 00202 00203 // 3. normalize the data 00204 chord_lengths /= chord_lengths(n-1); 00205 chord_lengths(n-1) = Scalar(1); 00206 } 00207 00212 template <typename SplineType> 00213 struct SplineFitting 00214 { 00215 typedef typename SplineType::KnotVectorType KnotVectorType; 00216 typedef typename SplineType::ParameterVectorType ParameterVectorType; 00217 00226 template <typename PointArrayType> 00227 static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree); 00228 00238 template <typename PointArrayType> 00239 static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters); 00240 00258 template <typename PointArrayType, typename IndexArray> 00259 static SplineType InterpolateWithDerivatives(const PointArrayType& points, 00260 const PointArrayType& derivatives, 00261 const IndexArray& derivativeIndices, 00262 const unsigned int degree); 00263 00280 template <typename PointArrayType, typename IndexArray> 00281 static SplineType InterpolateWithDerivatives(const PointArrayType& points, 00282 const PointArrayType& derivatives, 00283 const IndexArray& derivativeIndices, 00284 const unsigned int degree, 00285 const ParameterVectorType& parameters); 00286 }; 00287 00288 template <typename SplineType> 00289 template <typename PointArrayType> 00290 SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters) 00291 { 00292 typedef typename SplineType::KnotVectorType::Scalar Scalar; 00293 typedef typename SplineType::ControlPointVectorType ControlPointVectorType; 00294 00295 typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType; 00296 00297 KnotVectorType knots; 00298 KnotAveraging(knot_parameters, degree, knots); 00299 00300 DenseIndex n = pts.cols(); 00301 MatrixType A = MatrixType::Zero(n,n); 00302 for (DenseIndex i=1; i<n-1; ++i) 00303 { 00304 const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots); 00305 00306 // The segment call should somehow be told the spline order at compile time. 00307 A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots); 00308 } 00309 A(0,0) = 1.0; 00310 A(n-1,n-1) = 1.0; 00311 00312 HouseholderQR<MatrixType> qr(A); 00313 00314 // Here, we are creating a temporary due to an Eigen issue. 00315 ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose(); 00316 00317 return SplineType(knots, ctrls); 00318 } 00319 00320 template <typename SplineType> 00321 template <typename PointArrayType> 00322 SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree) 00323 { 00324 KnotVectorType chord_lengths; // knot parameters 00325 ChordLengths(pts, chord_lengths); 00326 return Interpolate(pts, degree, chord_lengths); 00327 } 00328 00329 template <typename SplineType> 00330 template <typename PointArrayType, typename IndexArray> 00331 SplineType 00332 SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points, 00333 const PointArrayType& derivatives, 00334 const IndexArray& derivativeIndices, 00335 const unsigned int degree, 00336 const ParameterVectorType& parameters) 00337 { 00338 typedef typename SplineType::KnotVectorType::Scalar Scalar; 00339 typedef typename SplineType::ControlPointVectorType ControlPointVectorType; 00340 00341 typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType; 00342 00343 const DenseIndex n = points.cols() + derivatives.cols(); 00344 00345 KnotVectorType knots; 00346 00347 KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots); 00348 00349 // fill matrix 00350 MatrixType A = MatrixType::Zero(n, n); 00351 00352 // Use these dimensions for quicker populating, then transpose for solving. 00353 MatrixType b(points.rows(), n); 00354 00355 DenseIndex startRow; 00356 DenseIndex derivativeStart; 00357 00358 // End derivatives. 00359 if (derivativeIndices[0] == 0) 00360 { 00361 A.template block<1, 2>(1, 0) << -1, 1; 00362 00363 Scalar y = (knots(degree + 1) - knots(0)) / degree; 00364 b.col(1) = y*derivatives.col(0); 00365 00366 startRow = 2; 00367 derivativeStart = 1; 00368 } 00369 else 00370 { 00371 startRow = 1; 00372 derivativeStart = 0; 00373 } 00374 if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1) 00375 { 00376 A.template block<1, 2>(n - 2, n - 2) << -1, 1; 00377 00378 Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree; 00379 b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1); 00380 } 00381 00382 DenseIndex row = startRow; 00383 DenseIndex derivativeIndex = derivativeStart; 00384 for (DenseIndex i = 1; i < parameters.size() - 1; ++i) 00385 { 00386 const DenseIndex span = SplineType::Span(parameters[i], degree, knots); 00387 00388 if (derivativeIndices[derivativeIndex] == i) 00389 { 00390 A.block(row, span - degree, 2, degree + 1) 00391 = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots); 00392 00393 b.col(row++) = points.col(i); 00394 b.col(row++) = derivatives.col(derivativeIndex++); 00395 } 00396 else 00397 { 00398 A.row(row++).segment(span - degree, degree + 1) 00399 = SplineType::BasisFunctions(parameters[i], degree, knots); 00400 } 00401 } 00402 b.col(0) = points.col(0); 00403 b.col(b.cols() - 1) = points.col(points.cols() - 1); 00404 A(0,0) = 1; 00405 A(n - 1, n - 1) = 1; 00406 00407 // Solve 00408 FullPivLU<MatrixType> lu(A); 00409 ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose(); 00410 00411 SplineType spline(knots, controlPoints); 00412 00413 return spline; 00414 } 00415 00416 template <typename SplineType> 00417 template <typename PointArrayType, typename IndexArray> 00418 SplineType 00419 SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points, 00420 const PointArrayType& derivatives, 00421 const IndexArray& derivativeIndices, 00422 const unsigned int degree) 00423 { 00424 ParameterVectorType parameters; 00425 ChordLengths(points, parameters); 00426 return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters); 00427 } 00428 } 00429 00430 #endif // EIGEN_SPLINE_FITTING_H