Loading...
Searching...
No Matches
VectorSpace.h
/*
* VectorSpace.h
*
* @date December 21, 2014
* @author Mike Bosse
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/base/Lie.h>
namespace gtsam {
struct vector_space_tag: public lie_group_tag {
};
template<typename T> struct traits;
namespace internal {
template<class Class, int N>
struct VectorSpaceImpl {
typedef Eigen::Matrix<double, N, 1> TangentVector;
typedef OptionalJacobian<N, N> ChartJacobian;
typedef Eigen::Matrix<double, N, N> Jacobian;
static int GetDimension(const Class&) { return N;}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
Class v = other-origin;
return v.vector();
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return origin + v;
}
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
if (Hm) *Hm = Jacobian::Identity();
return m.vector();
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
if (Hv) *Hv = Jacobian::Identity();
return Class(v);
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
ChartJacobian H2 = boost::none) {
if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return v2 - v1;
}
static Class Inverse(const Class& v, ChartJacobian H = boost::none) {
if (H) *H = - Jacobian::Identity();
return -v;
}
};
template<class Class>
struct VectorSpaceImpl<Class,Eigen::Dynamic> {
static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
static Class Inverse(const Class& m) { return -m;}
typedef Eigen::VectorXd TangentVector;
typedef OptionalJacobian<Eigen::Dynamic,Eigen::Dynamic> ChartJacobian;
static int GetDimension(const Class& m) { return m.dim();}
static Eigen::MatrixXd Eye(const Class& m) {
int dim = GetDimension(m);
return Eigen::MatrixXd::Identity(dim, dim);
}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Eye(origin);
if (H2) *H2 = Eye(other);
Class v = other-origin;
return v.vector();
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
return origin + v;
}
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
if (Hm) *Hm = Eye(m);
return m.vector();
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
Class result(v);
if (Hv)
*Hv = Eye(v);
return result;
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v2);
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Eye(v1);
if (H2) *H2 = Eye(v2);
return v2 - v1;
}
static Class Inverse(const Class& v, ChartJacobian H) {
if (H) *H = -Eye(v);
return -v;
}
};
template<class Class>
struct HasVectorSpacePrereqs {
enum { dim = Class::dimension };
Class p, q;
Vector v;
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
p = Class::Identity(); // identity
q = p + p; // addition
q = p - p; // subtraction
v = p.vector(); // conversion to vector
q = p + v; // addition of a vector on the right
}
};
template<class Class>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
typedef vector_space_tag structure_category;
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::Identity();}
enum { dimension = Class::dimension};
typedef Class ManifoldType;
};
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
template<typename Scalar>
struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
typedef vector_space_tag structure_category;
static void Print(Scalar m, const std::string& str = "") {
gtsam::print(m, str);
}
static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
return std::abs(v1 - v2) < tol;
}
typedef additive_group_tag group_flavor;
static Scalar Identity() { return 0;}
typedef Scalar ManifoldType;
enum { dimension = 1 };
typedef Eigen::Matrix<double, 1, 1> TangentVector;
typedef OptionalJacobian<1, 1> ChartJacobian;
static TangentVector Local(Scalar origin, Scalar other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = -1.0;
if (H2) (*H2)[0] = 1.0;
TangentVector result;
result(0) = other - origin;
return result;
}
static Scalar Retract(Scalar origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = 1.0;
if (H2) (*H2)[0] = 1.0;
return origin + v[0];
}
static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
if (H) (*H)[0] = 1.0;
return Local(0, m);
}
static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
if (H) (*H)[0] = 1.0;
return v[0];
}
};
} // namespace internal
template<> struct traits<double> : public internal::ScalarTraits<double> {
};
template<> struct traits<float> : public internal::ScalarTraits<float> {
};
// traits for any fixed double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
internal::VectorSpaceImpl<
Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
typedef vector_space_tag structure_category;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
static void Print(const Fixed& m, const std::string& str = "") {
gtsam::print(Eigen::MatrixXd(m), str);
}
static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) {
return equal_with_abs_tol(v1, v2, tol);
}
typedef additive_group_tag group_flavor;
static Fixed Identity() { return Fixed::Zero();}
enum { dimension = M*N};
typedef Fixed ManifoldType;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(const Fixed& origin, const Fixed& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1) = -Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = other - origin;
return result;
}
static Fixed Retract(const Fixed& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1) = Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
return origin + Eigen::Map<const Fixed>(v.data());
}
static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
if (H) *H = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = m;
return result;
}
static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
Fixed m;
m.setZero();
if (H) *H = Jacobian::Identity();
return m + Eigen::Map<const Fixed>(v.data());
}
};
namespace internal {
// traits for dynamic Eigen matrices
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct DynamicTraits {
typedef vector_space_tag structure_category;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Dynamic;
static void Print(const Dynamic& m, const std::string& str = "") {
gtsam::print(Eigen::MatrixXd(m), str);
}
static bool Equals(const Dynamic& v1, const Dynamic& v2,
double tol = 1e-8) {
return equal_with_abs_tol(v1, v2, tol);
}
typedef additive_group_tag group_flavor;
static Dynamic Identity() {
throw std::runtime_error("Identity not defined for dynamic types");
}
enum { dimension = Eigen::Dynamic };
typedef Eigen::VectorXd TangentVector;
typedef Eigen::MatrixXd Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Dynamic ManifoldType;
static int GetDimension(const Dynamic& m) {
return m.rows() * m.cols();
}
static Jacobian Eye(const Dynamic& m) {
int dim = GetDimension(m);
return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
}
static TangentVector Local(const Dynamic& m, const Dynamic& other, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = -Eye(m);
if (H2) *H2 = Eye(m);
TangentVector v(GetDimension(m));
Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
return v;
}
static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(m);
if (H2) *H2 = Eye(m);
return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
}
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) {
if (H) *H = Eye(m);
TangentVector result(GetDimension(m));
Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
return result;
}
static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) {
static_cast<void>(H);
throw std::runtime_error("Expmap not defined for dynamic types");
}
static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) {
if (H) *H = -Eye(m);
return -m;
}
static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v1);
return v1 + v2;
}
static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = -Eye(v1);
if (H2) *H2 = Eye(v1);
return v2 - v1;
}
};
} // \ internal
// traits for fully dynamic matrix
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, -1, -1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> {
};
// traits for dynamic column vector
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, -1, 1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> {
};
// traits for dynamic row vector
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, 1, -1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> {
};
template<typename T>
class IsVectorSpace: public IsLieGroup<T> {
public:
typedef typename traits<T>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<vector_space_tag, structure_category_tag>::value),
"This type's trait does not assert it as a vector space (or derived)");
r = p + q;
r = -p;
r = p - q;
}
private:
T p, q, r;
};
} // namespace gtsam
Lie.h
Base class and basic functions for Lie types.
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::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
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::additive_group_tag
Definition Group.h:38
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
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::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition OptionalJacobian.h:171
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::vector_space_tag
tag to assert a type is a vector space
Definition VectorSpace.h:16
gtsam::internal::VectorSpaceImpl
VectorSpaceTraits Implementation for Fixed sizes.
Definition VectorSpace.h:25
gtsam::internal::HasVectorSpacePrereqs
Requirements on type to pass it to Manifold template below.
Definition VectorSpace.h:164
gtsam::internal::VectorSpaceTraits
A helper that implements the traits interface for classes that define vector spaces To use this for y...
Definition VectorSpace.h:185
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
gtsam::internal::ScalarTraits
A helper that implements the traits interface for scalar vector spaces.
Definition VectorSpace.h:212
gtsam::internal::DynamicTraits
Definition VectorSpace.h:354
gtsam::IsVectorSpace
Vector Space concept.
Definition VectorSpace.h:470