Quelle SplineFitting.h Sprache: C

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 20010-2011 Hauke Heibel <hauke.heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_SPLINE_FITTING_H
#define EIGEN_SPLINE_FITTING_H

#include <algorithm>
#include <functional>
#include <numeric>
#include <vector>

#include "SplineFwd.h"

#include "../../../../Eigen/LU"
#include "../../../../Eigen/QR"

namespace Eigen
{
  /**
   * \brief Computes knot averages.
   * \ingroup Splines_Module
   *
   * The knots are computed as
   * \f{align*}
   *  u_0 & = \hdots = u_p = 0 \\
   *  u_{m-p} & = \hdots = u_{m} = 1 \\
   *  u_{j+p} & = \frac{1}{p}\sum_{i=j}^{j+p-1}\bar{u}_i \quad\quad j=1,\hdots,n-p
   * \f}
   * where \f$p\f$ is the degree and \f$m+1\f$ the number knots
   * of the desired interpolating spline.
   *
   * \param[in] parameters The input parameters. During interpolation one for each data point.
   * \param[in] degree The spline degree which is used during the interpolation.
   * \param[out] knots The output knot vector.
   *
   * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
   **/
  template <typename KnotVectorType>
  void KnotAveraging(const KnotVectorType& parameters, DenseIndex degree, KnotVectorType& knots)
  {
    knots.resize(parameters.size()+degree+1);

    for (DenseIndex j=1; j<parameters.size()-degree; ++j)
      knots(j+degree) = parameters.segment(j,degree).mean();

    knots.segment(0,degree+1) = KnotVectorType::Zero(degree+1);
    knots.segment(knots.size()-degree-1,degree+1) = KnotVectorType::Ones(degree+1);
  }

