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); \ ChartJacobian Hv=boost::none); \
static int GetDimension(const ManifoldType& m) { return dimension; } 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 * Manifold concept
*/ */
@ -174,7 +153,7 @@ public:
"This type's structure_category trait does not assert it as a manifold (or derived)"); "This type's structure_category trait does not assert it as a manifold (or derived)");
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); 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); v = traits<M>::Local(p,q);
q = traits<M>::Retract(p,v); q = traits<M>::Retract(p,v);
// and the versions with Jacobians. // and the versions with Jacobians.
@ -187,16 +166,6 @@ private:
TangentVector v; 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 * Group Concept
*/ */
@ -235,16 +204,30 @@ private:
G e, g, h; G e, g, h;
}; };
/// Check invariants /// Check invariants
//template<typename LG> template<typename G>
//BOOST_CONCEPT_REQUIRES(((Testable<LG>)),(bool)) check_invariants(const LG& a, BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
// const LG& b) { check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
// bool check_invariants(const LG& a, const LG& b) { G e = traits<G>::identity;
// return equal(Chart::Retract(a, b), a + b) return traits<G>::Equals(traits<G>::Compose(a, traits<G>::inverse(a)), e, tol)
// && equal(Chart::Local(a, b), b - a); && 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 * Lie Group Concept

89
gtsam/geometry/Quaternion.h
View File

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

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

@ -22,12 +22,12 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef Quaternion Q; // Typedef 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) { TEST(Quaternion , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >)); //BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >)); //BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >)); BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
} }
@ -40,7 +40,7 @@ TEST(Quaternion , Constructor) {
TEST(Quaternion , Invariants) { TEST(Quaternion , Invariants) {
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); 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); Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0, z_axis)); Q q1(Eigen::AngleAxisd(0, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis));
typedef manifold::traits::DefaultChart<Q>::type Chart;
QuaternionJacobian H1,H2; QuaternionJacobian H1,H2;
Vector3 expected(0, 0, 0.1); 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)); EXPECT(assert_equal((Vector)expected,actual));
} }
@ -60,10 +59,9 @@ TEST(Quaternion , Retract) {
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1);
Q q(Eigen::AngleAxisd(0, z_axis)); Q q(Eigen::AngleAxisd(0, z_axis));
Q expected(Eigen::AngleAxisd(0.1, z_axis)); Q expected(Eigen::AngleAxisd(0.1, z_axis));
typedef manifold::traits::DefaultChart<Q>::type Chart;
Vector3 v(0, 0, 0.1); Vector3 v(0, 0, 0.1);
QuaternionJacobian Hq,Hv; QuaternionJacobian Hq,Hv;
Q actual = Chart::Retract(q, v, Hq, Hv); Q actual = traits<Q>::Retract(q, v, Hq, Hv);
EXPECT(actual.isApprox(expected)); EXPECT(actual.isApprox(expected));
} }