diff --git a/.cproject b/.cproject index 42f61fa16..812108f7c 100644 --- a/.cproject +++ b/.cproject @@ -375,6 +375,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -401,7 +409,6 @@ make - tests/testBayesTree.run true false @@ -409,7 +416,6 @@ make - testBinaryBayesNet.run true false @@ -457,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -465,7 +470,6 @@ make - tests/testSymbolicFactor.run true false @@ -473,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -489,20 +492,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -529,6 +523,7 @@ make + testGraph.run true false @@ -600,6 +595,7 @@ make + testInference.run true false @@ -607,6 +603,7 @@ make + testGaussianFactor.run true false @@ -614,6 +611,7 @@ make + testJunctionTree.run true false @@ -621,6 +619,7 @@ make + testSymbolicBayesNet.run true false @@ -628,6 +627,7 @@ make + testSymbolicFactorGraph.run true false @@ -721,15 +721,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -737,14 +729,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -785,7 +769,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -793,6 +785,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1107,7 +1107,6 @@ make - testErrors.run true false @@ -1385,6 +1384,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -1507,6 +1514,7 @@ make + testSimulated2DOriented.run true false @@ -1546,6 +1554,7 @@ make + testSimulated2D.run true false @@ -1553,6 +1562,7 @@ make + testSimulated3D.run true false @@ -1800,6 +1810,7 @@ make + tests/testGaussianISAM2 true false @@ -1821,46 +1832,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -1957,6 +1928,62 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + make -j2 @@ -1997,22 +2024,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ea3c730bf..61e7b6dfe 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -15,6 +15,10 @@ * @author Richard Roberts * @author Alex Cunningham * + * This concept check provides a specialization on the Manifold type, + * in which the Manifolds represented require an algebra and group structure. + * All Lie types must also be a Manifold. + * * The necessary functions to implement for Lie are defined * below with additional details as to the interface. The * concept checking function in class Lie will check whether or not @@ -55,8 +59,7 @@ #pragma once -#include -#include +#include namespace gtsam { @@ -78,18 +81,15 @@ namespace gtsam { inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} /** - * Base class for Lie group type - * This class uses the Curiously Recurring Template design pattern to allow for - * concept checking using a private function. + * Concept check class for Lie group type * - * T is the derived Lie type, like Point2, Pose3, etc. + * T is the Lie type, like Point2, Pose3, etc. * * By convention, we use capital letters to designate a static function */ template - class Lie { + class LieConcept { private: - /** concept checking function - implement the functions this demands */ static void concept_check(const T& t) { @@ -182,25 +182,16 @@ namespace gtsam { return expm(xhat,K); } - /** - * function wrappers for full versions of expmap/logmap - * these will default simple types to using the existing expmap/logmap, - * but more complex ones can be specialized to use improved versions - * - * TODO: replace this approach with a naming scheme that doesn't call - * non-expmap operations "expmap" - use same approach, but with "update" - */ - - /** unary versions */ - template - T ExpmapFull(const Vector& xi) { return T::Expmap(xi); } - template - Vector LogmapFull(const T& p) { return T::Logmap(p); } - - /** binary versions */ - template - T expmapFull(const T& t, const Vector& v) { return t.expmap(v); } - template - Vector logmapFull(const T& t, const T& p2) { return t.logmap(p2); } - } // namespace gtsam + +/** + * Macros for using the ManifoldConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + * + * NOTE: intentionally not in the gtsam namespace to allow for classes not in + * the gtsam namespace to be more easily enforced as testable + */ +/// TODO: find better name for "INST" macro, something like "UNIT" or similar +#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::LieConcept; +#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 7d73255b7..a0f6cd6d6 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -24,7 +24,7 @@ namespace gtsam { /** * LieScalar is a wrapper around double to allow it to be a Lie type */ - struct LieScalar : public Lie { + struct LieScalar { /** default constructor - should be unnecessary */ LieScalar() {} @@ -72,6 +72,28 @@ namespace gtsam { /** Logmap around identity - just returns with default cast back */ static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); } + inline LieScalar between(const LieScalar& t2) const { return LieScalar(t2.value() - d_); } + + /** compose with another object */ + inline LieScalar compose(const LieScalar& t2) const { return LieScalar(t2.value() + d_); } + + /** invert the object and yield a new one */ + inline LieScalar inverse() const { return LieScalar(-d_); } + + // Manifold requirements + + inline LieScalar retract(const Vector& v) const { return expmap(v); } + + /** expmap around identity */ + inline static LieScalar Retract(const Vector& v) { return Expmap(v); } + + /** + * Returns inverse retraction + */ + inline Vector unretract(const LieScalar& t2) const { return logmap(t2); } + + /** Unretract around identity */ + inline static Vector Unretract(const LieScalar& t) { return Logmap(t); } private: double d_; diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index bec42f775..cf4e5b82b 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -25,7 +25,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector, public Lie { +struct LieVector : public Vector { /** default constructor - should be unnecessary */ LieVector() {} @@ -100,5 +100,20 @@ struct LieVector : public Vector, public Lie { inline LieVector inverse() const { return LieVector(-1.0 * vector()); } + + // Manifold requirements + + inline LieVector retract(const Vector& v) const { return expmap(v); } + + /** expmap around identity */ + inline static LieVector Retract(const Vector& v) { return Expmap(v); } + + /** + * Returns inverse retraction + */ + inline Vector unretract(const LieVector& t2) const { return logmap(t2); } + + /** Unretract around identity */ + inline static Vector Unretract(const LieVector& t) { return Logmap(t); } }; } // \namespace gtsam diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index b1a11a0fc..3383f7e57 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -25,7 +25,8 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h sources += timing.cpp debug.cpp check_PROGRAMS += tests/testDebug tests/testTestableAssertions -# Lie Groups +# Manifolds and Lie Groups +headers += Manifold.h headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h sources += LieVector.cpp check_PROGRAMS += tests/testLieVector tests/testLieScalar diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h new file mode 100644 index 000000000..75a922efb --- /dev/null +++ b/gtsam/base/Manifold.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Manifold.h + * @brief Base class and basic functions for Manifold types + * @author Richard Roberts + * @author Alex Cunningham + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. + * + * Returns dimensionality of the tangent space + * inline size_t dim() const; + * + * Returns Retraction update of T + * T retract(const Vector& v) const; + * + * Retract around identity + * static T Retract(const Vector& v); + * + * Returns inverse retraction operation + * A default implementation of unretract(*this, lp) is available: + * Vector unretract(const T& lp) const; + * + * Unretract around identity + * static Vector Unretract(const T& p); + * + */ + + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * T is the Manifold type, like Point2, Pose3, etc. + * + * By convention, we use capital letters to designate a static function + */ + template + class ManifoldConcept { + private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { + + /** assignment */ + T t2 = t; + + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); + + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); + + /** expmap around identity */ + T retract_identity_ret = T::Retract(gtsam::zero(dim_ret)); + + /** + * Returns inverse retraction + */ + Vector unretract_ret = t.unretract(t2); + + /** Unretract around identity */ + Vector unretract_identity_ret = T::Unretract(t); + } + }; + +} // namespace gtsam + +/** + * Macros for using the ManifoldConcept + * - An instantiation for use inside unit tests + * - A typedef for use inside generic algorithms + * + * NOTE: intentionally not in the gtsam namespace to allow for classes not in + * the gtsam namespace to be more easily enforced as testable + */ +/// TODO: find better name for "INST" macro, something like "UNIT" or similar +#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 795a39688..698f3517a 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -17,11 +17,14 @@ #include #include +#include #include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieScalar) +GTSAM_CONCEPT_MANIFOLD_INST(LieScalar) +GTSAM_CONCEPT_LIE_INST(LieScalar) const double tol=1e-9; diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index e8dfae741..1a2b84774 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -17,11 +17,14 @@ #include #include +#include #include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieVector) +GTSAM_CONCEPT_MANIFOLD_INST(LieVector) +GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) {