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 knots.resize(parameters.size()+degree+1); 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (DenseIndex j=1; j<parameters.size()-degree; ++j) 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots(j+degree) = parameters.segment(j,degree).mean(); 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1); 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1); 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Computes chord length parameters which are required for spline interpolation. 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \ingroup Splines_Module 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[in] pts The data points to which a spline should be fit. 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param[out] chord_lengths The resulting chord lenggth vector. 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType, typename KnotVectorType> 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths) 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename KnotVectorType::Scalar Scalar; 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const DenseIndex n = pts.cols(); 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 1. compute the column-wise norms 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths.resize(pts.cols()); 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths[0] = 0; 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm(); 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 2. compute the partial sums 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data()); 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 3. normalize the data 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths /= chord_lengths(n-1); 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath chord_lengths(n-1) = Scalar(1); 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Spline fitting methods. 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \ingroup Splines_Module 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename SplineType> 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath struct SplineFitting 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::KnotVectorType KnotVectorType; 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Fits an interpolating Spline to the given data points. 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param pts The points for which an interpolating spline will be computed. 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param degree The degree of the interpolating spline. 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns A spline interpolating the initially provided points. 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree); 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Fits an interpolating Spline to the given data points. 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param pts The points for which an interpolating spline will be computed. 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param degree The degree of the interpolating spline. 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param knot_parameters The knot parameters for the interpolation. 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \returns A spline interpolating the initially provided points. 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath **/ 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters); 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath }; 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename SplineType> 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters) 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::KnotVectorType::Scalar Scalar; 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename SplineType::ControlPointVectorType ControlPointVectorType; 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType; 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KnotVectorType knots; 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KnotAveraging(knot_parameters, degree, knots); 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath DenseIndex n = pts.cols(); 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath MatrixType A = MatrixType::Zero(n,n); 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (DenseIndex i=1; i<n-1; ++i) 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots); 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // The segment call should somehow be told the spline order at compile time. 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots); 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath A(0,0) = 1.0; 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath A(n-1,n-1) = 1.0; 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath HouseholderQR<MatrixType> qr(A); 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Here, we are creating a temporary due to an Eigen issue. 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose(); 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return SplineType(knots, ctrls); 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename SplineType> 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template <typename PointArrayType> 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree) 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath KnotVectorType chord_lengths; // knot parameters 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath ChordLengths(pts, chord_lengths); 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return Interpolate(pts, degree, chord_lengths); 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_SPLINE_FITTING_H 157