  /**
   * \brief Computes knot averages when derivative constraints are present.
   * Note that this is a technical interpretation of the referenced article
   * since the algorithm contained therein is incorrect as written.
   * \ingroup Splines_Module
   *
   * \param[in] parameters The parameters at which the interpolation B-Spline
   *            will intersect the given interpolation points. The parameters
   *            are assumed to be a non-decreasing sequence.
   * \param[in] degree The degree of the interpolating B-Spline. This must be
   *            greater than zero.
   * \param[in] derivativeIndices The indices corresponding to parameters at
   *            which there are derivative constraints. The indices are assumed
   *            to be a non-decreasing sequence.
   * \param[out] knots The calculated knot vector. These will be returned as a
   *             non-decreasing sequence
   *
   * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
   * Curve interpolation with directional constraints for engineering design.
   * Engineering with Computers
   **/
  template <typename KnotVectorType, typename ParameterVectorType, typename IndexArray>
  void KnotAveragingWithDerivatives(const ParameterVectorType& parameters,
                                    const unsigned int degree,
                                    const IndexArray& derivativeIndices,
                                    KnotVectorType& knots)
  {
    typedef typename ParameterVectorType::Scalar Scalar;

    DenseIndex numParameters = parameters.size();
    DenseIndex numDerivatives = derivativeIndices.size();

    if (numDerivatives < 1)
    {
      KnotAveraging(parameters, degree, knots);
      return;
    }

    DenseIndex startIndex;
    DenseIndex endIndex;

    DenseIndex numInternalDerivatives = numDerivatives;

    if (derivativeIndices[0] == 0)
    {
      startIndex = 0;
      --numInternalDerivatives;
    }
    else
    {
      startIndex = 1;
    }
    if (derivativeIndices[numDerivatives - 1] == numParameters - 1)
    {
      endIndex = numParameters - degree;
      --numInternalDerivatives;
    }
    else
    {
      endIndex = numParameters - degree - 1;
    }

    // There are (endIndex - startIndex + 1) knots obtained from the averaging
    // and 2 for the first and last parameters.
    DenseIndex numAverageKnots = endIndex - startIndex + 3;
    KnotVectorType averageKnots(numAverageKnots);
    averageKnots[0] = parameters[0];

    int newKnotIndex = 0;
    for (DenseIndex i = startIndex; i <= endIndex; ++i)
      averageKnots[++newKnotIndex] = parameters.segment(i, degree).mean();
    averageKnots[++newKnotIndex] = parameters[numParameters - 1];

    newKnotIndex = -1;

    ParameterVectorType temporaryParameters(numParameters + 1);
    KnotVectorType derivativeKnots(numInternalDerivatives);
    for (DenseIndex i = 0; i < numAverageKnots - 1; ++i)
    {
      temporaryParameters[0] = averageKnots[i];
      ParameterVectorType parameterIndices(numParameters);
      int temporaryParameterIndex = 1;
      for (DenseIndex j = 0; j < numParameters; ++j)
      {
        Scalar parameter = parameters[j];
        if (parameter >= averageKnots[i] && parameter < averageKnots[i + 1])
        {
          parameterIndices[temporaryParameterIndex] = j;
          temporaryParameters[temporaryParameterIndex++] = parameter;
        }
      }
      temporaryParameters[temporaryParameterIndex] = averageKnots[i + 1];

      for (int j = 0; j <= temporaryParameterIndex - 2; ++j)
      {
        for (DenseIndex k = 0; k < derivativeIndices.size(); ++k)
        {
          if (parameterIndices[j + 1] == derivativeIndices[k]
              && parameterIndices[j + 1] != 0
              && parameterIndices[j + 1] != numParameters - 1)
          {
            derivativeKnots[++newKnotIndex] = temporaryParameters.segment(j, 3).mean();
            break;
          }
        }
      }
    }

    KnotVectorType temporaryKnots(averageKnots.size() + derivativeKnots.size());

    std::merge(averageKnots.data(), averageKnots.data() + averageKnots.size(),
               derivativeKnots.data(), derivativeKnots.data() + derivativeKnots.size(),
               temporaryKnots.data());

    // Number of knots (one for each point and derivative) plus spline order.
    DenseIndex numKnots = numParameters + numDerivatives + degree + 1;
    knots.resize(numKnots);

    knots.head(degree).fill(temporaryKnots[0]);
    knots.tail(degree).fill(temporaryKnots.template tail<1>()[0]);
    knots.segment(degree, temporaryKnots.size()) = temporaryKnots;
  }

  /**
   * \brief Computes chord length parameters which are required for spline interpolation.
   * \ingroup Splines_Module
   *
   * \param[in] pts The data points to which a spline should be fit.
   * \param[out] chord_lengths The resulting chord length vector.
   *
   * \sa Les Piegl and Wayne Tiller, The NURBS book (2nd ed.), 1997, 9.2.1 Global Curve Interpolation to Point Data
   **/
  template <typename PointArrayType, typename KnotVectorType>
  void ChordLengths(const PointArrayType& pts, KnotVectorType& chord_lengths)
  {
    typedef typename KnotVectorType::Scalar Scalar;

    const DenseIndex n = pts.cols();

    // 1. compute the column-wise norms
    chord_lengths.resize(pts.cols());
    chord_lengths[0] = 0;
    chord_lengths.rightCols(n-1) = (pts.array().leftCols(n-1) - pts.array().rightCols(n-1)).matrix().colwise().norm();

    // 2. compute the partial sums
    std::partial_sum(chord_lengths.data(), chord_lengths.data()+n, chord_lengths.data());

    // 3. normalize the data
    chord_lengths /= chord_lengths(n-1);
    chord_lengths(n-1) = Scalar(1);
  }

  /**
   * \brief Spline fitting methods.
   * \ingroup Splines_Module
   **/
  template <typename SplineType>
  struct SplineFitting
  {
    typedef typename SplineType::KnotVectorType KnotVectorType;
    typedef typename SplineType::ParameterVectorType ParameterVectorType;

    /**
     * \brief Fits an interpolating Spline to the given data points.
     *
     * \param pts The points for which an interpolating spline will be computed.
     * \param degree The degree of the interpolating spline.
     *
     * \returns A spline interpolating the initially provided points.
     **/
    template <typename PointArrayType>
    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree);

