Added Manifold, Lie Group, and Vector Space concepts back in
dellaert
parent
01aab77504
commit
e2f250c160
|
|
@ -13,6 +13,7 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <boost/concept_check.hpp>
|
||||
#include <boost/concept/requires.hpp>
|
||||
#include <boost/static_assert.hpp>
|
||||
#include <boost/type_traits/is_base_of.hpp>
|
||||
|
||||
|
|
@ -25,9 +26,7 @@ namespace traits {
|
|||
* @brief Associate a unique tag with each of the main GTSAM concepts
|
||||
*/
|
||||
//@{
|
||||
template<typename T>
|
||||
struct structure_category;
|
||||
// specializations should be derived from one of the following tags
|
||||
template<typename T> struct structure_category;
|
||||
//@}
|
||||
|
||||
/**
|
||||
|
|
@ -35,81 +34,95 @@ struct structure_category;
|
|||
* @brief Possible values for traits::structure_category<T>::type
|
||||
*/
|
||||
//@{
|
||||
struct manifold_tag {
|
||||
};
|
||||
struct group_tag {
|
||||
};
|
||||
struct lie_group_tag: public manifold_tag, public group_tag {
|
||||
};
|
||||
struct vector_space_tag: public lie_group_tag {
|
||||
};
|
||||
struct manifold_tag {};
|
||||
struct group_tag {};
|
||||
struct lie_group_tag: public manifold_tag, public group_tag {};
|
||||
struct vector_space_tag: public lie_group_tag {};
|
||||
//@}
|
||||
|
||||
}// namespace traits
|
||||
|
||||
namespace manifold {
|
||||
|
||||
/** @name Free functions any Manifold needs to define */
|
||||
//@{
|
||||
//@}
|
||||
|
||||
namespace traits {
|
||||
|
||||
/** @name Manifold Traits */
|
||||
//@{
|
||||
template<typename Manifold> struct dimension;
|
||||
template<typename Manifold> struct TangentVector;
|
||||
template<typename Manifold> struct DefaultChart;
|
||||
//@}
|
||||
|
||||
}// namespace traits
|
||||
}// \ namespace traits
|
||||
|
||||
/*
|
||||
template<typename T>
|
||||
class ManifoldConcept {
|
||||
public:
|
||||
typedef T Manifold;
|
||||
/// Check invariants for Manifold type
|
||||
template<typename T>
|
||||
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
|
||||
check_invariants(const T& a, const T& b) {
|
||||
typedef typename traits::DefaultChart<T>::type Chart;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Base class for Charts
|
||||
* Derived has to implement local and retract as static methods
|
||||
*/
|
||||
template <class T, class Derived>
|
||||
struct Chart {
|
||||
typedef T ManifoldType;
|
||||
typedef typename traits::TangentVector<T>::type TangentVector;
|
||||
typedef typename traits::DefaultChart<T>::type DefaultChart;
|
||||
static const size_t dim = traits::dimension<T>::value;
|
||||
static TangentVector Local(const ManifoldType& p, const ManifoldType& q) {
|
||||
return Derived::local(p, q);
|
||||
}
|
||||
static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) {
|
||||
return Derived::retract(p, v);
|
||||
}
|
||||
protected:
|
||||
Chart() {
|
||||
(void) &Local;
|
||||
(void) &Retract;
|
||||
} // enforce early instantiation.
|
||||
};
|
||||
|
||||
BOOST_CONCEPT_USAGE(ManifoldConcept) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::manifold_tag, traits::structure<Manifold> >);
|
||||
} // \ namespace manifold
|
||||
|
||||
template<typename T>
|
||||
class IsManifold {
|
||||
public:
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
static const size_t dim = manifold::traits::dimension<T>::value;
|
||||
typedef typename manifold::traits::TangentVector<T>::type TangentVector;
|
||||
typedef typename manifold::traits::DefaultChart<T>::type DefaultChart;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsManifold) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::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);
|
||||
// no direct usage for manifold since most usage is through a chart
|
||||
}
|
||||
private:
|
||||
Manifold p;
|
||||
private:
|
||||
T p;
|
||||
TangentVector v;
|
||||
};
|
||||
|
||||
template<typename C>
|
||||
class ChartConcept {
|
||||
public:
|
||||
typedef C Chart;
|
||||
typedef typename traits::Manifold<Chart>::type Manifold;
|
||||
typedef typename traits::TangentVector<Manifold>::type TangentVector;
|
||||
|
||||
BOOST_CONCEPT_USAGE(ChartConcept) {
|
||||
v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p
|
||||
q = Chart::retract(p,v); // returns retracted update of p with v
|
||||
}
|
||||
|
||||
private:
|
||||
Manifold p,q;
|
||||
TangentVector v;
|
||||
|
||||
};
|
||||
*/
|
||||
};
|
||||
|
||||
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 T> T compose(const T&g, const T& h);
|
||||
template<typename T> T between(const T&g, const T& h);
|
||||
template<typename T> T inverse(const T&g);
|
||||
//@}
|
||||
|
||||
namespace traits {
|
||||
|
||||
/** @name Group Traits */
|
||||
//@{
|
||||
template<typename G> struct identity;
|
||||
template<typename G> struct flavor;
|
||||
template<typename T> struct identity;
|
||||
template<typename T> struct flavor;
|
||||
//@}
|
||||
|
||||
/** @name Group Flavor Tags */
|
||||
|
|
@ -120,31 +133,29 @@ struct multiplicative_tag {
|
|||
};
|
||||
//@}
|
||||
|
||||
} // \ namespace traits
|
||||
}// \ namespace traits
|
||||
|
||||
/// Check invariants
|
||||
template <typename G>
|
||||
//BOOST_CONCEPT_REQUIRES((Testable))
|
||||
bool check_invariants(const G& a, const G& b) {
|
||||
G e = traits::identity<G>::value;
|
||||
typename traits::flavor<G>::type flavor;
|
||||
return (equal(compose(a, inverse(a)), e))
|
||||
&& (equal(between(a, b), compose(inverse(a), b)))
|
||||
&& (equal(compose(a, between(a, b)), b)) //
|
||||
&& operator_usage(a, b, flavor);
|
||||
template<typename T>
|
||||
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
|
||||
check_invariants(const T& a, const T& b, double tol = 1e-9) {
|
||||
T e = traits::identity<T>::value;
|
||||
return compose(a, inverse(a)).equals(e, tol)
|
||||
&& between(a, b).equals(compose(inverse(a), b), tol)
|
||||
&& compose(a, between(a, b)).equals<T>(b, tol);
|
||||
}
|
||||
} // \ namespace group
|
||||
|
||||
/**
|
||||
* Group Concept
|
||||
*/
|
||||
template<typename G>
|
||||
template<typename T>
|
||||
class IsGroup {
|
||||
public:
|
||||
|
||||
typedef typename traits::structure_category<G>::type structure_category_tag;
|
||||
typedef typename group::traits::identity<G>::value_type identity_value_type;
|
||||
typedef typename group::traits::flavor<G>::type flavor_tag;
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
typedef typename group::traits::identity<T>::value_type identity_value_type;
|
||||
typedef typename group::traits::flavor<T>::type flavor_tag;
|
||||
|
||||
void operator_usage(group::traits::multiplicative_tag) {
|
||||
g = g * h;
|
||||
|
|
@ -159,9 +170,8 @@ public:
|
|||
using group::compose;
|
||||
using group::between;
|
||||
using group::inverse;
|
||||
BOOST_STATIC_ASSERT(
|
||||
boost::is_base_of<traits::group_tag, structure_category_tag>::value);
|
||||
e = group::traits::identity<G>::value;
|
||||
BOOST_STATIC_ASSERT( boost::is_base_of<traits::group_tag, structure_category_tag>::value, "This type's structure_category trait does not assert it as a group (or derived)");
|
||||
e = group::traits::identity<T>::value;
|
||||
g = compose(g, h);
|
||||
g = between(g, h);
|
||||
g = inverse(g);
|
||||
|
|
@ -170,42 +180,78 @@ public:
|
|||
|
||||
private:
|
||||
flavor_tag flavor;
|
||||
G e, g, h;
|
||||
T e, g, h;
|
||||
};
|
||||
|
||||
/*
|
||||
template <typename L>
|
||||
class LieGroupConcept : public GroupConcept<L>, public ManifoldConcept<L> {
|
||||
namespace lie_group {
|
||||
|
||||
BOOST_CONCEPT_USAGE(LieGroupConcept) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::lie_group_tag, traits::structure<L> >);
|
||||
}
|
||||
};
|
||||
/** @name Free functions any Group needs to define */
|
||||
//@{
|
||||
// TODO need Jacobians
|
||||
//template<typename T> T compose(const T&g, const T& h);
|
||||
//template<typename T> T between(const T&g, const T& h);
|
||||
//template<typename T> T inverse(const T&g);
|
||||
//@}
|
||||
|
||||
template <typename V>
|
||||
class VectorSpaceConcept : public LieGroupConcept {
|
||||
typedef typename traits::DefaultChart<V>::type Chart;
|
||||
typedef typename GroupConcept<V>::identity identity;
|
||||
namespace traits {
|
||||
|
||||
BOOST_CONCEPT_USAGE(VectorSpaceConcept) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::vector_space_tag, traits::structure<L> >);
|
||||
r = p+q;
|
||||
r = -p;
|
||||
r = p-q;
|
||||
}
|
||||
/** @name Lie Group Traits */
|
||||
//@{
|
||||
//@}
|
||||
|
||||
bool check_invariants(const V& a, const V& b) {
|
||||
return equal(compose(a, b), a+b)
|
||||
&& equal(inverse(a), -a)
|
||||
&& equal(between(a, b), b-a)
|
||||
&& equal(Chart::retract(a, b), a+b)
|
||||
&& equal(Chart::local(a, b), b-a);
|
||||
}
|
||||
}// \ namespace traits
|
||||
|
||||
private:
|
||||
V g,q,r;
|
||||
};
|
||||
/// Check invariants
|
||||
//template<typename T>
|
||||
//BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) check_invariants(const T& a,
|
||||
// const T& b) {
|
||||
// bool check_invariants(const V& a, const V& b) {
|
||||
// return equal(Chart::retract(a, b), a + b)
|
||||
// && equal(Chart::local(a, b), b - a);
|
||||
// }
|
||||
//}
|
||||
}// \ namespace lie_group
|
||||
|
||||
/**
|
||||
* Lie Group Concept
|
||||
*/
|
||||
template<typename T>
|
||||
class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
|
||||
public:
|
||||
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsLieGroup) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::lie_group_tag, structure_category_tag>::value,"This type's trait does not assert it as a Lie group (or derived)");
|
||||
// TODO Check with Jacobian
|
||||
// using lie_group::compose;
|
||||
// using lie_group::between;
|
||||
// using lie_group::inverse;
|
||||
// g = compose(g, h);
|
||||
// g = between(g, h);
|
||||
// g = inverse(g);
|
||||
}
|
||||
private:
|
||||
|
||||
T g, h;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
class IsVectorSpace: public IsLieGroup<T> {
|
||||
public:
|
||||
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsVectorSpace) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::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
|
||||
|
||||
|
|
|
|||
|
|
@ -81,6 +81,12 @@ TEST(Cyclic, Ivnverse) {
|
|||
EXPECT_LONGS_EQUAL(1, group::inverse(G(5)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Cyclic , Invariants) {
|
||||
G g(2), h(5);
|
||||
group::check_invariants(g,h);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -20,13 +20,47 @@
|
|||
namespace gtsam {
|
||||
|
||||
namespace traits {
|
||||
/// Define Eigen::Quaternion to be a model of the Group concept
|
||||
|
||||
/// Define Eigen::Quaternion to be a model of the Lie Group concept
|
||||
template<typename S, int O>
|
||||
struct structure_category<Eigen::Quaternion<S, O> > {
|
||||
typedef group_tag type;
|
||||
typedef lie_group_tag type;
|
||||
};
|
||||
|
||||
} // \namespace gtsam::traits
|
||||
|
||||
namespace manifold {
|
||||
|
||||
/// Chart for Eigen Quaternions
|
||||
template<typename S, int O>
|
||||
class QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>,
|
||||
QuaternionChart<S, O> > {
|
||||
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
/// Define the trait that asserts Quaternion manifold has dimension 3
|
||||
template<typename S, int O>
|
||||
struct dimension<Eigen::Quaternion<S, O> > : public boost::integral_constant<
|
||||
int, 3> {
|
||||
};
|
||||
|
||||
/// Define the trait that asserts Quaternion TangentVector is Vector3
|
||||
template<typename S, int O>
|
||||
struct TangentVector<Eigen::Quaternion<S, O> > {
|
||||
typedef Eigen::Matrix<S,3,1,O,3,1> type;
|
||||
};
|
||||
|
||||
/// Define the trait that asserts Quaternion TangentVector is Vector3
|
||||
template<typename S, int O>
|
||||
struct DefaultChart<Eigen::Quaternion<S, O> > {
|
||||
typedef QuaternionChart<S,O> type;
|
||||
};
|
||||
|
||||
} // \namespace gtsam::manifold::traits
|
||||
} // \namespace gtsam::manifold
|
||||
|
||||
namespace group {
|
||||
|
||||
template<typename S, int O>
|
||||
|
|
@ -95,7 +129,9 @@ typedef Quaternion Q; // Typedef
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
|
||||
// BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
|
||||
// BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
|
||||
// BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -103,6 +139,13 @@ TEST(Quaternion , Constructor) {
|
|||
Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Invariants) {
|
||||
Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
|
||||
// group::check_invariants(g,h); Does not satisfy Testable concept (yet!)
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Compose) {
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue