Loading...
Searching...
No Matches
Manifold.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/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
namespace gtsam {
struct manifold_tag {};
template <typename T> struct traits;
namespace internal {
template<class Class>
struct HasManifoldPrereqs {
enum { dim = Class::dimension };
Class p, q;
Eigen::Matrix<double, dim, 1> v;
OptionalJacobian<dim, dim> Hp, Hq, Hv;
BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
v = p.localCoordinates(q);
q = p.retract(v);
}
};
template<class Class, int N>
struct GetDimensionImpl {
// Compile-time dimensionality
static int GetDimension(const Class&) {
return N;
}
};
template<class Class>
struct GetDimensionImpl<Class, Eigen::Dynamic> {
// Run-time dimensionality
static int GetDimension(const Class& m) {
return m.dim();
}
};
template<class Class>
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
// Dimension of the manifold
enum { dimension = Class::dimension };
// Typedefs required by all manifold types.
typedef Class ManifoldType;
typedef manifold_tag structure_category;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
// Local coordinates
static TangentVector Local(const Class& origin, const Class& other) {
return origin.localCoordinates(other);
}
// Retraction back to manifold
static Class Retract(const Class& origin, const TangentVector& v) {
return origin.retract(v);
}
};
template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
} // \ namespace internal
template<typename T>
BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
typename traits<T>::TangentVector v = traits<T>::Local(a,b);
T c = traits<T>::Retract(a,v);
return v0.norm() < tol && traits<T>::Equals(b,c,tol);
}
template<typename T>
class IsManifold {
public:
typedef typename traits<T>::structure_category structure_category_tag;
static const int dim = traits<T>::dimension;
typedef typename traits<T>::ManifoldType ManifoldType;
typedef typename traits<T>::TangentVector TangentVector;
BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a manifold (or derived)");
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
// make sure Chart methods are defined
v = traits<T>::Local(p, q);
q = traits<T>::Retract(p, v);
}
private:
TangentVector v;
ManifoldType p, q;
};
template<typename T>
struct FixedDimension {
typedef const int value_type;
static const int value = traits<T>::dimension;
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type.");
};
} // \ namespace gtsam
// * Macros for using the ManifoldConcept
// * - An instantiation for use inside unit tests
// * - A typedef for use inside generic algorithms
// *
// * NOTE: intentionally not in the gtsam namespace to allow for classes not in
// * the gtsam namespace to be more easily enforced as testable
// */
#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;
Testable.h
Concept check for values that can be used in unit tests.
Matrix.h
typedef and functions to augment Eigen's MatrixXd
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::BOOST_CONCEPT_REQUIRES
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
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::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
gtsam::internal::HasManifoldPrereqs
Requirements on type to pass it to Manifold template below.
Definition Manifold.h:59
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
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::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151