diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 7935ab56b..4fe3f519b 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -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 -class IsChart { -public: - typedef typename C::ManifoldType ManifoldType; - typedef typename manifold::traits::TangentVector::type V; - static const int dim = manifold::traits::dimension::value; - typedef OptionalJacobian 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::Local(p,q); q = traits::Retract(p,v); // and the versions with Jacobians. @@ -187,16 +166,6 @@ private: TangentVector v; }; -/// Check invariants -template -BOOST_CONCEPT_REQUIRES(((Testable,IsGroup)),(bool)) // -check_group_invariants(const G& a, const G& b, double tol = 1e-9) { - G e = traits::identity; - return traits::Equals(traits::Compose(a, traits::inverse(a)), e, tol) - && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol) - && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); -} - /** * Group Concept */ @@ -235,16 +204,30 @@ private: G e, g, h; }; - /// Check invariants -//template -//BOOST_CONCEPT_REQUIRES(((Testable)),(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 +BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // +check_group_invariants(const G& a, const G& b, double tol = 1e-9) { + G e = traits::identity; + return traits::Equals(traits::Compose(a, traits::inverse(a)), e, tol) + && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol) + && traits::Equals(traits::Compose(a, traits::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 diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 06f20d45a..b043600b6 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -16,50 +16,72 @@ **/ #include +#include +#include #define QUATERNION_TEMPLATE typename _Scalar, int _Options #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> -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 +struct traits { + 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 -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 -struct structure_category > { - 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 - static QUATERNION_TYPE expmap(const Eigen::Ref& omega) { + // TODO: implement Jacobian + static Q Expmap(const Eigen::Ref& 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 - static QUATERNION_TANGENT logmap(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, 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 Quaternion; -typedef LieGroupChart QuaternionChart; } // \namespace gtsam diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index a86b3100a..c83e7a16f 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -22,12 +22,12 @@ using namespace std; using namespace gtsam; typedef Quaternion Q; // Typedef -typedef OptionalJacobian::value, manifold::traits::dimension::value> QuaternionJacobian; +typedef traits::ChartJacobian QuaternionJacobian; //****************************************************************************** TEST(Quaternion , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); + //BOOST_CONCEPT_ASSERT((IsGroup)); + //BOOST_CONCEPT_ASSERT((IsManifold)); BOOST_CONCEPT_ASSERT((IsLieGroup)); } @@ -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::type Chart; QuaternionJacobian H1,H2; Vector3 expected(0, 0, 0.1); - Vector3 actual = Chart::Local(q1, q2, H1, H2); + Vector3 actual = traits::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::type Chart; Vector3 v(0, 0, 0.1); QuaternionJacobian Hq,Hv; - Q actual = Chart::Retract(q, v, Hq, Hv); + Q actual = traits::Retract(q, v, Hq, Hv); EXPECT(actual.isApprox(expected)); }