From c0e4f76d94b70127d0c6c78a18ac88acc0b4679a Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sun, 14 Dec 2014 18:59:57 +0100 Subject: [PATCH] More manifolds...more numerical differentiation...more. --- gtsam/base/LieScalar.h | 17 +--- gtsam/base/Manifold.h | 43 ++++----- gtsam/base/concepts.h | 148 +++++++++++++++++++++++++++++- gtsam/base/numericalDerivative.h | 2 +- gtsam/nonlinear/AdaptAutoDiff.h | 8 +- gtsam_unstable/dynamics/PoseRTV.h | 19 +--- 6 files changed, 178 insertions(+), 59 deletions(-) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 33e5914a1..f83982fa9 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -32,6 +32,8 @@ namespace gtsam { */ struct GTSAM_EXPORT LieScalar { + enum { dimension = 1 }; + /** default constructor */ LieScalar() : d_(0.0) {} @@ -116,21 +118,8 @@ namespace gtsam { double d_; }; - // Define GTSAM traits - namespace traits { template<> - struct is_group : public boost::true_type { - }; - - template<> - struct is_manifold : public boost::true_type { - }; - - template<> - struct dimension : public boost::integral_constant { - }; - - } + struct traits_x : public internal::LieGroup {}; } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 038111260..85fa9b26d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -24,26 +24,27 @@ #include #include // -//namespace gtsam { -// -///** -// * A manifold defines a space in which there is a notion of a linear tangent space -// * that can be centered around a given point on the manifold. These nonlinear -// * spaces may have such properties as wrapping around (as is the case with rotations), -// * which might make linear operations on parameters not return a viable element of -// * the manifold. -// * -// * We perform optimization by computing a linear delta in the tangent space of the -// * current estimate, and then apply this change using a retraction operation, which -// * maps the change in tangent space back to the manifold itself. -// * -// * There may be multiple possible retractions for a given manifold, which can be chosen -// * between depending on the computational complexity. The important criteria for -// * the creation for the retract and localCoordinates functions is that they be -// * inverse operations. The new notion of a Chart guarantees that. -// * -// */ -// +namespace gtsam { + +/** + * A manifold defines a space in which there is a notion of a linear tangent space + * that can be centered around a given point on the manifold. These nonlinear + * spaces may have such properties as wrapping around (as is the case with rotations), + * which might make linear operations on parameters not return a viable element of + * the manifold. + * + * We perform optimization by computing a linear delta in the tangent space of the + * current estimate, and then apply this change using a retraction operation, which + * maps the change in tangent space back to the manifold itself. + * + * There may be multiple possible retractions for a given manifold, which can be chosen + * between depending on the computational complexity. The important criteria for + * the creation for the retract and localCoordinates functions is that they be + * inverse operations. The new notion of a Chart guarantees that. + * + */ + + //// Traits, same style as Boost.TypeTraits //// All meta-functions below ever only declare a single type //// or a type/value/value_type @@ -355,7 +356,7 @@ // } //}; // -//} // \ namespace gtsam +} // \ namespace gtsam // // TODO(ASL) Remove these and fix the compiler errors. diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index f804528a2..379cc1304 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -120,7 +120,7 @@ struct Manifold { template struct LieGroup { // Typedefs required by all manifold types. - typedef manifold_tag structure_category; + typedef lie_group_tag structure_category; enum { dimension = ManifoldType::dimension }; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; @@ -217,13 +217,155 @@ struct LieGroup { }; +/// A helper that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public ScalarTraits { }; +template +struct ScalarTraits { + // Typedefs required by all manifold types. + typedef vector_space_tag structure_category; + typedef additive_group_tag group_flavor; + enum { dimension = 1 }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + // For Testable + static void Print(Scalar m, const std::string& str = "") { + m.print(); + } + static bool Equals(Scalar m1, + Scalar m2, + double tol = 1e-8) { + return fabs(m1 - m2) < tol; + } + + static TangentVector Local(Scalar origin, + Scalar other) { + return TangentVector(other - origin); + } + + static Scalar Retract(Scalar origin, + const TangentVector& v) { + return origin + v[0]; + } + + static TangentVector Local(Scalar origin, + Scalar other, + ChartJacobian Horigin, + ChartJacobian Hother) { + if(Horigin) { + (*Horigin)[0] = -1.0; + } + if(Hother) { + (*Hother)[0] = 1.0; + } + return Local(origin, other); + } + + static Scalar Retract(Scalar origin, + const TangentVector& v, + ChartJacobian Horigin, + ChartJacobian Hv) { + if(Horigin) { + (*Horigin)[0] = 1.0; + } + if(Hv) { + (*Hv)[0] = 1.0; + } + return Retract(origin, v); + } + + static int GetDimension(Scalar m){ return 1; } + + // For Group. Only implemented for groups + 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) { + if(H1) { + (*H1)[0] = 1.0; + } + if(H2) { + (*H2)[0] = 1.0; + } + return Compose(m1, m2); + } + + static Scalar Between(Scalar m1, + Scalar m2, + ChartJacobian H1, + ChartJacobian H2) { + 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) { + 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); + } + +}; + } // namespace internal +template<> +struct traits_x : public internal::ScalarTraits {}; +template<> +struct traits_x : public internal::ScalarTraits {}; + + template struct Canonical { BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::structure_category_tag>::value), - "This type's trait does not assert it is a group (or derived)"); + (boost::is_base_of::structure_category>::value), + "This type's trait does not assert it is a manifold (or derived)"); typedef traits_x Traits; typedef typename Traits::TangentVector TangentVector; enum { dimension = Traits::dimension }; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index fb929eed1..c1ee87a26 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -139,7 +139,7 @@ Matrix numericalDerivative11(boost::function h, const X& x, dx(j) = delta; TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - TangentY dy2 = TraitsY::local(hx, h(TraitsX::Retract(x, dx))); + TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 9ba59d977..9767eba60 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -28,7 +28,7 @@ namespace gtsam { template class AdaptAutoDiff { - static const int N = traits:_x:dimension; + static const int N = traits_x::dimension; static const int M1 = traits_x::dimension; static const int M2 = traits_x::dimension; @@ -39,9 +39,9 @@ class AdaptAutoDiff { typedef Canonical Canonical1; typedef Canonical Canonical2; - typedef typename CanonicalT::vector VectorT; - typedef typename Canonical1::vector Vector1; - typedef typename Canonical2::vector Vector2; + typedef typename CanonicalT::TangentVector VectorT; + typedef typename Canonical1::TangentVector Vector1; + typedef typename Canonical2::TangentVector Vector2; // Instantiate function and charts CanonicalT chartT; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 43d018752..4c2c1d39b 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -26,6 +26,8 @@ protected: Velocity3 v_; public: + enum { dimension = 9 }; + // constructors - with partial versions PoseRTV() {} PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) @@ -183,23 +185,8 @@ private: } }; -// Define GTSAM traits -namespace traits { template<> -struct is_manifold : public boost::true_type { -}; +struct traits_x : public internal::LieGroup {}; -template<> -struct dimension : public boost::integral_constant { -}; - -template<> -struct zero { - static PoseRTV value() { - return PoseRTV(); - } -}; - -} } // \namespace gtsam