12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

starting transition to kitchen sink trait class

release/4.3a0
Mike Bosse 2014-12-12 11:43:54 +01:00
parent 6f992e623e
commit cc9ab9c6ed
1 changed files with 147 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

342
gtsam/base/concepts.h
View File

@ -18,15 +18,7 @@
namespace gtsam { namespace gtsam {
namespace traits { template <typename T> struct traits {};
/**
* @name Algebraic Structure Traits
* @brief Associate a unique tag with each of the main GTSAM concepts
*/
//@{
template<typename T> struct structure_category;
//@}
/** /**
* @name Algebraic Structure Tags * @name Algebraic Structure Tags
@ -39,46 +31,110 @@ struct lie_group_tag: public manifold_tag, public group_tag {};
struct vector_space_tag: public lie_group_tag {}; struct vector_space_tag: public lie_group_tag {};
//@} //@}
}// namespace traits // Group operator syntax flavors
struct multiplicative_group_tag {};
struct additive_group_tag {};
namespace manifold { template<typename Transformation>
struct traits<Transformation> {
/** @name Free functions any Manifold needs to define */ // Typedefs required by all manifold types.
//@{ typedef multiplicative_group_tag group_flavor;
//@} typedef lie_group_tag structure_category;
typedef Transformation ManifoldType;
namespace traits { enum { dimension = 6 };
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/** @name Manifold Traits */ // Required by all Manifold types.
//@{ static TangentVector Local(const ManifoldType& origin,
template<typename Manifold> struct dimension; const ManifoldType& other);
template<typename Manifold> struct TangentVector;
template<typename Manifold> struct DefaultChart; static ManifoldType Retract(const ManifoldType& origin,
template<typename Manifold> struct ChartJacobian { const TangentVector& v);
typedef OptionalJacobian<dimension<Manifold>::value, dimension<Manifold>::value> value;
static int GetDimension(const ManifoldType& m){ return dimension; }
// For Group. Only implemented for groups
static ManifoldType Compose(const ManifoldType& m1,
const ManifoldType& m2);
static ManifoldType Between(const ManifoldType& m1,
const ManifoldType& m2);
static ManifoldType Inverse(const ManifoldType& m);
static Vector3 Act(const ManifoldType& T,
const Vector3& p);
static Vector3 Act(const ManifoldType& T,
const Vector3& p,
OptionalJacobian<3,3> Hp);
// For Lie Group. Only implemented for lie groups.
static ManifoldType Compose(const ManifoldType& m1,
const ManifoldType& m2,
ChartJacobian H1,
ChartJacobian H2);
static ManifoldType Between(const ManifoldType& m1,
const ManifoldType& m2,
ChartJacobian H1,
ChartJacobian H2);
static ManifoldType Inverse(const ManifoldType& m,
ChartJacobian H);
static Vector3 Act(const ManifoldType& T,
const Vector3& p,
OptionalJacobian<3, dimension> HT,
OptionalJacobian<3, 3> Hp);
static const ManifoldType Identity;
static TangentVector Local(const ManifoldType& origin,
const ManifoldType& other,
ChartJacobian Horigin,
ChartJacobian Hother);
static ManifoldType Retract(const ManifoldType& origin,
const TangentVector& v,
ChartJacobian Horigin,
ChartJacobian Hv);
static TangentVector Logmap(const ManifoldType& m);
static ManifoldType Expmap(const TangentVector& v);
static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm);
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv);
// For Testable
static void Print(const ManifoldType& T);
static void Equals(const ManifoldType& m1,
const ManifoldType& m2,
double tol = 1e-8);
}; };
//@}
}// \ namespace manifold::traits
/// Check invariants for Manifold type /// Check invariants for Manifold type
template<typename T> template<typename T>
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) // BOOST_CONCEPT_REQUIRES(((Testable<traits<T> >)),(bool)) //
check_invariants(const T& a, const T& b) { check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typedef typename traits::DefaultChart<T>::type Chart; traits<T>::TangentVector v0 = traits<T>::Local(a,a);
// no invariants to check for manifolds, so always true if concept check compiles traits<T>::TangentVector v = traits<T>::Local(a,b);
return true; T c = traits<T>::Retract(a,v);
return v0.norm() < tol && traits<T>::Equals(b,c,tol);
} }
} // \ namespace manifold #define GTSAM_MANIFOLD_DECLARATIONS(MANIFOLD,DIM,TANGENT_VECTOR) \
typedef MANIFOLD ManifoldType;\
typedef manifold_tag structure_category; \
struct dimension : public boost::integral_constant<int, DIM> {};\
typedef TANGENT_VECTOR TangentVector;\
typedef OptionalJacobian<dimension, dimension> ChartJacobian; \
static TangentVector Local(const ManifoldType& origin, \
const ManifoldType& other, \
ChartJacobian Horigin=boost::none, \
ChartJacobian Hother=boost::none); \
static ManifoldType Retract(const ManifoldType& origin, \
const TangentVector& v,\
ChartJacobian Horigin=boost::none, \
ChartJacobian Hv=boost::none); \
static int GetDimension(const ManifoldType& m) { return dimension; }
#define GTSAM_MANIFOLD(TEMPLATE,MANIFOLD,DIM,TANGENT_VECTOR,DEFAULT_CHART) \
namespace manifold { \
namespace traits { \
template<TEMPLATE> struct dimension < MANIFOLD > : public boost::integral_constant<int, DIM> {};\
template<TEMPLATE> struct TangentVector< MANIFOLD > { typedef TANGENT_VECTOR type;};\
template<TEMPLATE> struct DefaultChart < MANIFOLD > { typedef DEFAULT_CHART type;};\
}}
/** /**
* Chart concept * Chart concept
@ -92,12 +148,7 @@ public:
typedef OptionalJacobian<dim, dim> OptionalJacobian; typedef OptionalJacobian<dim, dim> OptionalJacobian;
BOOST_CONCEPT_USAGE(IsChart) { BOOST_CONCEPT_USAGE(IsChart) {
// make sure methods in Chart are defined
v = C::Local(p,q);
q = C::Retract(p,v);
// and the versions with Jacobians.
v = C::Local(p,q,Hp,Hq);
q = C::Retract(p,v,Hp,Hv);
} }
private: private:
ManifoldType p,q; ManifoldType p,q;
@ -111,89 +162,40 @@ private:
template<typename M> template<typename M>
class IsManifold { class IsManifold {
public: public:
typedef typename traits::structure_category<M>::type structure_category_tag; typedef typename traits<M>::structure_category structure_category_tag;
static const size_t dim = manifold::traits::dimension<M>::value; static const size_t dim = traits<M>::dimension;
typedef typename manifold::traits::TangentVector<M>::type TangentVector; typedef typename traits<M>::ManifoldType ManifoldType;
typedef typename manifold::traits::DefaultChart<M>::type DefaultChart; typedef typename traits<M>::TangentVector TangentVector;
typedef typename traits<M>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsManifold) { BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG( BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<traits::manifold_tag, structure_category_tag>::value), (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)"); "This type's structure_category trait does not assert it as a manifold (or derived)");
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
BOOST_CONCEPT_ASSERT((IsChart<DefaultChart >));
// make sure methods in Chart are defined
v = traits<M>::Local(p,q);
q = traits<M>::Retract(p,v);
// and the versions with Jacobians.
v = traits<M>::Local(p,q,Hp,Hq);
q = traits<M>::Retract(p,v,Hp,Hv);
} }
private:
ManifoldType p,q;
ChartJacobian Hp,Hq,Hv;
TangentVector v;
}; };
template<typename Manifold>
struct Group;
namespace group {
/** @name Free functions any Group needs to define */
//@{
template<typename G> G compose(const G& g, const G& h);
template<typename G> G between(const G& g, const G& h);
template<typename G> G inverse(const G& g);
template<typename G, typename S> S act(const G& g, const S& s);
//@}
namespace traits {
/** @name Group Traits */
//@{
template<typename G> struct identity;
template<typename G> struct flavor;
//@}
/** @name Group Flavor Tags */
//@{
struct additive_tag {};
struct multiplicative_tag {};
//@}
}// \ namespace traits
/// Check invariants /// Check invariants
template<typename G> template<typename G>
BOOST_CONCEPT_REQUIRES(((Testable<G>)),(bool)) // BOOST_CONCEPT_REQUIRES(((Testable<G>,IsGroup<G>)),(bool)) //
check_invariants(const G& a, const G& b, double tol = 1e-9) { check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits::identity<G>::value; G e = traits<G>::identity;
return compose(a, inverse(a)).equals(e, tol) return traits<G>::Equals(traits<G>::Compose(a, traits<G>::inverse(a)), e, tol)
&& between(a, b).equals(compose(inverse(a), b), tol) && traits<G>::Equals(traits<G>::Between(a, b), traits<G>::Compose(traits<G>::Inverse(a), b), tol)
&& compose(a, between(a, b)).equals(b, tol); && traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
} }
} // \ namespace group
#define GTSAM_GROUP_IDENTITY0(GROUP) \
namespace group { namespace traits { \
template<> struct identity<GROUP > { static const GROUP value; typedef GROUP value_type;};\
const GROUP identity<GROUP >::value = GROUP::Identity();\
}}
#define GTSAM_GROUP_IDENTITY(TEMPLATE,GROUP) \
namespace group { namespace traits { \
template<TEMPLATE> struct identity<GROUP > { static const GROUP value; typedef GROUP value_type;};\
template<TEMPLATE> const GROUP identity<GROUP >::value = GROUP::Identity();\
}}
#define GTSAM_ADDITIVE_GROUP(TEMPLATE,GROUP) \
namespace group { \
template<TEMPLATE> GROUP compose(const GROUP &g, const GROUP & h) { return g + h;} \
template<TEMPLATE> GROUP between(const GROUP &g, const GROUP & h) { return h - g;} \
template<TEMPLATE> GROUP inverse(const GROUP &g) { return -g;} \
namespace traits { \
template<TEMPLATE> struct flavor<GROUP > { typedef additive_tag type;};\
}}
#define GTSAM_MULTIPLICATIVE_GROUP(TEMPLATE,GROUP) \
namespace group { \
template<TEMPLATE> GROUP compose(const GROUP &g, const GROUP & h) { return g * h;} \
template<TEMPLATE> GROUP between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
template<TEMPLATE> GROUP inverse(const GROUP &g) { return g.inverse();} \
namespace traits { \
template<TEMPLATE> struct flavor<GROUP > { typedef multiplicative_tag type;};\
}}
/** /**
* Group Concept * Group Concept
@ -201,29 +203,29 @@ template<TEMPLATE> struct flavor<GROUP > { typedef multiplicative_tag type;};\
template<typename G> template<typename G>
class IsGroup { class IsGroup {
public: public:
typedef typename traits<G>::structure_category structure_category_tag;
typedef typename traits::structure_category<G>::type structure_category_tag; typedef typename traits<G>::group_flavor flavor_tag;
typedef typename group::traits::identity<G>::value_type identity_value_type; static const size_t dim = traits<G>::dimension;
typedef typename group::traits::flavor<G>::type flavor_tag; //typedef typename traits<G>::identity::value_type identity_value_type;
BOOST_CONCEPT_USAGE(IsGroup) { BOOST_CONCEPT_USAGE(IsGroup) {
BOOST_STATIC_ASSERT_MSG( BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<traits::group_tag, structure_category_tag>::value), (boost::is_base_of<group_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a group (or derived)"); "This type's structure_category trait does not assert it as a group (or derived)");
e = group::traits::identity<G>::value; e = traits<G>::identity;
e = group::compose(g, h); e = traits<G>::Compose(g, h);
e = group::between(g, h); e = traits<G>::Between(g, h);
e = group::inverse(g); e = traits<G>::Inverse(g);
operator_usage(flavor); operator_usage(flavor);
// todo: how do we test the act concept? or do we even need to? // todo: how do we test the act concept? or do we even need to?
} }
private: private:
void operator_usage(group::traits::multiplicative_tag) { void operator_usage(multiplicative_group_tag) {
e = g * h; e = g * h;
//e = -g; // todo this should work, but it is failing for Quaternions //e = -g; // todo this should work, but it is failing for Quaternions
} }
void operator_usage(group::traits::additive_tag) { void operator_usage(additive_group_tag) {
e = g + h; e = g + h;
e = h - g; e = h - g;
e = -g; e = -g;
@ -234,33 +236,6 @@ private:
}; };
template<typename ManifoldType>
struct LieGroup;
namespace lie_group {
/** @name Free functions any Lie Group needs to define */
//@{
template<typename LG> LG compose(const LG& g, const LG& h,
manifold::traits::ChartJacobian<LG>::type Hg, manifold::traits::ChartJacobian<LG>::type Hh);
template<typename LG> LG between(const LG& g, const LG& h,
manifold::traits::ChartJacobian<LG>::type Hg, manifold::traits::ChartJacobian<LG>::type Hh);
template<typename LG> LG inverse(const LG& g, manifold::ChartJacobian<LG>::type Hg);
template<typename LG> typename manifold::traits::TangentVector<LG>::type logmap(const LG & g);
template<typename LG> LG expmap(const typename manifold::traits::TangentVector<LG>::type& v);
template<typename LG> manifold::traits::TangentVector<LG>::type log(const LG& v);
//@}
namespace traits {
/** @name Lie Group Traits */
//@{
//@}
}// \ namespace traits
/// Check invariants /// Check invariants
//template<typename LG> //template<typename LG>
//BOOST_CONCEPT_REQUIRES(((Testable<LG>)),(bool)) check_invariants(const LG& a, //BOOST_CONCEPT_REQUIRES(((Testable<LG>)),(bool)) check_invariants(const LG& a,
@ -270,7 +245,6 @@ namespace traits {
// && equal(Chart::Local(a, b), b - a); // && equal(Chart::Local(a, b), b - a);
// } // }
//} //}
}// \ namespace lie_group
/** /**
* Lie Group Concept * Lie Group Concept
@ -278,65 +252,43 @@ namespace traits {
template<typename LG> template<typename LG>
class IsLieGroup: public IsGroup<LG>, public IsManifold<LG> { class IsLieGroup: public IsGroup<LG>, public IsManifold<LG> {
public: public:
typedef typename traits<LG>::structure_category structure_category_tag;
typedef typename traits<LG>::ManifoldType ManifoldType;
typedef typename traits<LG>::TangentVector TangentVector;
typedef typename traits<LG>::ChartJacobian ChartJacobian;
typedef typename traits::structure_category<LG>::type structure_category_tag;
typedef OptionalJacobian<IsManifold<LG>::dim, IsManifold<LG>::dim> OptionalJacobian;
typedef typename manifold::traits::TangentVector<LG>::type V;
BOOST_CONCEPT_USAGE(IsLieGroup) { BOOST_CONCEPT_USAGE(IsLieGroup) {
BOOST_STATIC_ASSERT_MSG( BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<traits::lie_group_tag, structure_category_tag>::value), (boost::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)"); "This type's trait does not assert it is a Lie group (or derived)");
// TODO Check with Jacobian
using lie_group::compose; // group opertations with Jacobians
using lie_group::between; g = traits<LG>::Compose(g, h, Hg, Hh);
using lie_group::inverse; g = traits<LG>::Between(g, h, Hg, Hh);
g = compose(g, h, Hg, Hh); g = traits<LG>::Inverse(g, Hg);
g = between(g, h, Hg, Hh); // log and exp map without Jacobians
g = inverse(g, Hg); g = traits<LG>::Expmap(v);
g = lie_group::expmap<LG>(v); v = traits<LG>::Logmap(g);
v = lie_group::logmap<LG>(g); // log and exp map with Jacobians
g = traits<LG>::Expmap(v, Hg);
v = traits<LG>::Logmap(g, Hg);
} }
private: private:
LG g, h; LG g, h;
V v; TangentVector v;
OptionalJacobian Hg, Hh; OptionalJacobian Hg, Hh;
}; };
/**
* A Lie Group Chart
* Creates Local/Retract from exponential map and its inverse
* Assumes Expmap and Logmap defined in Derived
*/
template<typename M>
struct LieGroupChart {
typedef M ManifoldType;
typedef typename manifold::traits::TangentVector<ManifoldType>::type TangentVector;
static const int dim = manifold::traits::dimension<ManifoldType>::value;
typedef OptionalJacobian<dim, dim> OptionalJacobian;
/// retract, composes with Expmap around identity
static ManifoldType Retract(const ManifoldType& p, const TangentVector& omega, OptionalJacobian Hp=boost::none, OptionalJacobian Hw=boost::none) {
// todo: use the chain rule with Jacobian of Expmap
return lie_group::compose(p, lie_group::expmap<ManifoldType>(omega), Hp, Hw);
}
/// local is our own, as there is a slight bug in Eigen
static TangentVector Local(const ManifoldType& p, const ManifoldType& q, OptionalJacobian Hp=boost::none, OptionalJacobian Hq=boost::none) {
// todo: use the chain rule with Jacobian of Logmap
return lie_group::logmap<ManifoldType>(lie_group::between(p, q, Hp, Hq));
}
};
template<typename V> template<typename V>
class IsVectorSpace: public IsLieGroup<V> { class IsVectorSpace: public IsLieGroup<V> {
public: public:
typedef typename traits::structure_category<V>::type structure_category_tag; typedef typename traits<V>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) { BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG( BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<traits::vector_space_tag, structure_category_tag>::value), (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)"); "This type's trait does not assert it as a vector space (or derived)");
r = p + q; r = p + q;
r = -p; r = -p;