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

cyclic trait refactored

release/4.3a0
Mike Bosse 2014-12-12 14:51:16 +01:00
parent edb1bbaa7b
commit d94c8c72b8
3 changed files with 22 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/base/concepts.h
View File

@ -16,6 +16,9 @@
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_base_of.hpp>
//FIXME temporary until all conflicts with namespace traits resolved
#define traits traits_foo
namespace gtsam {
template <typename T> struct traits {};
@ -35,7 +38,9 @@ struct vector_space_tag: public lie_group_tag {};
struct multiplicative_group_tag {};
struct additive_group_tag {};
template<typename Transformation>
// a fictitious example
class Transformation;
template<>
struct traits<Transformation> {
// Typedefs required by all manifold types.
@ -113,8 +118,8 @@ struct traits<Transformation> {
template<typename T>
BOOST_CONCEPT_REQUIRES(((Testable<traits<T> >)),(bool)) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
traits<T>::TangentVector v0 = traits<T>::Local(a,a);
traits<T>::TangentVector v = traits<T>::Local(a,b);
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);
}
@ -174,7 +179,6 @@ class IsGroup {
public:
typedef typename traits<G>::structure_category structure_category_tag;
typedef typename traits<G>::group_flavor flavor_tag;
static const size_t dim = traits<G>::dimension;
//typedef typename traits<G>::identity::value_type identity_value_type;
BOOST_CONCEPT_USAGE(IsGroup) {
@ -216,7 +220,7 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
#define GTSAM_ADDITIVE_GROUP(GROUP) \
typedef additive_gropu_tag group_flavor; \
typedef additive_group_tag group_flavor; \
static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \
static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \
static GROUP Inverse(const GROUP &g) { return -g;}
@ -259,7 +263,7 @@ public:
private:
LG g, h;
TangentVector v;
OptionalJacobian Hg, Hh;
ChartJacobian Hg, Hh;
};

14
gtsam/geometry/Cyclic.h
View File

@ -64,16 +64,14 @@ public:
#define CYCLIC_TEMPLATE size_t N
#define CYCLIC_TYPE Cyclic<N>
GTSAM_GROUP_IDENTITY(CYCLIC_TEMPLATE, CYCLIC_TYPE)
GTSAM_ADDITIVE_GROUP(CYCLIC_TEMPLATE, CYCLIC_TYPE)
/// Define cyclic group to be a model of the Group concept
namespace traits {
template<size_t N>
struct structure_category<Cyclic<N> > {
typedef group_tag type;
/// Define cyclic group traits to be a model of the Group concept
template <CYCLIC_TEMPLATE>
struct traits<CYCLIC_TYPE > {
typedef group_tag structure_category;
GTSAM_ADDITIVE_GROUP(CYCLIC_TYPE);
static const CYCLIC_TYPE identity = CYCLIC_TYPE::Identity();
};
}
} // \namespace gtsam

13
gtsam/geometry/Quaternion.h
View File

@ -17,7 +17,6 @@
#include <gtsam/base/concepts.h>
#include <gtsam/base/Matrix.h>
#include <Geometry/Quaternion.h>
#define QUATERNION_TEMPLATE typename _Scalar, int _Options
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -37,7 +36,7 @@ struct traits<QUATERNION_TYPE> {
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
typedef OptionalJacobian<3, 3> ChartJacobian;
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=NULL, ChartJacobian Hh=NULL) {
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=boost::none, ChartJacobian Hh=boost::none) {
if (Hg) {
//TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
*Hg = h.toRotationMatrix().transpose();
@ -48,7 +47,7 @@ struct traits<QUATERNION_TYPE> {
}
return g * h;
}
static Q Between(const Q &g, const Q & h, ChartJacobian Hg=NULL, ChartJacobian Hh=NULL) {
static Q Between(const Q &g, const Q & h, ChartJacobian Hg=boost::none, ChartJacobian Hh=boost::none) {
Q d = g.inverse() * h;
if (Hg) {
//TODO : check Jacobian consistent with chart
@ -60,7 +59,7 @@ struct traits<QUATERNION_TYPE> {
}
return d;
}
static Q Inverse(const Q &g, ChartJacobian H=NULL) {
static Q Inverse(const Q &g, ChartJacobian H=boost::none) {
if (H) {
//TODO : check Jacobian consistent with chart
*H = -g.toRotationMatrix();
@ -70,7 +69,7 @@ struct traits<QUATERNION_TYPE> {
/// Exponential map, simply be converting omega to axis/angle representation
// TODO: implement Jacobian
static Q Expmap(const Eigen::Ref<const TangentVector >& omega, ChartJacobian H=NULL) {
static Q Expmap(const Eigen::Ref<const TangentVector >& omega, ChartJacobian H=boost::none) {
if (omega.isZero())
return Q::Identity();
else {
@ -81,7 +80,7 @@ struct traits<QUATERNION_TYPE> {
/// We use our own Logmap, as there is a slight bug in Eigen
// TODO: implement Jacobian
static TangentVector Logmap(const Q& q, ChartJacobian H=NULL) {
static TangentVector Logmap(const Q& q, ChartJacobian H=boost::none) {
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
@ -109,7 +108,7 @@ struct traits<QUATERNION_TYPE> {
}
}
static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=NULL, ChartJacobian Hother=NULL) {
static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none) {
return Logmap(Between(origin,other,Horigin,Hother));
// TODO: incorporate Jacobian of Logmap
}