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 <gtsam/basis/ParameterMatrix.h>

#include <iostream>

namespace gtsam {

using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */

template <size_t M>
Matrix kroneckerProductIdentity(const Weights& w) {
  Matrix result(M, w.cols() * M);
  result.setZero();

  for (int i = 0; i < w.cols(); i++) {
    result.block(0, i * M, M, M).diagonal().array() = w(i);
  }
  return result;
}

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;
    }
  };

  template <int M>
  class VectorEvaluationFunctor : protected EvaluationFunctor {
   protected:
    using VectorM = Eigen::Matrix<double, M, 1>;
    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
    Jacobian H_;

    void calculateJacobian() {
      H_ = kroneckerProductIdentity<M>(this->weights_);
    }

   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VectorEvaluationFunctor() {}

    VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
      calculateJacobian();
    }

    VectorEvaluationFunctor(size_t N, double x, double a, double b)
        : EvaluationFunctor(N, x, a, b) {
      calculateJacobian();
    }

    VectorM apply(const ParameterMatrix<M>& P,
                  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.matrix() * this->weights_.transpose();
    }

    VectorM operator()(const ParameterMatrix<M>& P,
                       OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };

  template <int M>
  class VectorComponentFunctor : public EvaluationFunctor {
   protected:
    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
    size_t rowIndex_;
    Jacobian H_;

    /*
     * 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(size_t N) {
      H_.setZero(1, M * N);
      for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
        H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
    }

   public:
    VectorComponentFunctor() {}

    VectorComponentFunctor(size_t N, size_t i, double x)
        : EvaluationFunctor(N, x), rowIndex_(i) {
      calculateJacobian(N);
    }

    VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
        : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
      calculateJacobian(N);
    }

    double apply(const ParameterMatrix<M>& P,
                 OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
    }

    double operator()(const ParameterMatrix<M>& P,
                      OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };

  template <class T>
  class ManifoldEvaluationFunctor
      : public VectorEvaluationFunctor<traits<T>::dimension> {
    enum { M = traits<T>::dimension };
    using Base = VectorEvaluationFunctor<M>;

   public:
    ManifoldEvaluationFunctor() {}

    ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}

    ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
        : Base(N, x, a, b) {}

    T apply(const ParameterMatrix<M>& 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 ParameterMatrix<M>& 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
    }
  };

  template <int M>
  class VectorDerivativeFunctor : protected DerivativeFunctorBase {
   protected:
    using VectorM = Eigen::Matrix<double, M, 1>;
    using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
    Jacobian H_;

    void calculateJacobian() {
      H_ = kroneckerProductIdentity<M>(this->weights_);
    }

   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VectorDerivativeFunctor() {}

    VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
      calculateJacobian();
    }

    VectorDerivativeFunctor(size_t N, double x, double a, double b)
        : DerivativeFunctorBase(N, x, a, b) {
      calculateJacobian();
    }

    VectorM apply(const ParameterMatrix<M>& P,
                  OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.matrix() * this->weights_.transpose();
    }
    VectorM operator()(
        const ParameterMatrix<M>& P,
        OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };

  template <int M>
  class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
   protected:
    using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
    size_t rowIndex_;
    Jacobian H_;

    /*
     * 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(size_t N) {
      H_.setZero(1, M * N);
      for (int j = 0; j < this->weights_.size(); j++)
        H_(0, rowIndex_ + j * M) = this->weights_(j);
    }

   public:
    ComponentDerivativeFunctor() {}

    ComponentDerivativeFunctor(size_t N, size_t i, double x)
        : DerivativeFunctorBase(N, x), rowIndex_(i) {
      calculateJacobian(N);
    }

    ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
        : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
      calculateJacobian(N);
    }
    double apply(const ParameterMatrix<M>& P,
                 OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      if (H) *H = H_;
      return P.row(rowIndex_) * this->weights_.transpose();
    }
    double operator()(const ParameterMatrix<M>& P,
                      OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
      return apply(P, H);
    }
  };

};

}  // namespace gtsam