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

quaternion trait refactored

release/4.3a0
Mike Bosse 2014-12-12 14:11:19 +01:00
parent cc9ab9c6ed
commit edb1bbaa7b
3 changed files with 87 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam/base/concepts.h
View File

@ -135,27 +135,6 @@ check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
ChartJacobian Hv=boost::none); \
static int GetDimension(const ManifoldType& m) { return dimension; }
/**
* Chart concept
*/
template<typename C>
class IsChart {
public:
typedef typename C::ManifoldType ManifoldType;
typedef typename manifold::traits::TangentVector<ManifoldType>::type V;
static const int dim = manifold::traits::dimension<ManifoldType>::value;
typedef OptionalJacobian<dim, dim> OptionalJacobian;
BOOST_CONCEPT_USAGE(IsChart) {
}
private:
ManifoldType p,q;
OptionalJacobian Hp,Hq,Hv;
V v;
};
/**
* Manifold concept
*/
@ -174,7 +153,7 @@ public:
"This type's structure_category trait does not assert it as a manifold (or derived)");
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
// make sure methods in Chart are defined
// make sure Chart methods are defined
v = traits<M>::Local(p,q);
q = traits<M>::Retract(p,v);
// and the versions with Jacobians.
@ -187,16 +166,6 @@ private:
TangentVector v;
};
/// Check invariants
template<typename G>
BOOST_CONCEPT_REQUIRES(((Testable<G>,IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits<G>::identity;
return traits<G>::Equals(traits<G>::Compose(a, traits<G>::inverse(a)), e, tol)
&& traits<G>::Equals(traits<G>::Between(a, b), traits<G>::Compose(traits<G>::Inverse(a), b), tol)
&& traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
}
/**
* Group Concept
*/
@ -235,16 +204,30 @@ private:
G e, g, h;
};
/// Check invariants
//template<typename LG>
//BOOST_CONCEPT_REQUIRES(((Testable<LG>)),(bool)) check_invariants(const LG& a,
// const LG& b) {
// bool check_invariants(const LG& a, const LG& b) {
// return equal(Chart::Retract(a, b), a + b)
// && equal(Chart::Local(a, b), b - a);
// }
//}
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits<G>::identity;
return traits<G>::Equals(traits<G>::Compose(a, traits<G>::inverse(a)), e, tol)
&& traits<G>::Equals(traits<G>::Between(a, b), traits<G>::Compose(traits<G>::Inverse(a), b), tol)
&& traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
}
#define GTSAM_ADDITIVE_GROUP(GROUP) \
typedef additive_gropu_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;}
#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \
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 g.inverse() * h;} \
static GROUP Inverse(const GROUP &g) { return g.inverse();}
/**
* Lie Group Concept

89
gtsam/geometry/Quaternion.h
View File

@ -16,50 +16,72 @@
**/
#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>
template<QUATERNION_TEMPLATE>
QUATERNION_TYPE operator-(const QUATERNION_TYPE &q) { return gtsam::group::inverse(q); }
namespace gtsam {
// Define group traits
GTSAM_GROUP_IDENTITY(QUATERNION_TEMPLATE, QUATERNION_TYPE)
GTSAM_MULTIPLICATIVE_GROUP(QUATERNION_TEMPLATE, QUATERNION_TYPE)
template<QUATERNION_TEMPLATE>
struct traits<QUATERNION_TYPE> {
typedef QUATERNION_TYPE ManifoldType;
typedef QUATERNION_TYPE Q;
typedef lie_group_tag structure_category;
static const Q identity = Q::Identity();
// Define manifold traits
#define QUATERNION_TANGENT Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1>
#define QUATERNION_CHART LieGroupChart<typename QUATERNION_TYPE>
GTSAM_MANIFOLD(QUATERNION_TEMPLATE, QUATERNION_TYPE, 3, QUATERNION_TANGENT,
QUATERNION_CHART)
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
typedef OptionalJacobian<3, 3> ChartJacobian;
/// Define Eigen::Quaternion to be a model of the Lie Group concept
namespace traits {
template<typename _Scalar, int _Options>
struct structure_category<Eigen::Quaternion<_Scalar,_Options> > {
typedef lie_group_tag type;
};
}
/// lie_group for Eigen Quaternions
namespace lie_group {
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=NULL, ChartJacobian Hh=NULL) {
if (Hg) {
//TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
*Hg = h.toRotationMatrix().transpose();
}
if (Hh) {
//TODO : check Jacobian consistent with chart ( I(3)? )
*Hh = I_3x3;
}
return g * h;
}
static Q Between(const Q &g, const Q & h, ChartJacobian Hg=NULL, ChartJacobian Hh=NULL) {
Q d = g.inverse() * h;
if (Hg) {
//TODO : check Jacobian consistent with chart
*Hg = -d.toRotationMatrix().transpose();
}
if (Hh) {
//TODO : check Jacobian consistent with chart (my guess I(3) )
*Hh = I_3x3;
}
return d;
}
static Q Inverse(const Q &g, ChartJacobian H=NULL) {
if (H) {
//TODO : check Jacobian consistent with chart
*H = -g.toRotationMatrix();
}
return g.inverse();
}
/// Exponential map, simply be converting omega to axis/angle representation
template <typename _Scalar, int _Options>
static QUATERNION_TYPE expmap<QUATERNION_TYPE>(const Eigen::Ref<const typename QUATERNION_TANGENT >& omega) {
// TODO: implement Jacobian
static Q Expmap(const Eigen::Ref<const TangentVector >& omega, ChartJacobian H=NULL) {
if (omega.isZero())
return QUATERNION_TYPE::Identity();
return Q::Identity();
else {
_Scalar angle = omega.norm();
return QUATERNION_TYPE(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
}
}
/// We use our own Logmap, as there is a slight bug in Eigen
template <QUATERNION_TEMPLATE>
static QUATERNION_TANGENT logmap<QUATERNION_TYPE>(const QUATERNION_TYPE& q) {
// TODO: implement Jacobian
static TangentVector Logmap(const Q& q, ChartJacobian H=NULL) {
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
@ -86,15 +108,18 @@ namespace lie_group {
return (angle / s) * q.vec();
}
}
} // end namespace lie_group
/**
* 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.
*/
static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=NULL, ChartJacobian Hother=NULL) {
return Logmap(Between(origin,other,Horigin,Hother));
// TODO: incorporate Jacobian of Logmap
}
static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin, ChartJacobian Hv) {
return Compose(origin, Expmap(v), Horigin, Hv);
// TODO : incorporate Jacobian of Expmap
}
};
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
typedef LieGroupChart<Quaternion> QuaternionChart;
} // \namespace gtsam

14
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -22,12 +22,12 @@ using namespace std;
using namespace gtsam;
typedef Quaternion Q; // Typedef
typedef OptionalJacobian<manifold::traits::dimension<Q>::value, manifold::traits::dimension<Q>::value> QuaternionJacobian;
typedef traits<Q>::ChartJacobian QuaternionJacobian;
//******************************************************************************
TEST(Quaternion , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
//BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
//BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
}
@ -40,7 +40,7 @@ TEST(Quaternion , Constructor) {
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!)
check_group_invariants(q1,q2);
}
//******************************************************************************
@ -48,10 +48,9 @@ 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;
QuaternionJacobian H1,H2;
Vector3 expected(0, 0, 0.1);
Vector3 actual = Chart::Local(q1, q2, H1, H2);
Vector3 actual = traits<Q>::Local(q1, q2, H1, H2);
EXPECT(assert_equal((Vector)expected,actual));
}
@ -60,10 +59,9 @@ 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);
QuaternionJacobian Hq,Hv;
Q actual = Chart::Retract(q, v, Hq, Hv);
Q actual = traits<Q>::Retract(q, v, Hq, Hv);
EXPECT(actual.isApprox(expected));
}