Eigen::Quaternions now satisfy IsManifold, IsGroup, and IsLieGroup! Also reverted back to a simpler treatment of Charts.
Merge branch 'traits/quaternion' into feature/tighteningTraitsrelease/4.3a0
dellaert
commit
e0e2a9b063
|
|
@ -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
|
||||
------
|
||||
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/concept_check.hpp>
|
||||
#include <stdio.h>
|
||||
#include <string>
|
||||
|
||||
|
|
@ -50,17 +51,20 @@ namespace gtsam {
|
|||
* @tparam T is the type this constrains to be testable - assumes print() and equals()
|
||||
*/
|
||||
template <class T>
|
||||
class TestableConcept {
|
||||
static bool checkTestableConcept(const T& d) {
|
||||
class Testable {
|
||||
T t;
|
||||
bool r1,r2;
|
||||
public:
|
||||
|
||||
BOOST_CONCEPT_USAGE(Testable) {
|
||||
// check print function, with optional string
|
||||
d.print(std::string());
|
||||
d.print();
|
||||
t.print(std::string());
|
||||
t.print();
|
||||
|
||||
// check print, with optional threshold
|
||||
double tol = 1.0;
|
||||
bool r1 = d.equals(d, tol);
|
||||
bool r2 = d.equals(d);
|
||||
return r1 && r2;
|
||||
r1 = t.equals(t, tol);
|
||||
r2 = t.equals(t);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -129,6 +133,7 @@ namespace gtsam {
|
|||
*
|
||||
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
|
||||
* the gtsam namespace to be more easily enforced as testable
|
||||
* @deprecated please use BOOST_CONCEPT_ASSERT and
|
||||
*/
|
||||
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept<T>;
|
||||
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;
|
||||
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::Testable<T>;
|
||||
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::Testable<T> _gtsam_Testable_##T;
|
||||
|
|
|
|||
|
|
@ -11,7 +11,9 @@
|
|||
//#include "manifold.h"
|
||||
//#include "chart.h"
|
||||
#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>
|
||||
|
||||
|
|
@ -24,9 +26,7 @@ namespace traits {
|
|||
* @brief Associate a unique tag with each of the main GTSAM concepts
|
||||
*/
|
||||
//@{
|
||||
template<class T>
|
||||
struct structure_category;
|
||||
// specializations should be derived from one of the following tags
|
||||
template<typename T> struct structure_category;
|
||||
//@}
|
||||
|
||||
/**
|
||||
|
|
@ -34,81 +34,98 @@ 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<class Manifold> struct TangentVector;
|
||||
template<class Manifold> struct DefaultChart;
|
||||
template<typename Manifold> struct dimension;
|
||||
template<typename Manifold> struct TangentVector;
|
||||
template<typename Manifold> struct DefaultChart;
|
||||
//@}
|
||||
|
||||
}// namespace traits
|
||||
}// \ namespace traits
|
||||
|
||||
/*
|
||||
template<class T>
|
||||
class ManifoldConcept {
|
||||
public:
|
||||
typedef T Manifold;
|
||||
typedef typename traits::TangentVector<T>::type TangentVector;
|
||||
typedef typename traits::DefaultChart<T>::type DefaultChart;
|
||||
static const size_t dim = traits::dimension<T>::value;
|
||||
/// 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;
|
||||
}
|
||||
|
||||
BOOST_CONCEPT_USAGE(ManifoldConcept) {
|
||||
BOOST_STATIC_ASSERT(boost::is_base_of<traits::manifold_tag, traits::structure<Manifold> >);
|
||||
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
|
||||
// no direct usage for manifold since most usage is through a chart
|
||||
}
|
||||
private:
|
||||
Manifold p;
|
||||
TangentVector v;
|
||||
};
|
||||
} // \ namespace manifold
|
||||
|
||||
template<class 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;
|
||||
|
||||
};
|
||||
/**
|
||||
* 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:
|
||||
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_MSG(
|
||||
(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_CONCEPT_ASSERT((IsChart<DefaultChart >));
|
||||
}
|
||||
private:
|
||||
T 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<class G> struct identity;
|
||||
template<class G> struct flavor;
|
||||
template<typename T> struct identity;
|
||||
template<typename T> struct flavor;
|
||||
//@}
|
||||
|
||||
/** @name Group Flavor Tags */
|
||||
|
|
@ -119,94 +136,131 @@ struct multiplicative_tag {
|
|||
};
|
||||
//@}
|
||||
|
||||
} // \ namespace traits
|
||||
}// \ namespace traits
|
||||
|
||||
/// Check invariants
|
||||
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>
|
||||
class Group {
|
||||
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;
|
||||
|
||||
BOOST_CONCEPT_USAGE(Group) {
|
||||
void operator_usage(group::traits::multiplicative_tag) {
|
||||
g = g * h;
|
||||
}
|
||||
void operator_usage(group::traits::additive_tag) {
|
||||
g = g + h;
|
||||
g = h - g;
|
||||
g = -g;
|
||||
}
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsGroup) {
|
||||
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;
|
||||
d = compose(g, h);
|
||||
d = between(g, h);
|
||||
ig = inverse(g);
|
||||
test = operator_usage(g, h, flavor);
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
(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);
|
||||
operator_usage(flavor);
|
||||
}
|
||||
|
||||
// TODO: these all require default constructors :-(
|
||||
// Also, requires equal which is not required of a group
|
||||
// Group():e(group::traits::identity<G>::value) {
|
||||
// }
|
||||
//
|
||||
// bool check_invariants(const G& a, const G& b) {
|
||||
// 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);
|
||||
// }
|
||||
|
||||
private:
|
||||
flavor_tag flavor;
|
||||
G e, g, h, gh, ig, d;
|
||||
bool test, test2;
|
||||
|
||||
bool operator_usage(const G& a, const G& b,
|
||||
group::traits::multiplicative_tag) {
|
||||
// return group::compose(a, b) == a * b;
|
||||
return true;
|
||||
}
|
||||
bool operator_usage(const G& a, const G& b, group::traits::additive_tag) {
|
||||
return group::compose(a, b) == a + b;
|
||||
}
|
||||
|
||||
T e, g, h;
|
||||
};
|
||||
|
||||
/*
|
||||
template <class 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 <class 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_MSG(
|
||||
(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_MSG(
|
||||
(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
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Cyclic, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((Group<G>));
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<G>));
|
||||
EXPECT_LONGS_EQUAL(0, group::traits::identity<G>::value);
|
||||
G g(2), h(3);
|
||||
// EXPECT(Group<G>().check_invariants(g,h))
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -19,47 +19,134 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/// Typedef to an Eigen Quaternion<double>, we disable alignment because
|
||||
/// geometry objects are stored in boost pool allocators, in Values
|
||||
/// containers, and and these pool allocators do not support alignment.
|
||||
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
|
||||
namespace traits {
|
||||
|
||||
/// 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 lie_group_tag type;
|
||||
};
|
||||
|
||||
} // \namespace gtsam::traits
|
||||
|
||||
namespace manifold {
|
||||
|
||||
/// Chart for Eigen Quaternions
|
||||
template<typename S, int O>
|
||||
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 Omega& omega) {
|
||||
double theta = omega.norm();
|
||||
if (std::abs(theta) < 1e-10)
|
||||
return Q::Identity();
|
||||
return Q(Eigen::AngleAxisd(theta, omega / theta));
|
||||
}
|
||||
|
||||
/// retract, simply be converting omega to AngleAxis
|
||||
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 Omega Logmap(const Q& q) {
|
||||
using std::acos;
|
||||
using std::sqrt;
|
||||
static const double twoPi = 2.0 * M_PI,
|
||||
// define these compile time constants to avoid std::abs:
|
||||
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
|
||||
|
||||
const double qw = q.w();
|
||||
if (qw > NearlyOne) {
|
||||
// Taylor expansion of (angle / s) at 1
|
||||
return (2 - 2 * (qw - 1) / 3) * q.vec();
|
||||
} else if (qw < NearlyNegativeOne) {
|
||||
// Angle is zero, return zero vector
|
||||
return Vector3::Zero();
|
||||
} else {
|
||||
// Normal, away from zero case
|
||||
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
|
||||
// Important: convert to [-pi,pi] to keep error continuous
|
||||
if (angle > M_PI)
|
||||
angle -= twoPi;
|
||||
else if (angle < -M_PI)
|
||||
angle += twoPi;
|
||||
return (angle / s) * q.vec();
|
||||
}
|
||||
}
|
||||
|
||||
/// local is our own, as there is a slight bug in Eigen
|
||||
static Omega Local(const Q& q1, const Q& q2) {
|
||||
return Logmap(q1.inverse() * q2);
|
||||
}
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
/// Define Quaternion to be a model of the Group concept
|
||||
template<>
|
||||
struct structure_category<Quaternion> {
|
||||
typedef group_tag type;
|
||||
|
||||
/// 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> {
|
||||
};
|
||||
} // \namespace gtsam::traits
|
||||
|
||||
/// 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 {
|
||||
|
||||
Quaternion compose(const Quaternion&g, const Quaternion& h) {
|
||||
template<typename S, int O>
|
||||
Eigen::Quaternion<S, O> compose(const Eigen::Quaternion<S, O> &g,
|
||||
const Eigen::Quaternion<S, O> & h) {
|
||||
return g * h;
|
||||
}
|
||||
|
||||
Quaternion between(const Quaternion&g, const Quaternion& h) {
|
||||
template<typename S, int O>
|
||||
Eigen::Quaternion<S, O> between(const Eigen::Quaternion<S, O> &g,
|
||||
const Eigen::Quaternion<S, O> & h) {
|
||||
return g.inverse() * h;
|
||||
}
|
||||
|
||||
Quaternion inverse(const Quaternion&g) {
|
||||
template<typename S, int O>
|
||||
Eigen::Quaternion<S, O> inverse(const Eigen::Quaternion<S, O> &g) {
|
||||
return g.inverse();
|
||||
}
|
||||
|
||||
namespace traits {
|
||||
|
||||
/// Define the trait that specifies Quaternion's identity element
|
||||
template<>
|
||||
struct identity<Quaternion> {
|
||||
static const Quaternion value;
|
||||
typedef Quaternion value_type;
|
||||
/// Declare the trait that specifies a quaternion's identity element
|
||||
template<typename S, int O>
|
||||
struct identity<Eigen::Quaternion<S, O> > {
|
||||
static const Eigen::Quaternion<S, O> value;
|
||||
typedef Eigen::Quaternion<S, O> value_type;
|
||||
};
|
||||
|
||||
const Quaternion identity<Quaternion>::value = Quaternion(0);
|
||||
/// Out of line definition of identity
|
||||
template<typename S, int O>
|
||||
const Eigen::Quaternion<S, O> identity<Eigen::Quaternion<S, O> >::value =
|
||||
Eigen::Quaternion<S, O>::Identity();
|
||||
|
||||
/// Define the trait that asserts Quaternion is an additive group
|
||||
template<>
|
||||
struct flavor<Quaternion> {
|
||||
/// Define the trait that asserts quaternions are a multiplicative group
|
||||
template<typename S, int O>
|
||||
struct flavor<Eigen::Quaternion<S, O> > {
|
||||
typedef multiplicative_tag type;
|
||||
};
|
||||
|
||||
|
|
@ -67,6 +154,13 @@ struct flavor<Quaternion> {
|
|||
} // \namespace gtsam::group
|
||||
} // \namespace gtsam
|
||||
|
||||
/**
|
||||
* GSAM typedef to an Eigen::Quaternion<double>, we disable alignment because
|
||||
* geometry objects are stored in boost pool allocators, in Values containers,
|
||||
* and and these pool allocators do not support alignment.
|
||||
*/
|
||||
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
|
||||
|
||||
/**
|
||||
* @file testCyclic.cpp
|
||||
* @brief Unit tests for cyclic group
|
||||
|
|
@ -75,6 +169,7 @@ struct flavor<Quaternion> {
|
|||
|
||||
//#include <gtsam/geometry/Quaternion.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -83,25 +178,56 @@ using namespace gtsam;
|
|||
typedef Quaternion Q; // Typedef
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((Group<Q>));
|
||||
TEST(Quaternion , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion, Constructor) {
|
||||
Q g(0);
|
||||
TEST(Quaternion , Constructor) {
|
||||
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion, Compose) {
|
||||
TEST(Quaternion , Invariants) {
|
||||
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
|
||||
// group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!)
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion, Between) {
|
||||
TEST(Quaternion , Local) {
|
||||
Vector3 z_axis(0, 0, 1);
|
||||
Q q1(Eigen::AngleAxisd(0, z_axis));
|
||||
Q q2(Eigen::AngleAxisd(0.1, z_axis));
|
||||
typedef manifold::traits::DefaultChart<Q>::type Chart;
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = Chart::Local(q1, q2);
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion, Ivnverse) {
|
||||
TEST(Quaternion , Retract) {
|
||||
Vector3 z_axis(0, 0, 1);
|
||||
Q q(Eigen::AngleAxisd(0, z_axis));
|
||||
Q expected(Eigen::AngleAxisd(0.1, z_axis));
|
||||
typedef manifold::traits::DefaultChart<Q>::type Chart;
|
||||
Vector3 v(0, 0, 0.1);
|
||||
Q actual = Chart::Retract(q, v);
|
||||
EXPECT(actual.isApprox(expected));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Compose) {
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Between) {
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Inverse) {
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue