1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_SPLINE_FITTING_H 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define EIGEN_SPLINE_FITTING_H 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <numeric> 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include "SplineFwd.h" 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <Eigen/QR> 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Computes knot averages. 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \ingroup Splines_Module 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * The knots are computed as 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \f{align*} 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * u_0 & = \hdots = u_p = 0 \\ 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * u_{m-p} & = \hdots = u_{m} = 1 \\ 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \f} 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * where \f$p\f$ is the degree and \f$m+1\f$ the number knots 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * of the desired interpolating spline. 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[in] parameters The input parameters. During interpolation one for each data point. 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[in] degree The spline degree which is used during the interpolation. 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[out] knots The output knot vector. 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename KnotVectorType> 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots) 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename KnotVectorType::Scalar Scalar; 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots.resize(parameters.size()+degree+1); 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (DenseIndex j=1; j<parameters.size()-degree; ++j) 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots(j+degree) = parameters.segment(j,degree).mean(); 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1); 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1); 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Computes chord length parameters which are required for spline interpolation. 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \ingroup Splines_Module 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[in] pts The data points to which a spline should be fit. 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[out] chord_lengths The resulting chord lenggth vector. 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType, typename KnotVectorType> 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths) 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename KnotVectorType::Scalar Scalar; 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const DenseIndex n = pts.cols(); 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 1. compute the column-wise norms 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths.resize(pts.cols()); 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths[0] = 0; 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm(); 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 2. compute the partial sums 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data()); 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 3. normalize the data 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths /= chord_lengths(n-1); 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths(n-1) = Scalar(1); 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Spline fitting methods. 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \ingroup Splines_Module 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename SplineType> 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath struct SplineFitting 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::KnotVectorType KnotVectorType; 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Fits an interpolating Spline to the given data points. 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param pts The points for which an interpolating spline will be computed. 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param degree The degree of the interpolating spline. 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns A spline interpolating the initially provided points. 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree); 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Fits an interpolating Spline to the given data points. 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param pts The points for which an interpolating spline will be computed. 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param degree The degree of the interpolating spline. 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param knot_parameters The knot parameters for the interpolation. 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns A spline interpolating the initially provided points. 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters); 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename SplineType> 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters) 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::KnotVectorType::Scalar Scalar; 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::BasisVectorType BasisVectorType; 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::ControlPointVectorType ControlPointVectorType; 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType; 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KnotVectorType knots; 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KnotAveraging(knot_parameters, degree, knots); 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath DenseIndex n = pts.cols(); 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MatrixType A = MatrixType::Zero(n,n); 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (DenseIndex i=1; i<n-1; ++i) 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots); 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // The segment call should somehow be told the spline order at compile time. 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots); 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath A(0,0) = 1.0; 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath A(n-1,n-1) = 1.0; 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HouseholderQR<MatrixType> qr(A); 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Here, we are creating a temporary due to an Eigen issue. 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose(); 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return SplineType(knots, ctrls); 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename SplineType> 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree) 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KnotVectorType chord_lengths; // knot parameters 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ChordLengths(pts, chord_lengths); 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Interpolate(pts, degree, chord_lengths); 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_SPLINE_FITTING_H 160