    /**
     * \brief Fits an interpolating Spline to the given data points.
     *
     * \param pts The points for which an interpolating spline will be computed.
     * \param degree The degree of the interpolating spline.
     * \param knot_parameters The knot parameters for the interpolation.
     *
     * \returns A spline interpolating the initially provided points.
     **/
    template <typename PointArrayType>
    static SplineType Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters);

    /**
     * \brief Fits an interpolating spline to the given data points and
     * derivatives.
     *
     * \param points The points for which an interpolating spline will be computed.
     * \param derivatives The desired derivatives of the interpolating spline at interpolation
     *                    points.
     * \param derivativeIndices An array indicating which point each derivative belongs to. This
     *                          must be the same size as @a derivatives.
     * \param degree The degree of the interpolating spline.
     *
     * \returns A spline interpolating @a points with @a derivatives at those points.
     *
     * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
     * Curve interpolation with directional constraints for engineering design.
     * Engineering with Computers
     **/
    template <typename PointArrayType, typename IndexArray>
    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
                                                 const PointArrayType& derivatives,
                                                 const IndexArray& derivativeIndices,
                                                 const unsigned int degree);

    /**
     * \brief Fits an interpolating spline to the given data points and derivatives.
     *
     * \param points The points for which an interpolating spline will be computed.
     * \param derivatives The desired derivatives of the interpolating spline at interpolation points.
     * \param derivativeIndices An array indicating which point each derivative belongs to. This
     *                          must be the same size as @a derivatives.
     * \param degree The degree of the interpolating spline.
     * \param parameters The parameters corresponding to the interpolation points.
     *
     * \returns A spline interpolating @a points with @a derivatives at those points.
     *
     * \sa Les A. Piegl, Khairan Rajab, Volha Smarodzinana. 2008.
     * Curve interpolation with directional constraints for engineering design.
     * Engineering with Computers
     */
    template <typename PointArrayType, typename IndexArray>
    static SplineType InterpolateWithDerivatives(const PointArrayType& points,
                                                 const PointArrayType& derivatives,
                                                 const IndexArray& derivativeIndices,
                                                 const unsigned int degree,
                                                 const ParameterVectorType& parameters);
  };

  template <typename SplineType>
  template <typename PointArrayType>
  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree, const KnotVectorType& knot_parameters)
  {
    typedef typename SplineType::KnotVectorType::Scalar Scalar;
    typedef typename SplineType::ControlPointVectorType ControlPointVectorType;

    typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;

    KnotVectorType knots;
    KnotAveraging(knot_parameters, degree, knots);

    DenseIndex n = pts.cols();
    MatrixType A = MatrixType::Zero(n,n);
    for (DenseIndex i=1; i<n-1; ++i)
    {
      const DenseIndex span = SplineType::Span(knot_parameters[i], degree, knots);

      // The segment call should somehow be told the spline order at compile time.
      A.row(i).segment(span-degree, degree+1) = SplineType::BasisFunctions(knot_parameters[i], degree, knots);
    }
    A(0,0) = 1.0;
    A(n-1,n-1) = 1.0;

    HouseholderQR<MatrixType> qr(A);

    // Here, we are creating a temporary due to an Eigen issue.
    ControlPointVectorType ctrls = qr.solve(MatrixType(pts.transpose())).transpose();

    return SplineType(knots, ctrls);
  }

  template <typename SplineType>
  template <typename PointArrayType>
  SplineType SplineFitting<SplineType>::Interpolate(const PointArrayType& pts, DenseIndex degree)
  {
    KnotVectorType chord_lengths; // knot parameters
    ChordLengths(pts, chord_lengths);
    return Interpolate(pts, degree, chord_lengths);
  }

  template <typename SplineType>
  template <typename PointArrayType, typename IndexArray>
  SplineType
  SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
                                                        const PointArrayType& derivatives,
                                                        const IndexArray& derivativeIndices,
                                                        const unsigned int degree,
                                                        const ParameterVectorType& parameters)
  {
    typedef typename SplineType::KnotVectorType::Scalar Scalar;
    typedef typename SplineType::ControlPointVectorType ControlPointVectorType;

    typedef Matrix<Scalar, Dynamic, Dynamic> MatrixType;

    const DenseIndex n = points.cols() + derivatives.cols();

    KnotVectorType knots;

    KnotAveragingWithDerivatives(parameters, degree, derivativeIndices, knots);

    // fill matrix
    MatrixType A = MatrixType::Zero(n, n);

    // Use these dimensions for quicker populating, then transpose for solving.
    MatrixType b(points.rows(), n);

    DenseIndex startRow;
    DenseIndex derivativeStart;

    // End derivatives.
    if (derivativeIndices[0] == 0)
    {
      A.template block<1, 2>(1, 0) << -1, 1;

      Scalar y = (knots(degree + 1) - knots(0)) / degree;
      b.col(1) = y*derivatives.col(0);

      startRow = 2;
      derivativeStart = 1;
    }
    else
    {
      startRow = 1;
      derivativeStart = 0;
    }
    if (derivativeIndices[derivatives.cols() - 1] == points.cols() - 1)
    {
      A.template block<1, 2>(n - 2, n - 2) << -1, 1;

      Scalar y = (knots(knots.size() - 1) - knots(knots.size() - (degree + 2))) / degree;
      b.col(b.cols() - 2) = y*derivatives.col(derivatives.cols() - 1);
    }

    DenseIndex row = startRow;
    DenseIndex derivativeIndex = derivativeStart;
    for (DenseIndex i = 1; i < parameters.size() - 1; ++i)
    {
      const DenseIndex span = SplineType::Span(parameters[i], degree, knots);

      if (derivativeIndex < derivativeIndices.size() && derivativeIndices[derivativeIndex] == i)
      {
        A.block(row, span - degree, 2, degree + 1)
          = SplineType::BasisFunctionDerivatives(parameters[i], 1, degree, knots);

        b.col(row++) = points.col(i);
        b.col(row++) = derivatives.col(derivativeIndex++);
      }
      else
      {
        A.row(row).segment(span - degree, degree + 1)
          = SplineType::BasisFunctions(parameters[i], degree, knots);
        b.col(row++) = points.col(i);
      }
    }
    b.col(0) = points.col(0);
    b.col(b.cols() - 1) = points.col(points.cols() - 1);
    A(0,0) = 1;
    A(n - 1, n - 1) = 1;

    // Solve
    FullPivLU<MatrixType> lu(A);
    ControlPointVectorType controlPoints = lu.solve(MatrixType(b.transpose())).transpose();

    SplineType spline(knots, controlPoints);

    return spline;
  }

  template <typename SplineType>
  template <typename PointArrayType, typename IndexArray>
  SplineType
  SplineFitting<SplineType>::InterpolateWithDerivatives(const PointArrayType& points,
                                                        const PointArrayType& derivatives,
                                                        const IndexArray& derivativeIndices,
                                                        const unsigned int degree)
  {
    ParameterVectorType parameters;
    ChordLengths(points, parameters);
    return InterpolateWithDerivatives(points, derivatives, derivativeIndices, degree, parameters);
  }
}

#endif // EIGEN_SPLINE_FITTING_H

quality89%

¤ Dauer der Verarbeitung: 0.30 Sekunden (vorverarbeitet) ¤

Wurzel

Suchen

Beweissystem der NASA

Beweissystem Isabelle

NIST Cobol Testsuite

Cephes Mathematical Library

Wiener Entwicklungsmethode

Haftungshinweis

Die Informationen auf dieser Webseite wurden nach bestem Wissen sorgfältig zusammengestellt. Es wird jedoch weder Vollständigkeit, noch Richtigkeit, noch Qualität der bereit gestellten Informationen zugesichert.

Bemerkung:

Die farbliche Syntaxdarstellung ist noch experimentell.