Basis.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/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
 
#include <iostream>
 
namespace gtsam {
 
using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
 
Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w);
 
template <typename DERIVED>
class Basis {
 public:
  static Matrix WeightMatrix(size_t N, const Vector& X) {
    Matrix W(X.size(), N);
    for (int i = 0; i < X.size(); i++)
      W.row(i) = DERIVED::CalculateWeights(N, X(i));
    return W;
  }
 
  static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) {
    Matrix W(X.size(), N);
    for (int i = 0; i < X.size(); i++)
      W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
    return W;
  }
 
  class EvaluationFunctor {
   protected:
    Weights weights_;
 
   public:
    EvaluationFunctor() {}
 
    EvaluationFunctor(size_t N, double x)
        : weights_(DERIVED::CalculateWeights(N, x)) {}
 
    EvaluationFunctor(size_t N, double x, double a, double b)
        : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
 
    double apply(const typename DERIVED::Parameters& p,
                 OptionalJacobian<-1, -1> H = {}) const {
      if (H) *H = weights_;
      return (weights_ * p)(0);
    }
 
    double operator()(const typename DERIVED::Parameters& p,
                      OptionalJacobian<-1, -1> H = {}) const {
      return apply(p, H);  // might call apply in derived
    }
 
    void print(const std::string& s = "") const {
      std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
    }
  };
 
  class VectorEvaluationFunctor : protected EvaluationFunctor {
   protected:
    using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
    Jacobian H_;
 
    size_t M_;
 
    void calculateJacobian() {
      H_ = kroneckerProductIdentity(M_, this->weights_);
    }
 
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    VectorEvaluationFunctor() {}
 
    VectorEvaluationFunctor(size_t M, size_t N, double x)
        : EvaluationFunctor(N, x), M_(M) {
      calculateJacobian();
    }
 
    VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b)
        : EvaluationFunctor(N, x, a, b), M_(M) {
      calculateJacobian();
    }
 
    Vector apply(const Matrix& P,
                 OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.matrix() * this->weights_.transpose();
    }
 
    Vector operator()(const Matrix& P,
                      OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };
 
  class VectorComponentFunctor : public EvaluationFunctor {
   protected:
    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
    Jacobian H_;
 
    size_t M_;
    size_t rowIndex_;
 
    /*
     * Calculate the `1*(M*N)` Jacobian of this functor with respect to
     * the M*N parameter matrix `P`.
     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
     *      H=[w(0) 0    w(1) 0    w(2) 0]    for rowIndex==0
     *      H=[0    w(0) 0    w(1) 0    w(2)] for rowIndex==1
     * i.e., one row of the Kronecker product of weights_ with the
     * MxM identity matrix. See also VectorEvaluationFunctor.
     */
    void calculateJacobian() {
      H_.setZero(1, M_ * EvaluationFunctor::weights_.size());
      for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
        H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j);
    }
 
   public:
    VectorComponentFunctor() {}
 
    VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
        : EvaluationFunctor(N, x), M_(M), rowIndex_(i) {
      calculateJacobian();
    }
 
    VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a,
                           double b)
        : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) {
      calculateJacobian();
    }
 
    double apply(const Matrix& P,
                 OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
    }
 
    double operator()(const Matrix& P,
                      OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };
 
  template <class T>
  class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
    inline constexpr static auto M = traits<T>::dimension;
    using Base = VectorEvaluationFunctor;
 
   public:
    ManifoldEvaluationFunctor() {}
 
    ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {}
 
    ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
        : Base(M, N, x, a, b) {}
 
    T apply(const Matrix& P, OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
      // Interpolate the M-dimensional vector to yield a vector in tangent space
      Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
 
      // Now call retract with this M-vector, possibly with derivatives
      Eigen::Matrix<double, M, M> D_result_xi;
      T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
 
      // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
      // derivative of interpolation and D_result_xi is MxM derivative of
      // retract.
      if (H) *H = D_result_xi * (*H);
 
      // and return a T
      return result;
    }
 
    T operator()(const Matrix& P,
                 OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
      return apply(P, H);  // might call apply in derived
    }
  };
 
  class DerivativeFunctorBase {
   protected:
    Weights weights_;
 
   public:
    DerivativeFunctorBase() {}
 
    DerivativeFunctorBase(size_t N, double x)
        : weights_(DERIVED::DerivativeWeights(N, x)) {}
 
    DerivativeFunctorBase(size_t N, double x, double a, double b)
        : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
 
    void print(const std::string& s = "") const {
      std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
    }
  };
 
  class DerivativeFunctor : protected DerivativeFunctorBase {
   public:
    DerivativeFunctor() {}
 
    DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
 
    DerivativeFunctor(size_t N, double x, double a, double b)
        : DerivativeFunctorBase(N, x, a, b) {}
 
    double apply(const typename DERIVED::Parameters& p,
                 OptionalJacobian</*1xN*/ -1, -1> H = {}) const {
      if (H) *H = this->weights_;
      return (this->weights_ * p)(0);
    }
    double operator()(const typename DERIVED::Parameters& p,
                      OptionalJacobian</*1xN*/ -1, -1> H = {}) const {
      return apply(p, H);  // might call apply in derived
    }
  };
 
  class VectorDerivativeFunctor : protected DerivativeFunctorBase {
   protected:
    using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
    Jacobian H_;
 
    size_t M_;
 
    void calculateJacobian() {
      H_ = kroneckerProductIdentity(M_, this->weights_);
    }
 
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    VectorDerivativeFunctor() {}
 
    VectorDerivativeFunctor(size_t M, size_t N, double x)
        : DerivativeFunctorBase(N, x), M_(M) {
      calculateJacobian();
    }
 
    VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b)
        : DerivativeFunctorBase(N, x, a, b), M_(M) {
      calculateJacobian();
    }
 
    Vector apply(const Matrix& P,
                 OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.matrix() * this->weights_.transpose();
    }
    Vector operator()(const Matrix& P,
                      OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };
 
  class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
   protected:
    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
    Jacobian H_;
 
    size_t M_;
    size_t rowIndex_;
 
    /*
     * Calculate the `1*(M*N)` Jacobian of this functor with respect to
     * the M*N parameter matrix `P`.
     * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
     *      H=[w(0) 0    w(1) 0    w(2) 0]    for rowIndex==0
     *      H=[0    w(0) 0    w(1) 0    w(2)] for rowIndex==1
     * i.e., one row of the Kronecker product of weights_ with the
     * MxM identity matrix. See also VectorDerivativeFunctor.
     */
    void calculateJacobian() {
      H_.setZero(1, M_ * this->weights_.size());
      for (int j = 0; j < this->weights_.size(); j++)
        H_(0, rowIndex_ + j * M_) = this->weights_(j);
    }
 
   public:
    ComponentDerivativeFunctor() {}
 
    ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
        : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) {
      calculateJacobian();
    }
 
    ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a,
                               double b)
        : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) {
      calculateJacobian();
    }
    double apply(const Matrix& P,
                 OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.row(rowIndex_) * this->weights_.transpose();
    }
    double operator()(const Matrix& P,
                      OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };
};
 
}  // namespace gtsam