CardinalSplineBasis.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/basis/Basis.h>
#include <gtsam/basis/IrwinHall.h>
 
#include <stdexcept>
 
namespace gtsam {

class CardinalSplineBasis : public Basis<CardinalSplineBasis> {
 public:
  using Parameters = Vector;

  static Weights CalculateWeights(size_t N, double x) {
    return calculateWeights(N, x, 0);
  }

  static Weights CalculateWeights(size_t N, double x, double a, double b) {
    const double scale = coordinateScale(N, a, b);
    return calculateWeights(N, (x - a) * scale, 0);
  }

  static Weights DerivativeWeights(size_t N, double x) {
    return calculateWeights(N, x, 1);
  }

  static Weights DerivativeWeights(size_t N, double x, double a, double b) {
    const double scale = coordinateScale(N, a, b);
    return scale * calculateWeights(N, (x - a) * scale, 1);
  }
 
 private:
  static double coordinateScale(size_t N, double a, double b) {
    if (a == b) {
      throw std::invalid_argument(
          "CardinalSplineBasis interval must have nonzero length");
    }
    return (static_cast<double>(N > 0 ? N - 1 : 0) +
            kernels::IrwinHallCDF2.getLength()) /
           (b - a);
  }

  static Weights calculateWeights(size_t N, double x, size_t derivative) {
    if (N == 0) return Weights(0);
    if (N == 1) {
      Weights weights(1);
      weights(0) = derivative == 0 ? 1.0 : 0.0;
      return weights;
    }
 
    const double kernelTime = x + kernels::IrwinHallCDF2.getBeginning();
    Weights cumulative(N);
    for (size_t index = 0; index < N; ++index) {
      cumulative(static_cast<Eigen::Index>(index)) =
          kernels::IrwinHallCDF2.evaluateDerivative(
              derivative, kernelTime - static_cast<double>(index));
    }
 
    Weights weights(N);
    weights(0) = derivative == 0 ? 1.0 - cumulative(1) : -cumulative(1);
    for (size_t index = 1; index + 1 < N; ++index) {
      weights(static_cast<Eigen::Index>(index)) =
          cumulative(static_cast<Eigen::Index>(index)) -
          cumulative(static_cast<Eigen::Index>(index + 1));
    }
    weights(static_cast<Eigen::Index>(N - 1)) =
        cumulative(static_cast<Eigen::Index>(N - 1));
    return weights;
  }
};
 
}  // namespace gtsam