00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_SPLINE_FITTING_H
00026 #define EIGEN_SPLINE_FITTING_H
00027
00028 #include <numeric>
00029
00030 #include "SplineFwd.h"
00031
00032 #include <Eigen/QR>
00033
00034 namespace Eigen
00035 {
00055 template <typename KnotVectorType>
00056 void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
00057 {
00058 typedef typename KnotVectorType::Scalar Scalar;
00059
00060 knots.resize(parameters.size()+degree+1);
00061
00062 for (DenseIndex j=1; j<parameters.size()-degree; ++j)
00063 knots(j+degree) = parameters.segment(j,degree).mean();
00064
00065 knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
00066 knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
00067 }
00068
00078 template <typename PointArrayType, typename KnotVectorType>
00079 void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
00080 {
00081 typedef typename KnotVectorType::Scalar Scalar;
00082
00083 const DenseIndex n = pts.cols();
00084
00085
00086 chord_lengths.resize(pts.cols());
00087 chord_lengths[0] = 0;
00088 chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();
00089
00090
00091 std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());
00092
00093
00094 chord_lengths /= chord_lengths(n-1);
00095 chord_lengths(n-1) = Scalar(1);
00096 }
00097
00102 template <typename SplineType>
00103 struct SplineFitting
00104 {
00105 typedef typename SplineType::KnotVectorType KnotVectorType;
00106
00115 template <typename PointArrayType>
00116 static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);
00117
00127 template <typename PointArrayType>
00128 static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);
00129 };
00130
00131 template <typename SplineType>
00132 template <typename PointArrayType>
00133 SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
00134 {
00135 typedef typename SplineType::KnotVectorType::Scalar Scalar;
00136 typedef typename SplineType::BasisVectorType BasisVectorType;
00137 typedef typename SplineType::ControlPointVectorType ControlPointVectorType;
00138
00139 typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
00140
00141 KnotVectorType knots;
00142 KnotAveraging(knot_parameters, degree, knots);
00143
00144 DenseIndex n = pts.cols();
00145 MatrixType A = MatrixType::Zero(n,n);
00146 for (DenseIndex i=1; i<n-1; ++i)
00147 {
00148 const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);
00149
00150
00151 A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
00152 }
00153 A(0,0) = 1.0;
00154 A(n-1,n-1) = 1.0;
00155
00156 HouseholderQR<MatrixType> qr(A);
00157
00158
00159 ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();
00160
00161 return SplineType(knots, ctrls);
00162 }
00163
00164 template <typename SplineType>
00165 template <typename PointArrayType>
00166 SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
00167 {
00168 KnotVectorType chord_lengths;
00169 ChordLengths(pts, chord_lengths);
00170 return Interpolate(pts, degree, chord_lengths);
00171 }
00172 }
00173
00174 #endif // EIGEN_SPLINE_FITTING_H