Loading...
Searching...
No Matches
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 = boost::none) const {
if (H) *H = weights_;
return (weights_ * p)(0);
}
double operator()(const typename DERIVED::Parameters& p,
OptionalJacobian<-1, -1> H = boost::none) 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 = boost::none) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
VectorM operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = boost::none) 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 = boost::none) const {
if (H) *H = H_;
return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
}
double operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) 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 = boost::none) 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 = boost::none) 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 = boost::none) const {
if (H) *H = this->weights_;
return (this->weights_ * p)(0);
}
double operator()(const typename DERIVED::Parameters& p,
OptionalJacobian</*1xN*/ -1, -1> H = boost::none) 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 = boost::none) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
VectorM operator()(
const ParameterMatrix<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) 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 = boost::none) const {
if (H) *H = H_;
return P.row(rowIndex_) * this->weights_.transpose();
}
double operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
return apply(P, H);
}
};
};
} // namespace gtsam
Matrix.h
typedef and functions to augment Eigen's MatrixXd
OptionalJacobian.h
Special class for optional Jacobian arguments.
ParameterMatrix.h
Define ParameterMatrix class which is used to store values at interpolation points.
gtsam::kroneckerProductIdentity
Matrix kroneckerProductIdentity(const Weights &w)
Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix ...
Definition Basis.h:85
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::apply
DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
free versions of apply
Definition DecisionTree.h:413
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
gtsam::Basis::WeightMatrix
static Matrix WeightMatrix(size_t N, const Vector &X, double a, double b)
Calculate weights for all x in vector X, with interval [a,b].
Definition Basis.h:123
gtsam::Basis::WeightMatrix
static Matrix WeightMatrix(size_t N, const Vector &X)
Calculate weights for all x in vector X.
Definition Basis.h:107
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition Basis.h:146
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition Basis.h:150
gtsam::Basis::EvaluationFunctor::apply
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
Regular 1D evaluation.
Definition Basis.h:154
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor()
For serialization.
Definition Basis.h:143
gtsam::Basis::EvaluationFunctor::operator()
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:161
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
VectorEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:208
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition Basis.h:200
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
VectorEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:203
gtsam::Basis::VectorEvaluationFunctor::calculateJacobian
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:192
gtsam::Basis::VectorEvaluationFunctor::operator()
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:221
gtsam::Basis::VectorEvaluationFunctor::apply
VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
M-dimensional evaluation.
Definition Basis.h:214
gtsam::Basis::VectorComponentFunctor::operator()
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:280
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor()
For serialization.
Definition Basis.h:258
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:261
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:267
gtsam::Basis::VectorComponentFunctor::apply
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate component of component rowIndex_ of P.
Definition Basis.h:273
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:313
gtsam::Basis::ManifoldEvaluationFunctor::apply
T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Manifold evaluation.
Definition Basis.h:317
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor()
For serialization.
Definition Basis.h:307
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:310
gtsam::Basis::ManifoldEvaluationFunctor::operator()
T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:336
gtsam::Basis::DerivativeFunctorBase
Base class for functors below that calculate derivative weights.
Definition Basis.h:343
gtsam::Basis::DerivativeFunctorBase::DerivativeFunctorBase
DerivativeFunctorBase()
For serialization.
Definition Basis.h:349
gtsam::Basis::DerivativeFunctor::DerivativeFunctor
DerivativeFunctor()
For serialization.
Definition Basis.h:372
gtsam::Basis::DerivativeFunctor::operator()
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:385
gtsam::Basis::VectorDerivativeFunctor::calculateJacobian
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:414
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
VectorDerivativeFunctor(size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition Basis.h:430
gtsam::Basis::VectorDerivativeFunctor::operator()
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:441
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition Basis.h:422
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
VectorDerivativeFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:425
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:482
gtsam::Basis::ComponentDerivativeFunctor::operator()
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:499
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor()
For serialization.
Definition Basis.h:479
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:488
gtsam::Basis::ComponentDerivativeFunctor::apply
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate derivative of component rowIndex_ of F.
Definition Basis.h:493
gtsam::ParameterMatrix
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38