From 88a83fe9e7452fc23bfd0e5d19eeda8fe84f12b0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Dec 2014 16:46:20 +0100 Subject: [PATCH] re-formatted ScalarTraits while checking correctness (seems correct) --- gtsam/base/concepts.h | 126 ++++++++-------------------- gtsam/geometry/tests/testPoint2.cpp | 28 +++++++ 2 files changed, 65 insertions(+), 89 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 0b3fa7149..2bb87c9fa 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -231,129 +231,77 @@ struct ScalarTraits { typedef additive_group_tag group_flavor; typedef Scalar ManifoldType; enum { dimension = 1 }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian<1, 1> ChartJacobian; // For Testable static void Print(Scalar m, const std::string& str = "") { - gtsam::print(m,str); + gtsam::print(m, str); } - static bool Equals(Scalar m1, - Scalar m2, - double tol = 1e-8) { + static bool Equals(Scalar m1, Scalar m2, double tol = 1e-8) { return fabs(m1 - m2) < tol; } - static TangentVector Local(Scalar origin, - Scalar other) { + static TangentVector Local(Scalar origin, Scalar other) { return TangentVector(other - origin); } - static Scalar Retract(Scalar origin, - const TangentVector& v) { + static Scalar Retract(Scalar origin, const TangentVector& v) { return origin + v[0]; } - static TangentVector Local(Scalar origin, - Scalar other, - ChartJacobian Horigin, + static TangentVector Local(Scalar origin, Scalar other, ChartJacobian Horigin, ChartJacobian Hother = boost::none) { - if(Horigin) { - (*Horigin)[0] = -1.0; - } - if(Hother) { - (*Hother)[0] = 1.0; - } - return Local(origin, other); + if (Horigin) (*Horigin)[0] = -1.0; + if (Hother) (*Hother)[0] = 1.0; + return TangentVector(other - origin); } - static Scalar Retract(Scalar origin, - const TangentVector& v, - ChartJacobian Horigin, - ChartJacobian Hv = boost::none) { - if(Horigin) { - (*Horigin)[0] = 1.0; - } - if(Hv) { - (*Hv)[0] = 1.0; - } - return Retract(origin, v); + static Scalar Retract(Scalar origin, const TangentVector& v, + ChartJacobian Horigin, ChartJacobian Hv = boost::none) { + if (Horigin) (*Horigin)[0] = 1.0; + if (Hv) (*Hv)[0] = 1.0; + return origin + v[0]; } - static int GetDimension(Scalar m){ return 1; } + static int GetDimension(Scalar m) { return 1; } // For Group. Only implemented for groups - static Scalar Compose(Scalar m1, - Scalar m2) { + static Scalar Compose(Scalar m1, Scalar m2) { return m1 + m2;} + static Scalar Between(Scalar m1, Scalar m2) { return m2 - m1;} + static Scalar Inverse(Scalar m) { return -m;} + + static Scalar Compose(Scalar m1, Scalar m2, ChartJacobian H1, + ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = 1.0; + if (H2) (*H2)[0] = 1.0; return m1 + m2; } - static Scalar Between(Scalar m1, - Scalar m2) { + static Scalar Between(Scalar m1, Scalar m2, ChartJacobian H1, + ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = -1.0; + if (H2) (*H2)[0] = 1.0; return m2 - m1; } - static Scalar Inverse(Scalar m) { + static Scalar Inverse(Scalar m, ChartJacobian H) { + if (H) (*H)[0] = -1; return -m; } - static Scalar Compose(Scalar m1, - Scalar m2, - ChartJacobian H1, - ChartJacobian H2 = boost::none) { - if(H1) { - (*H1)[0] = 1.0; - } - if(H2) { - (*H2)[0] = 1.0; - } - return Compose(m1, m2); - } + static Scalar Identity() { return 0; } + static TangentVector Logmap(Scalar m) {return TangentVector(m);} + static Scalar Expmap(const TangentVector& v) { return v[0];} - static Scalar Between(Scalar m1, - Scalar m2, - ChartJacobian H1, - ChartJacobian H2 = boost::none) { - if(H1) { - (*H1)[0] = -1.0; - } - if(H2) { - (*H2)[0] = 1.0; - } - return Between(m1, m2); - } - - static Scalar Inverse(Scalar m, ChartJacobian H) { - if(H) { - (*H)[0] = -1; - } - return Inverse(m); - } - - static Scalar Identity() { - return 0; - } - - static TangentVector Logmap(Scalar m) { + static TangentVector Logmap(Scalar m, ChartJacobian Hm) { + if (Hm) (*Hm)[0] = 1.0; return TangentVector(m); } - static Scalar Expmap(const TangentVector& v) { - return v[0]; - } - - static TangentVector Logmap(Scalar m, ChartJacobian Hm) { - if(Hm) { - (*Hm)[0] = 1.0; - } - return Scalar::Logmap(m); - } - static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { - if(Hv) { - (*Hv)[0] = 1.0; - } - return Scalar::Expmap(v); + if (Hv) (*Hv)[0] = 1.0; + return v[0]; } }; diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index ef72ce988..d77f5ff2d 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -27,6 +27,34 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) +//****************************************************************************** +TEST(Double , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Double , Invariants) { + double p1(2), p2(5); + check_group_invariants(p1, p2); + check_manifold_invariants(p1, p2); +} + +//****************************************************************************** +TEST(Point2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Point2 , Invariants) { + Point2 p1(1, 2), p2(4, 5); + check_group_invariants(p1, p2); + check_manifold_invariants(p1, p2); +} + /* ************************************************************************* */ TEST(Point2, constructor) { Point2 p1(1, 2), p2 = p1;