Reverted on Chart base class. But no chart-specific traits needed, as assumed created by us.
parent
a31e596448
commit
cdc0029158
|
@ -146,30 +146,6 @@ TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts.
|
|||
and we also define a limited number of `gtsam::tags` to select the correct implementation
|
||||
of certain functions at compile time (tag dispatching). Charts are done more conventionally, so we start there...
|
||||
|
||||
Interfaces
|
||||
----------
|
||||
|
||||
Because Charts are always written by the user (or automatically generated, see below for vector spaces),
|
||||
we enforce the Chart concept using an abstract base class, acting as an interface:
|
||||
|
||||
```
|
||||
#!c++
|
||||
template <class T, class Derived>
|
||||
struct Chart {
|
||||
typedef T ManifoldType;
|
||||
typedef typename traits::TangentVector<T>::type TangentVector;
|
||||
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.
|
||||
}
|
||||
```
|
||||
|
||||
The [CRTP](http://en.wikipedia.org/wiki/Curiously_recurring_template_pattern) and the protected constructor
|
||||
automatically check for the existence of the methods in the Derived class, whenever a new Chart is created by
|
||||
|
||||
struct MyChart : Chart<MyType,MyChart> { ... }
|
||||
|
||||
Traits
|
||||
------
|
||||
|
||||
|
|
|
@ -67,32 +67,30 @@ check_invariants(const T& a, const T& b) {
|
|||
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;
|
||||
|
||||
// TODO, maybe we need Retract and Local to be unary, or both
|
||||
// TOOD, also, this indirection mechanism does not seem to help
|
||||
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. TODO does not seem to work
|
||||
};
|
||||
|
||||
} // \ namespace manifold
|
||||
|
||||
/**
|
||||
* Chart concept
|
||||
*/
|
||||
template<typename T>
|
||||
class IsChart {
|
||||
public:
|
||||
typedef typename T::ManifoldType ManifoldType;
|
||||
typedef typename manifold::traits::TangentVector<ManifoldType>::type V;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsChart) {
|
||||
// make sure Derived methods in Chart are defined
|
||||
v = T::Local(p,q);
|
||||
q = T::Retract(p,v);
|
||||
}
|
||||
private:
|
||||
ManifoldType p,q;
|
||||
V v;
|
||||
};
|
||||
|
||||
/**
|
||||
* Manifold concept
|
||||
*/
|
||||
template<typename T>
|
||||
class IsManifold {
|
||||
public:
|
||||
|
@ -106,12 +104,7 @@ public:
|
|||
(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);
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
(boost::is_base_of<manifold::Chart<T,DefaultChart>, DefaultChart>::value),
|
||||
"This type's DefaultChart does not derive from manifold::Chart, as required");
|
||||
// make sure Derived methods in Chart are defined
|
||||
v = DefaultChart::local(p,q);
|
||||
q = DefaultChart::retract(p,v);
|
||||
BOOST_CONCEPT_ASSERT((IsChart<DefaultChart >));
|
||||
}
|
||||
private:
|
||||
T p,q;
|
||||
|
|
|
@ -33,13 +33,17 @@ namespace manifold {
|
|||
|
||||
/// Chart for Eigen Quaternions
|
||||
template<typename S, int O>
|
||||
struct QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>,
|
||||
QuaternionChart<S, O> > {
|
||||
typedef Eigen::Quaternion<S, O> Q;
|
||||
typedef typename traits::TangentVector<Q>::type V;
|
||||
struct QuaternionChart {
|
||||
|
||||
// required
|
||||
typedef Eigen::Quaternion<S, O> ManifoldType;
|
||||
|
||||
// internal
|
||||
typedef ManifoldType Q;
|
||||
typedef typename traits::TangentVector<Q>::type Omega;
|
||||
|
||||
/// Exponential map, simply be converting omega to AngleAxis
|
||||
static Q Expmap(const V& omega) {
|
||||
static Q Expmap(const Omega& omega) {
|
||||
double theta = omega.norm();
|
||||
if (std::abs(theta) < 1e-10)
|
||||
return Q::Identity();
|
||||
|
@ -47,12 +51,12 @@ struct QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>,
|
|||
}
|
||||
|
||||
/// retract, simply be converting omega to AngleAxis
|
||||
static Q retract(const Q& p, const V& omega) {
|
||||
static Q Retract(const Q& p, const Omega& omega) {
|
||||
return p * Expmap(omega);
|
||||
}
|
||||
|
||||
/// We use our own Logmap, as there is a slight bug in Eigen
|
||||
static V Logmap(const Q& q) {
|
||||
static Omega Logmap(const Q& q) {
|
||||
using std::acos;
|
||||
using std::sqrt;
|
||||
static const double twoPi = 2.0 * M_PI,
|
||||
|
@ -79,7 +83,7 @@ struct QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>,
|
|||
}
|
||||
|
||||
/// local is our own, as there is a slight bug in Eigen
|
||||
static V local(const Q& q1, const Q& q2) {
|
||||
static Omega Local(const Q& q1, const Q& q2) {
|
||||
return Logmap(q1.inverse() * q2);
|
||||
}
|
||||
};
|
||||
|
@ -176,9 +180,7 @@ typedef Quaternion Q; // Typedef
|
|||
//******************************************************************************
|
||||
TEST(Quaternion , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
|
||||
// not strictly needed
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
|
||||
// not strictly needed
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
|
||||
}
|
||||
|
||||
|
@ -202,8 +204,6 @@ TEST(Quaternion , Local) {
|
|||
typedef manifold::traits::DefaultChart<Q>::type Chart;
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = Chart::Local(q1, q2);
|
||||
cout << expected << endl;
|
||||
cout << actual << endl;
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue