diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 5b2574ee3..de33f6225 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -19,43 +19,93 @@ #pragma once -#include +#include +#include +#include +#include namespace gtsam { +/// tag to assert a type is a group +struct group_tag {}; + +/// Group operator syntax flavors +struct multiplicative_group_tag {}; +struct additive_group_tag {}; + +template struct traits_x; + /** - * This concept check enforces a Group structure on a variable type, - * in which we require the existence of basic algebraic operations. + * Group Concept */ -template -class GroupConcept { -private: - static T concept_check(const T& t) { - /** assignment */ - T t2 = t; +template +class IsGroup { +public: + typedef typename traits_x::structure_category structure_category_tag; + typedef typename traits_x::group_flavor flavor_tag; + //typedef typename traits_x::identity::value_type identity_value_type; - /** identity */ - T identity = T::identity(); - - /** compose with another object */ - T compose_ret = identity.compose(t2); - - /** invert the object and yield a new one */ - T inverse_ret = compose_ret.inverse(); - - return inverse_ret; + BOOST_CONCEPT_USAGE(IsGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a group (or derived)"); + e = traits_x::Identity(); + e = traits_x::Compose(g, h); + e = traits_x::Between(g, h); + e = traits_x::Inverse(g); + operator_usage(flavor); + // todo: how do we test the act concept? or do we even need to? } + +private: + void operator_usage(multiplicative_group_tag) { + e = g * h; + //e = -g; // todo this should work, but it is failing for Quaternions + } + void operator_usage(additive_group_tag) { + e = g + h; + e = h - g; + e = -g; + } + + flavor_tag flavor; + G e, g, h; + bool b; }; -} // \namespace gtsam +/// Check invariants +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // +check_group_invariants(const G& a, const G& b, double tol = 1e-9) { + G e = traits_x::Identity(); + return traits_x::Equals(traits_x::Compose(a, traits_x::Inverse(a)), e, tol) + && traits_x::Equals(traits_x::Between(a, b), traits_x::Compose(traits_x::Inverse(a), b), tol) + && traits_x::Equals(traits_x::Compose(a, traits_x::Between(a, b)), b, tol); +} + +/// Macro to add group traits, additive flavor +#define GTSAM_ADDITIVE_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 h - g;} \ + static GROUP Inverse(const GROUP &g) { return -g;} + +/// Macro to add group traits, multiplicative flavor +#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();} + +} // \ namespace gtsam /** - * Macros for using the GroupConcept + * Macros for using the IsGroup * - 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 */ -#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept; -#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; +#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup; +#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup _gtsam_IsGroup_##T; diff --git a/gtsam/base/Lie-inl.h b/gtsam/base/Lie-inl.h deleted file mode 100644 index f1bb947a2..000000000 --- a/gtsam/base/Lie-inl.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Lie-inl.h - * @date Jan 9, 2010 - * @author Richard Roberts - * @brief Instantiate macro for Lie type - */ - -#pragma once - -#include - -#define INSTANTIATE_LIE(T) \ - template T between_default(const T&, const T&); \ - template Vector logmap_default(const T&, const T&); \ - template T expmap_default(const T&, const Vector&); - - diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 7953bf511..5f550e69d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -24,6 +24,105 @@ namespace gtsam { +/// tag to assert a type is a Lie group +struct lie_group_tag: public manifold_tag, public group_tag {}; + +namespace internal { + +/// A helper that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public LieGroup { }; +template +struct LieGroup : Testable { + typedef lie_group_tag structure_category; + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static T Identity() { return T::identity();} + static T Compose(const T& m1, const T& m2) { return m1 * m2;} + static T Between(const T& m1, const T& m2) { return m1.inverse() * m2;} + static T Inverse(const T& m) { return m.inverse();} + /// @} + + /// @name Manifold + /// @{ + typedef T ManifoldType; + enum { dimension = ManifoldType::dimension }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + static int GetDimension(const ManifoldType& m){ return m.dim(); } + + static TangentVector Local(const ManifoldType& origin, + const ManifoldType& other) { + return origin.localCoordinates(other); + } + + static ManifoldType Retract(const ManifoldType& origin, + const TangentVector& v) { + return origin.retract(v); + } + + static TangentVector Local(const ManifoldType& origin, + const ManifoldType& other, + ChartJacobian Horigin, + ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other, Horigin, Hother); + } + + static ManifoldType Retract(const ManifoldType& origin, + const TangentVector& v, + ChartJacobian Horigin, + ChartJacobian Hv = boost::none) { + return origin.retract(v, Horigin, Hv); + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const ManifoldType& m) { + return ManifoldType::Logmap(m); + } + + static ManifoldType Expmap(const TangentVector& v) { + return ManifoldType::Expmap(v); + } + + static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) { + return ManifoldType::Logmap(m, Hm); + } + + static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) { + return ManifoldType::Expmap(v, Hv); + } + + static ManifoldType Compose(const ManifoldType& m1, + const ManifoldType& m2, + ChartJacobian H1, + ChartJacobian H2 = boost::none) { + return m1.compose(m2, H1, H2); + } + + static ManifoldType Between(const ManifoldType& m1, + const ManifoldType& m2, + ChartJacobian H1, + ChartJacobian H2 = boost::none) { + return m1.between(m2, H1, H2); + } + + static ManifoldType Inverse(const ManifoldType& m, + ChartJacobian H) { + return m.inverse(H); + } + + /// @} + +}; + +} // \ namepsace internal + /** * These core global functions can be specialized by new Lie types * for better performance. @@ -31,75 +130,53 @@ namespace gtsam { /** Compute l0 s.t. l2=l1*l0 */ template -inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} +inline T between_default(const T& l1, const T& l2) { + return l1.inverse().compose(l2); +} /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ template -inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} +inline Vector logmap_default(const T& l0, const T& lp) { + return T::Logmap(l0.between(lp)); +} /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ template -inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} +inline T expmap_default(const T& t, const Vector& d) { + return t.compose(T::Expmap(d)); +} /** - * Concept check class for Lie group type - * - * 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 - * the function exists and throw compile-time errors. - * - * The exponential map is a specific retraction for Lie groups, - * which maps the tangent space in canonical coordinates back to - * the underlying manifold. Because we can enforce a group structure, - * a retraction of delta v, with tangent space centered at x1 can be performed - * as x2 = x1.compose(Expmap(v)). - * - * Expmap around identity - * static T Expmap(const Vector& v); - * - * Logmap around identity - * static Vector Logmap(const T& p); - * - * Compute l0 s.t. l2=l1*l0, where (*this) is l1 - * A default implementation of between(*this, lp) is available: - * between_default() - * T between(const T& l2) const; - * - * By convention, we use capital letters to designate a static function - * - * @tparam T is a Lie type, like Point2, Pose3, etc. + * Lie Group Concept */ -template -class LieConcept { -private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { +template +class IsLieGroup: public IsGroup, public IsManifold { +public: + typedef typename traits_x::structure_category structure_category_tag; + typedef typename traits_x::ManifoldType ManifoldType; + typedef typename traits_x::TangentVector TangentVector; + typedef typename traits_x::ChartJacobian ChartJacobian; - /** assignment */ - T t2 = t; + BOOST_CONCEPT_USAGE(IsLieGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it is a Lie group (or derived)"); - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); - - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); - - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = expmap_identity_ret.between(t2); - - return between_ret; + // group opertations with Jacobians + g = traits_x::Compose(g, h, Hg, Hh); + g = traits_x::Between(g, h, Hg, Hh); + g = traits_x::Inverse(g, Hg); + // log and exp map without Jacobians + g = traits_x::Expmap(v); + v = traits_x::Logmap(g); + // log and exp map with Jacobians + //g = traits_x::Expmap(v, Hg); + //v = traits_x::Logmap(g, Hg); } - +private: + LG g, h; + TangentVector v; + ChartJacobian Hg, Hh; }; /** @@ -146,8 +223,5 @@ T expm(const Vector& x, int K=7) { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -#define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::IsLieGroup; - -#define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::IsLieGroup _gtsam_LieConcept_##T; +#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; +#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup _gtsam_IsLieGroup_##T; diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index ba665e6e5..486513037 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -74,7 +74,7 @@ struct LieMatrix : public Matrix { inline bool equals(const LieMatrix& expected, double tol=1e-5) const { return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); } - + /// @} /// @name Standard Interface /// @{ @@ -83,7 +83,7 @@ struct LieMatrix : public Matrix { inline Matrix matrix() const { return static_cast(*this); } - + /// @} /// @name Manifold interface /// @{ @@ -102,8 +102,10 @@ struct LieMatrix : public Matrix { Eigen::Map >( &v(0), this->rows(), this->cols())); } - inline LieMatrix retract(const Vector& v, OptionalJacobian<-1,-1> Horigin, OptionalJacobian<-1,-1> Hv) const { - CONCEPT_NOT_IMPLEMENTED; + inline LieMatrix retract(const Vector& v, OptionalJacobian<-1, -1> Horigin, + OptionalJacobian<-1, -1> Hv) const { + if (Horigin || Hv) + throw std::runtime_error("LieMatrix::retract derivative not implemented"); return retract(v); } /** @return the local coordinates of another object. The elements of the @@ -115,8 +117,10 @@ struct LieMatrix : public Matrix { &result(0), this->rows(), this->cols()) = t2 - *this; return result; } - Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1,-1> Horigin, OptionalJacobian<-1,-1> Hother) const { - CONCEPT_NOT_IMPLEMENTED; + Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1, -1> Horigin, + OptionalJacobian<-1, -1> Hother) const { + if (Horigin || Hother) + throw std::runtime_error("LieMatrix::localCoordinates derivative not implemented"); return localCoordinates(ts); } @@ -171,13 +175,13 @@ struct LieMatrix : public Matrix { /** Logmap around identity */ static inline Vector Logmap(const LieMatrix& p, OptionalJacobian<-1,-1> H = boost::none) { - if (H) { CONCEPT_NOT_IMPLEMENTED; } + if (H) throw std::runtime_error("LieMatrix::Logmap derivative not implemented"); Vector result(p.size()); Eigen::Map >( result.data(), p.rows(), p.cols()) = p; return result; } - + /// @} private: diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 5cd4cc8dd..7ecd2d50e 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -24,8 +24,7 @@ #endif #include -#include -#include +#include namespace gtsam { diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index ec63c733d..c3c3dbad7 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -125,14 +125,18 @@ struct LieVector : public Vector { // Lie functions /** Expmap around identity */ - static LieVector Expmap(const Vector& v, OptionalJacobian<-1,-1> H = boost::none) { - if (H) { CONCEPT_NOT_IMPLEMENTED; } + static LieVector Expmap(const Vector& v, OptionalJacobian<-1, -1> H = + boost::none) { + if (H) + throw std::runtime_error("LieVector::Expmap derivative not implemented"); return LieVector(v); } /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieVector& p, OptionalJacobian<-1,-1> H = boost::none) { - if (H) { CONCEPT_NOT_IMPLEMENTED; } + static Vector Logmap(const LieVector& p, OptionalJacobian<-1, -1> H = + boost::none) { + if (H) + throw std::runtime_error("LieVector::Logmap derivative not implemented"); return p; } diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 85fa9b26d..d60464e9e 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -14,18 +14,24 @@ * @brief Base class and basic functions for Manifold types * @author Alex Cunningham * @author Frank Dellaert + * @author Mike Bosse */ #pragma once -#include #include -#include -#include -#include -// +#include +#include + +#include +#include +#include + namespace gtsam { +/// tag to assert a type is a manifold +struct manifold_tag {}; + /** * 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 @@ -44,322 +50,112 @@ namespace gtsam { * */ +template struct traits_x; + +namespace internal { + +/// A helper that implements the traits interface for GTSAM manifolds. +/// To use this for your gtsam type, define: +/// template<> struct traits : public Manifold { }; +template +struct Manifold : Testable<_ManifoldType> { + // Typedefs required by all manifold types. + typedef _ManifoldType ManifoldType; + typedef manifold_tag structure_category; + enum { dimension = ManifoldType::dimension }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + static TangentVector Local(const ManifoldType& origin, + const ManifoldType& other) { + return origin.localCoordinates(other); + } + + static ManifoldType Retract(const ManifoldType& origin, + const TangentVector& v) { + return origin.retract(v); + } + + static TangentVector Local(const ManifoldType& origin, + const ManifoldType& other, + ChartJacobian Horigin, + ChartJacobian Hother) { + return origin.localCoordinates(other, Horigin, Hother); + } + + static ManifoldType Retract(const ManifoldType& origin, + const TangentVector& v, + ChartJacobian Horigin, + ChartJacobian Hv) { + return origin.retract(v, Horigin, Hv); + } + + static int GetDimension(const ManifoldType& m){ return m.dim(); } + +}; + +} // \ namespace internal + +/// Check invariants for Manifold type +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { + typename traits_x::TangentVector v0 = traits_x::Local(a,a); + typename traits_x::TangentVector v = traits_x::Local(a,b); + T c = traits_x::Retract(a,v); + return v0.norm() < tol && traits_x::Equals(b,c,tol); +} + +#define GTSAM_MANIFOLD_DECLARATIONS(MANIFOLD,DIM,TANGENT_VECTOR) \ + typedef MANIFOLD ManifoldType;\ + typedef manifold_tag structure_category; \ + struct dimension : public boost::integral_constant {};\ + typedef TANGENT_VECTOR TangentVector;\ + typedef OptionalJacobian ChartJacobian; \ + static TangentVector Local(const ManifoldType& origin, \ + const ManifoldType& other, \ + ChartJacobian Horigin=boost::none, \ + ChartJacobian Hother=boost::none); \ + static ManifoldType Retract(const ManifoldType& origin, \ + const TangentVector& v,\ + ChartJacobian Horigin=boost::none, \ + ChartJacobian Hv=boost::none); \ + static int GetDimension(const ManifoldType& m) { return dimension; } + +/** + * Manifold concept + */ +template +class IsManifold { +public: + typedef typename traits_x::structure_category structure_category_tag; + static const size_t dim = traits_x::dimension; + typedef typename traits_x::ManifoldType ManifoldType; + typedef typename traits_x::TangentVector TangentVector; + typedef typename traits_x::ChartJacobian ChartJacobian; + + BOOST_CONCEPT_USAGE(IsManifold) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + + // make sure Chart methods are defined + v = traits_x::Local(p,q); + q = traits_x::Retract(p,v); + // and the versions with Jacobians. + //v = traits_x::Local(p,q,Hp,Hq); + //q = traits_x::Retract(p,v,Hp,Hv); + } +private: + ManifoldType p,q; + ChartJacobian Hp,Hq,Hv; + TangentVector v; + bool b; +}; -//// Traits, same style as Boost.TypeTraits -//// All meta-functions below ever only declare a single type -//// or a type/value/value_type -//namespace traits { -// -//// is group, by default this is false -//template -//struct is_group: public boost::false_type { -//}; -// -//// identity, no default provided, by default given by default constructor -//template -//struct identity { -// static T value() { -// return T(); -// } -//}; -// -//// is manifold, by default this is false -//template -//struct is_manifold: public boost::false_type { -//}; -// -//// dimension, can return Eigen::Dynamic (-1) if not known at compile time -//// defaults to dynamic, TODO makes sense ? -//typedef boost::integral_constant Dynamic; -////template -////struct dimension: public Dynamic { -////struct dimension : public boost::integral_constant { // just temporary fix to minimize compiler errors while refactoring -////}; -// -///** -// * zero::value is intended to be the origin of a canonical coordinate system -// * with canonical(t) == DefaultChart::local(zero::value, t) -// * Below we provide the group identity as zero *in case* it is a group -// */ -//template struct zero: public identity { -// BOOST_STATIC_ASSERT(is_group::value); -//}; -// -//// double -// -//template<> -//struct is_group : public boost::true_type { -//}; -// -//template<> -//struct is_manifold : public boost::true_type { -//}; -// -//template<> -//struct dimension : public boost::integral_constant { -//}; -// -//template<> -//struct zero { -// static double value() { -// return 0; -// } -//}; -// -//// Fixed size Eigen::Matrix type -// -//template -//struct is_group > : public boost::true_type { -//}; -// -//template -//struct is_manifold > : public boost::true_type { -//}; -// -//template -//struct dimension > : public boost::integral_constant< -// int, -// M == Eigen::Dynamic ? Eigen::Dynamic : -// (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { -// //TODO after switch to c++11 : the above should should be extracted to a constexpr function -// // for readability and to reduce code duplication -//}; -// -//template -//struct zero > { -// BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), -// "traits::zero is only supported for fixed-size matrices"); -// static Eigen::Matrix value() { -// return Eigen::Matrix::Zero(); -// } -//}; -// -//template -//struct identity > : public zero< -// Eigen::Matrix > { -//}; -// -//template struct is_chart: public boost::false_type { -//}; -// -//} // \ namespace traits -// -//// Chart is a map from T -> vector, retract is its inverse -//template -//struct DefaultChart { -// //BOOST_STATIC_ASSERT(traits::is_manifold::value); -// typedef T type; -// typedef Eigen::Matrix::dimension, 1> vector; -// -// static vector local(const T& origin, const T& other) { -// return origin.localCoordinates(other); -// } -// static T retract(const T& origin, const vector& d) { -// return origin.retract(d); -// } -// static int getDimension(const T& origin) { -// return origin.dim(); -// } -//}; -// -//namespace traits { -//// populate default traits -//template struct is_chart > : public boost::true_type { -//}; -//template struct dimension > : public dimension { -//}; -//} -// -//template -//struct ChartConcept { -//public: -// typedef typename C::type type; -// typedef typename C::vector vector; -// -// BOOST_CONCEPT_USAGE(ChartConcept) { -// // is_chart trait should be true -// BOOST_STATIC_ASSERT((traits::is_chart::value)); -// -// /** -// * Returns Retraction update of val_ -// */ -// type retract_ret = C::retract(val_, vec_); -// -// /** -// * Returns local coordinates of another object -// */ -// vec_ = C::local(val_, retract_ret); -// -// // a way to get the dimension that is compatible with dynamically sized types -// dim_ = C::getDimension(val_); -// } -// -//private: -// type val_; -// vector vec_; -// int dim_; -//}; -// -///** -// * CanonicalChart > is a chart around zero::value -// * Canonical is CanonicalChart > -// * An example is Canonical -// */ -//template struct CanonicalChart { -// BOOST_CONCEPT_ASSERT((ChartConcept)); -// -// typedef C Chart; -// typedef typename Chart::type type; -// typedef typename Chart::vector vector; -// -// // Convert t of type T into canonical coordinates -// vector local(const type& t) { -// return Chart::local(traits::zero::value(), t); -// } -// // Convert back from canonical coordinates to T -// type retract(const vector& v) { -// return Chart::retract(traits::zero::value(), v); -// } -//}; -//template struct Canonical: public CanonicalChart > { -//}; -// -//// double -// -//template<> -//struct DefaultChart { -// typedef double type; -// typedef Eigen::Matrix vector; -// -// static vector local(double origin, double other) { -// vector d; -// d << other - origin; -// return d; -// } -// static double retract(double origin, const vector& d) { -// return origin + d[0]; -// } -// static int getDimension(double) { -// return 1; -// } -//}; -// -//// Fixed size Eigen::Matrix type -// -//template -//struct DefaultChart > { -// /** -// * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). -// * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. -// */ -// typedef Eigen::Matrix type; -// typedef type T; -// typedef Eigen::Matrix::value, 1> vector; -// -// BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), -// "Internal error: DefaultChart for Dynamic should be handled by template below"); -// -// static vector local(const T& origin, const T& other) { -// return reshape(other) -// - reshape(origin); -// } -// static T retract(const T& origin, const vector& d) { -// return origin + reshape(d); -// } -// static int getDimension(const T&origin) { -// return origin.rows() * origin.cols(); -// } -//}; -// -//// Dynamically sized Vector -//template<> -//struct DefaultChart { -// typedef Vector type; -// typedef Vector vector; -// static vector local(const Vector& origin, const Vector& other) { -// return other - origin; -// } -// static Vector retract(const Vector& origin, const vector& d) { -// return origin + d; -// } -// static int getDimension(const Vector& origin) { -// return origin.size(); -// } -//}; -// -//// Dynamically sized Matrix -//template<> -//struct DefaultChart { -// typedef Matrix type; -// typedef Vector vector; -// static vector local(const Matrix& origin, const Matrix& other) { -// Matrix d = other - origin; -// return Eigen::Map(d.data(),getDimension(d)); -// } -// static Matrix retract(const Matrix& origin, const vector& d) { -// return origin + Eigen::Map(d.data(),origin.rows(),origin.cols()); -// } -// static int getDimension(const Matrix& m) { -// return m.size(); -// } -//}; -// -///** -// * Old Concept check class for Manifold types -// * Requires a mapping between a linear tangent space and the underlying -// * manifold, of which Lie is a specialization. -// * -// * 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, which may be smaller than the number -// * of nonlinear parameters. -// * size_t dim() const; -// * -// * Returns a new T that is a result of updating *this with the delta v after pulling -// * the updated value back to the manifold T. -// * T retract(const Vector& v) const; -// * -// * Returns the linear coordinates of lp in the tangent space centered around *this. -// * Vector localCoordinates(const T& lp) const; -// * -// * By convention, we use capital letters to designate a static function -// * @tparam T is a Lie type, like Point2, Pose3, etc. -// */ -//template -//class ManifoldConcept { -//private: -// /** concept checking function - implement the functions this demands */ -// static T 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)); -// -// /** -// * Returns local coordinates of another object -// */ -// Vector localCoords_ret = t.localCoordinates(t2); -// -// return retract_ret; -// } -//}; -// } // \ namespace gtsam -// -// TODO(ASL) Remove these and fix the compiler errors. ///** // * Macros for using the ManifoldConcept // * - An instantiation for use inside unit tests @@ -368,5 +164,5 @@ namespace gtsam { // * NOTE: intentionally not in the gtsam namespace to allow for classes not in // * the gtsam namespace to be more easily enforced as testable // */ -#define GTSAM_CONCEPT_MANIFOLD_INST(T) -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) +#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold _gtsam_IsManifold_##T; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 4fac6d3e0..0c88886b4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -22,7 +22,6 @@ #pragma once -#include #include #include #include diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h new file mode 100644 index 000000000..8a4fd4c7c --- /dev/null +++ b/gtsam/base/VectorSpace.h @@ -0,0 +1,237 @@ +/* + * VectorSpace.h + * + * @date December 21, 2014 + * @author Mike Bosse + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/// tag to assert a type is a vector space +struct vector_space_tag: public lie_group_tag {}; + +template struct traits_x; + +namespace internal { + +/// 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; + typedef Scalar ManifoldType; + enum { dimension = 1 }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian<1, 1> ChartJacobian; + + // For Testable + static void Print(Scalar m, const std::string& str = "") { + gtsam::print(m, str); + } + static bool Equals(Scalar m1, Scalar m2, double tol = 1e-8) { + return fabs(m1 - m2) < tol; + } + + static TangentVector Local(Scalar origin, Scalar other) { + TangentVector result; + result(0) = other - origin; + return result; + } + + static Scalar Retract(Scalar origin, const TangentVector& v) { + return origin + v[0]; + } + + 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); + } + + 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; } + + // 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 = boost::none) { + if (H1) (*H1)[0] = 1.0; + if (H2) (*H2)[0] = 1.0; + return m1 + 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, ChartJacobian H) { + if (H) (*H)[0] = -1; + return -m; + } + + static Scalar Identity() { return 0; } + static TangentVector Logmap(Scalar m) {return Local(0,m);} + static Scalar Expmap(const TangentVector& v) { return v[0];} + + static TangentVector Logmap(Scalar m, ChartJacobian Hm) { + if (Hm) (*Hm)[0] = 1.0; + return Local(0,m); + } + + static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { + if (Hv) (*Hv)[0] = 1.0; + return v[0]; + } + +}; + +} // namespace internal + +/// double +template<> struct traits_x : public internal::ScalarTraits {}; + +/// float +template<> struct traits_x : public internal::ScalarTraits {}; + +// traits for any double Eigen matrix +template +struct traits_x< Eigen::Matrix > { +// BOOST_STATIC_ASSERT_MSG( +// M != Eigen::Dynamic && N != Eigen::Dynamic, +// "These traits are only valid on fixed-size types."); + + // Typedefs required by all manifold types. + typedef vector_space_tag structure_category; + enum { dimension = (M == Eigen::Dynamic ? Eigen::Dynamic : + (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)) }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix ManifoldType; + + static int GetDimension(const ManifoldType& m){ return m.rows()*m.cols(); } + + static Eigen::Matrix Eye(const ManifoldType& m) { + int dim = GetDimension(m); + return Eigen::Matrix::Identity(dim,dim); + } + + // For Testable + static void Print(const ManifoldType& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const ManifoldType& m1, const ManifoldType& m2, + double tol = 1e-8) { + return equal_with_abs_tol(m1, m2, 1e-9); + } + + static TangentVector Local(const ManifoldType& origin, + const ManifoldType& other, + ChartJacobian Horigin = boost::none, + ChartJacobian Hother = boost::none) { + if (Horigin) *Horigin = -Eye(origin); + if (Hother) *Hother = Eye(origin); + TangentVector result(GetDimension(origin)); + Eigen::Map >( + result.data(), origin.rows(), origin.cols()) = other - origin; + return result; + } + + static ManifoldType Retract(const ManifoldType& origin, + const TangentVector& v, + ChartJacobian Horigin = boost::none, + ChartJacobian Hv = boost::none) { + if (Horigin) *Horigin = Eye(origin); + if (Hv) *Hv = Eye(origin); + return origin + Eigen::Map >(v.data(), origin.rows(), origin.cols()); + } + + static ManifoldType Compose(const ManifoldType& m1, + const ManifoldType& m2, + ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(m1); + if (H2) *H2 = Eye(m1); + return m1+m2; + } + + static ManifoldType Between(const ManifoldType& m1, + const ManifoldType& m2, + ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { + if (H1) *H1 = -Eye(m1); + if (H2) *H2 = Eye(m1); + return m2-m1; + } + + static ManifoldType Inverse(const ManifoldType& m, + ChartJacobian H = boost::none) { + if (H) *H = -Eye(m); + return -m; + } + + static ManifoldType Identity() { + //FIXME: this won't work for dynamic matrices, but where to get the size??? + return ManifoldType::Zero(); + } + + static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) { + if (Hm) *Hm = Eye(m); + TangentVector result(GetDimension(m)); + Eigen::Map >( + result.data()) = m; + return result; + } + + //FIXME: this also does not work for dynamic matrices + static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + ManifoldType m; m.setZero(); + if (Hv) *Hv = Eye(m); + return m + + Eigen::Map >(v.data()); + } + +}; + +/// Vector Space concept +template +class IsVectorSpace: public IsLieGroup { +public: + + typedef typename traits_x::structure_category structure_category_tag; + + BOOST_CONCEPT_USAGE(IsVectorSpace) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a vector space (or derived)"); + r = p + q; + r = -p; + r = p - q; + } + +private: + V p, q, r; +}; + +} // namespace gtsam + diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 861488558..da01be89c 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -1,16 +1,18 @@ /* * concepts.h * - * @data Dec 4, 2014 + * @date Dec 4, 2014 * @author Mike Bosse * @author Frank Dellaert */ #pragma once +#include #include #include #include + #include #include #include @@ -31,357 +33,8 @@ namespace gtsam { - - -/** - * @name Algebraic Structure Tags - * @brief Possible values for traits::structure_category::type - */ -//@{ -struct manifold_tag {}; -struct group_tag {}; -struct lie_group_tag: public manifold_tag, public group_tag {}; -struct vector_space_tag: public lie_group_tag {}; -//@} - -// Group operator syntax flavors -struct multiplicative_group_tag {}; -struct additive_group_tag {}; - template struct traits_x; -namespace internal { - -/// A helper that implements the traits interface for GTSAM manifolds. -/// To use this for your gtsam type, define: -/// template<> struct traits : public Manifold { }; -template -struct Manifold : Testable<_ManifoldType> { - // Typedefs required by all manifold types. - typedef _ManifoldType ManifoldType; - typedef manifold_tag structure_category; - enum { dimension = ManifoldType::dimension }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; - - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other) { - return origin.localCoordinates(other); - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v) { - return origin.retract(v); - } - - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other, - ChartJacobian Horigin, - ChartJacobian Hother) { - return origin.localCoordinates(other, Horigin, Hother); - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v, - ChartJacobian Horigin, - ChartJacobian Hv) { - return origin.retract(v, Horigin, Hv); - } - - static int GetDimension(const ManifoldType& m){ return m.dim(); } - -}; - - -/// A helper that implements the traits interface for GTSAM lie groups. -/// To use this for your gtsam type, define: -/// template<> struct traits : public LieGroup { }; -template -struct LieGroup : Testable<_ManifoldType> { - // Typedefs required by all manifold types. - typedef _ManifoldType ManifoldType; - typedef lie_group_tag structure_category; - typedef _group_flavor group_flavor; - - enum { dimension = ManifoldType::dimension }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; - - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other) { - return origin.localCoordinates(other); - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v) { - return origin.retract(v); - } - - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other, - ChartJacobian Horigin, - ChartJacobian Hother) { - return origin.localCoordinates(other, Horigin, Hother); - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v, - ChartJacobian Horigin, - ChartJacobian Hv) { - return origin.retract(v, Horigin, Hv); - } - - static int GetDimension(const ManifoldType& m){ return m.dim(); } - - // For Group. Only implemented for groups - static ManifoldType Compose(const ManifoldType& m1, - const ManifoldType& m2) { - return m1.compose(m2); - } - - static ManifoldType Between(const ManifoldType& m1, - const ManifoldType& m2) { - return m1.between(m2); - } - - static ManifoldType Inverse(const ManifoldType& m) { - return m.inverse(); - } - - static ManifoldType Compose(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1, - ChartJacobian H2) { - return m1.compose(m2, H1, H2); - } - - static ManifoldType Between(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1, - ChartJacobian H2) { - return m1.between(m2, H1, H2); - } - - static ManifoldType Inverse(const ManifoldType& m, - ChartJacobian H) { - return m.inverse(H); - } - - static ManifoldType Identity() { - return ManifoldType::identity(); - } - - static TangentVector Logmap(const ManifoldType& m) { - return ManifoldType::Logmap(m); - } - - static ManifoldType Expmap(const TangentVector& v) { - return ManifoldType::Expmap(v); - } - - static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) { - return ManifoldType::Logmap(m, Hm); - } - - static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) { - return ManifoldType::Expmap(v, Hv); - } - -}; - -/// 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; - typedef Scalar ManifoldType; - enum { dimension = 1 }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian<1, 1> ChartJacobian; - - // For Testable - static void Print(Scalar m, const std::string& str = "") { - gtsam::print(m, str); - } - static bool Equals(Scalar m1, Scalar m2, double tol = 1e-8) { - return fabs(m1 - m2) < tol; - } - - static TangentVector Local(Scalar origin, Scalar other) { - TangentVector result; - result(0) = other - origin; - return result; - } - - static Scalar Retract(Scalar origin, const TangentVector& v) { - return origin + v[0]; - } - - 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); - } - - 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; } - - // 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 = boost::none) { - if (H1) (*H1)[0] = 1.0; - if (H2) (*H2)[0] = 1.0; - return m1 + 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, ChartJacobian H) { - if (H) (*H)[0] = -1; - return -m; - } - - static Scalar Identity() { return 0; } - static TangentVector Logmap(Scalar m) {return Local(0,m);} - static Scalar Expmap(const TangentVector& v) { return v[0];} - - static TangentVector Logmap(Scalar m, ChartJacobian Hm) { - if (Hm) (*Hm)[0] = 1.0; - return Local(0,m); - } - - static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { - if (Hv) (*Hv)[0] = 1.0; - return v[0]; - } - -}; - -} // namespace internal - -template<> -struct traits_x : public internal::ScalarTraits {}; -template<> -struct traits_x : public internal::ScalarTraits {}; - -// traits for any double Eigen matrix -template -struct traits_x< Eigen::Matrix > { -// BOOST_STATIC_ASSERT_MSG( -// M != Eigen::Dynamic && N != Eigen::Dynamic, -// "These traits are only valid on fixed-size types."); - - // Typedefs required by all manifold types. - typedef vector_space_tag structure_category; - enum { dimension = (M == Eigen::Dynamic ? Eigen::Dynamic : - (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)) }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; - typedef Eigen::Matrix ManifoldType; - - static int GetDimension(const ManifoldType& m){ return m.rows()*m.cols(); } - - static Eigen::Matrix Eye(const ManifoldType& m) { - int dim = GetDimension(m); - return Eigen::Matrix::Identity(dim,dim); - } - - // For Testable - static void Print(const ManifoldType& m, const std::string& str = "") { - gtsam::print(Eigen::MatrixXd(m), str); - } - static bool Equals(const ManifoldType& m1, const ManifoldType& m2, - double tol = 1e-8) { - return equal_with_abs_tol(m1, m2, 1e-9); - } - - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other, - ChartJacobian Horigin = boost::none, - ChartJacobian Hother = boost::none) { - if (Horigin) *Horigin = -Eye(origin); - if (Hother) *Hother = Eye(origin); - TangentVector result(GetDimension(origin)); - Eigen::Map >( - result.data(), origin.rows(), origin.cols()) = other - origin; - return result; - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v, - ChartJacobian Horigin = boost::none, - ChartJacobian Hv = boost::none) { - if (Horigin) *Horigin = Eye(origin); - if (Hv) *Hv = Eye(origin); - return origin + Eigen::Map >(v.data(), origin.rows(), origin.cols()); - } - - static ManifoldType Compose(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { - if (H1) *H1 = Eye(m1); - if (H2) *H2 = Eye(m1); - return m1+m2; - } - - static ManifoldType Between(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { - if (H1) *H1 = -Eye(m1); - if (H2) *H2 = Eye(m1); - return m2-m1; - } - - static ManifoldType Inverse(const ManifoldType& m, - ChartJacobian H = boost::none) { - if (H) *H = -Eye(m); - return -m; - } - - static ManifoldType Identity() { - //FIXME: this won't work for dynamic matrices, but where to get the size??? - return ManifoldType::Zero(); - } - - static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) { - if (Hm) *Hm = Eye(m); - TangentVector result(GetDimension(m)); - Eigen::Map >( - result.data()) = m; - return result; - } - - //FIXME: this also does not work for dynamic matrices - static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - ManifoldType m; m.setZero(); - if (Hv) *Hv = Eye(m); - return m + - Eigen::Map >(v.data()); - } - -}; - template struct Canonical { BOOST_STATIC_ASSERT_MSG( @@ -413,178 +66,5 @@ struct Canonical { } }; -/// Check invariants for Manifold type -template -BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // -check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { - typename traits_x::TangentVector v0 = traits_x::Local(a,a); - typename traits_x::TangentVector v = traits_x::Local(a,b); - T c = traits_x::Retract(a,v); - return v0.norm() < tol && traits_x::Equals(b,c,tol); -} - -#define GTSAM_MANIFOLD_DECLARATIONS(MANIFOLD,DIM,TANGENT_VECTOR) \ - typedef MANIFOLD ManifoldType;\ - typedef manifold_tag structure_category; \ - struct dimension : public boost::integral_constant {};\ - typedef TANGENT_VECTOR TangentVector;\ - typedef OptionalJacobian ChartJacobian; \ - static TangentVector Local(const ManifoldType& origin, \ - const ManifoldType& other, \ - ChartJacobian Horigin=boost::none, \ - ChartJacobian Hother=boost::none); \ - static ManifoldType Retract(const ManifoldType& origin, \ - const TangentVector& v,\ - ChartJacobian Horigin=boost::none, \ - ChartJacobian Hv=boost::none); \ - static int GetDimension(const ManifoldType& m) { return dimension; } - -/** - * Manifold concept - */ -template -class IsManifold { -public: - typedef typename traits_x::structure_category structure_category_tag; - static const size_t dim = traits_x::dimension; - typedef typename traits_x::ManifoldType ManifoldType; - typedef typename traits_x::TangentVector TangentVector; - typedef typename traits_x::ChartJacobian ChartJacobian; - - BOOST_CONCEPT_USAGE(IsManifold) { - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), - "This type's structure_category trait does not assert it as a manifold (or derived)"); - BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - - // make sure Chart methods are defined - v = traits_x::Local(p,q); - q = traits_x::Retract(p,v); - // and the versions with Jacobians. - //v = traits_x::Local(p,q,Hp,Hq); - //q = traits_x::Retract(p,v,Hp,Hv); - } -private: - ManifoldType p,q; - ChartJacobian Hp,Hq,Hv; - TangentVector v; - bool b; -}; - -/** - * Group Concept - */ -template -class IsGroup { -public: - typedef typename traits_x::structure_category structure_category_tag; - typedef typename traits_x::group_flavor flavor_tag; - //typedef typename traits_x::identity::value_type identity_value_type; - - BOOST_CONCEPT_USAGE(IsGroup) { - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), - "This type's structure_category trait does not assert it as a group (or derived)"); - e = traits_x::Identity(); - e = traits_x::Compose(g, h); - e = traits_x::Between(g, h); - e = traits_x::Inverse(g); - operator_usage(flavor); - // todo: how do we test the act concept? or do we even need to? - } - -private: - void operator_usage(multiplicative_group_tag) { - e = g * h; - //e = -g; // todo this should work, but it is failing for Quaternions - } - void operator_usage(additive_group_tag) { - e = g + h; - e = h - g; - e = -g; - } - - flavor_tag flavor; - G e, g, h; - bool b; -}; - -/// Check invariants -template -BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // -check_group_invariants(const G& a, const G& b, double tol = 1e-9) { - G e = traits_x::Identity(); - return traits_x::Equals(traits_x::Compose(a, traits_x::Inverse(a)), e, tol) - && traits_x::Equals(traits_x::Between(a, b), traits_x::Compose(traits_x::Inverse(a), b), tol) - && traits_x::Equals(traits_x::Compose(a, traits_x::Between(a, b)), b, tol); -} - -/// Macro to add group traits, additive flavor -#define GTSAM_ADDITIVE_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 h - g;} \ - static GROUP Inverse(const GROUP &g) { return -g;} - -/// Macro to add group traits, multiplicative flavor -#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 - */ -template -class IsLieGroup: public IsGroup, public IsManifold { -public: - typedef typename traits_x::structure_category structure_category_tag; - typedef typename traits_x::ManifoldType ManifoldType; - typedef typename traits_x::TangentVector TangentVector; - typedef typename traits_x::ChartJacobian ChartJacobian; - - BOOST_CONCEPT_USAGE(IsLieGroup) { - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), - "This type's trait does not assert it is a Lie group (or derived)"); - - // group opertations with Jacobians - g = traits_x::Compose(g, h, Hg, Hh); - g = traits_x::Between(g, h, Hg, Hh); - g = traits_x::Inverse(g, Hg); - // log and exp map without Jacobians - g = traits_x::Expmap(v); - v = traits_x::Logmap(g); - // log and exp map with Jacobians - //g = traits_x::Expmap(v, Hg); - //v = traits_x::Logmap(g, Hg); - } -private: - LG g, h; - TangentVector v; - ChartJacobian Hg, Hh; -}; - - -template -class IsVectorSpace: public IsLieGroup { -public: - - typedef typename traits_x::structure_category structure_category_tag; - - BOOST_CONCEPT_USAGE(IsVectorSpace) { - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), - "This type's trait does not assert it as a vector space (or derived)"); - r = p + q; - r = -p; - r = p - q; - } - -private: - V p, q, r; -}; - } // namespace gtsam diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 0cc533e61..cda220ec0 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -28,12 +28,10 @@ #pragma GCC diagnostic pop #endif -#include -#include -#include #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 3c8790bea..c89390c04 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -15,8 +15,11 @@ * @author Frank Dellaert **/ -#include +#include #include +#include +#include +#include namespace gtsam { @@ -28,7 +31,7 @@ public: /// Constructor Cyclic(size_t i) : i_(i) { - assert(i struct traits_x > { - typedef group_tag structure_category; - GTSAM_ADDITIVE_GROUP(Cyclic) - static Cyclic Identity() { return Cyclic::Identity();} - static bool Equals(const Cyclic& a, const Cyclic& b, double tol = 1e-9) { + typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) + static Cyclic Identity() { + return Cyclic::Identity(); + } + static bool Equals(const Cyclic& a, const Cyclic& b, + double tol = 1e-9) { return a.equals(b, tol); } - static void Print(const Cyclic& c, const std::string &s = "") {c.print(s);} + static void Print(const Cyclic& c, const std::string &s = "") { + c.print(s); + } }; } // \namespace gtsam diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 17e3ef370..4b2111efc 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include @@ -24,9 +23,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Point2); - /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 4a7809cad..b16d7f2d0 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -17,12 +17,9 @@ #pragma once +#include #include -#include -#include -#include - namespace gtsam { /** @@ -158,17 +155,14 @@ public: inline size_t dim() const { return 2; } /// Updates a with tangent space delta - inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - inline Point2 retract(const Vector& v, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const { - CONCEPT_NOT_IMPLEMENTED; + inline Point2 retract(const Vector& v) const { return *this + Point2(v); } /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - inline Vector localCoordinates(const Point2& t2, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const { - CONCEPT_NOT_IMPLEMENTED; - return Logmap(between(t2)); + inline Vector localCoordinates(const Point2& t2) const { + Point2 dp = t2 - *this; + return Vector2(dp.x(), dp.y()); } /// @} @@ -176,20 +170,17 @@ public: /// @{ /// Exponential map around identity - just create a Point2 from a vector - static inline Point2 Expmap(const Vector2& v) { return Point2(v); } - static Point2 Expmap(const Vector2& v, OptionalJacobian<2,2> H) { - CONCEPT_NOT_IMPLEMENTED; + static Point2 Expmap(const Vector2& v, OptionalJacobian<2, 2> H) { + if (H) *H = I_2x2; + return Point2(v); } /// Logmap around identity - static inline Vector2 Logmap(const Point2& dp) { + static inline Vector2 Logmap(const Point2& dp, OptionalJacobian<2, 2> H) { + if (H) *H = I_2x2; return Vector2(dp.x(), dp.y()); } - static inline Vector2 Logmap(const Point2& dp, OptionalJacobian<2,2> H) { - CONCEPT_NOT_IMPLEMENTED; - } - /// Left-trivialized derivative of the exponential map static Matrix2 dexpL(const Vector2& v) { return I_2x2; @@ -200,6 +191,23 @@ public: return I_2x2; } + /// Updates a with tangent space delta + inline Point2 retract(const Vector& v, OptionalJacobian<2, 2> H1, + OptionalJacobian<2, 2> H2 = boost::none) const { + if (H1) *H1 = I_2x2; + if (H2) *H2 = I_2x2; + return *this + Point2(v); + } + + /// Local coordinates of manifold neighborhood around current value + inline Vector localCoordinates(const Point2& t2, OptionalJacobian<2, 2> H1, + OptionalJacobian<2, 2> H2 = boost::none) const { + if (H1) *H1 = - I_2x2; + if (H2) *H2 = I_2x2; + Point2 dp = t2 - *this; + return Vector2(dp.x(), dp.y()); + } + /// @} /// @name Vector Space /// @{ diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 330fafb97..a87aeb650 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -15,15 +15,11 @@ */ #include -#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Point3); - /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 6075c2643..0cefb79c8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,12 +21,8 @@ #pragma once -#include -#include -#include - +#include #include - #include namespace gtsam { @@ -131,18 +127,10 @@ namespace gtsam { /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return Point3(*this + v); } - inline Point3 retract(const Vector& v, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> Hv) const { - CONCEPT_NOT_IMPLEMENTED; - return Point3(*this + v); - } /// Returns inverse retraction inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } - /// Returns inverse retraction - inline Vector3 localCoordinates(const Point3& q, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> Ha) const { - CONCEPT_NOT_IMPLEMENTED; return (q -*this).vector(); - } /// @} /// @name Lie Group /// @{ @@ -159,6 +147,21 @@ namespace gtsam { return Vector3(dp.x(), dp.y(), dp.z()); } + inline Point3 retract(const Vector& v, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; + return *this + Point3(v); + } + + inline Vector3 localCoordinates(const Point3& q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) *H1 = - I_3x3; + if (H2) *H2 = I_3x3; + Point3 dp = q - *this; + return Vector3(dp.x(), dp.y(), dp.z()); + } + /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector& v) { return I_3x3; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index fa3606704..f32580ed2 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include @@ -27,9 +26,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose2); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); @@ -111,8 +107,12 @@ Vector3 Pose2::localCoordinates(const Pose2& p2) const { } /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose -Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3,3> Hthis, OptionalJacobian<3,3> Hother) const { - CONCEPT_NOT_IMPLEMENTED; +Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis, + OptionalJacobian<3, 3> Hother) const { + if (Hthis || Hother) + throw std::runtime_error( + "Pose2::localCoordinates derivatives not implemented"); + return localCoordinates(p2); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index a9b07faf1..2efb8d42e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -20,11 +20,9 @@ #pragma once -#include -#include -#include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0bc07b753..e8ad50de0 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -17,7 +17,7 @@ #include #include #include -#include + #include #include #include @@ -26,9 +26,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose3); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 80e1d2d2a..0c0098f0d 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -25,9 +25,9 @@ #define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP #endif -#include #include #include +#include namespace gtsam { @@ -157,13 +157,8 @@ public: /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, - OptionalJacobian<6,6> Horigin, OptionalJacobian<6,6> Hother) const { - CONCEPT_NOT_IMPLEMENTED; - } - /// @} + /// @name Lie Group /// @{ @@ -173,6 +168,14 @@ public: /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation static Vector6 Logmap(const Pose3& p); + /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose + Vector6 localCoordinates(const Pose3& T2, OptionalJacobian<6,6> Horigin, + OptionalJacobian<6,6> Hother = boost::none) const { + if (Horigin || Hother) + throw std::runtime_error("Pose3::localCoordinates derivatives not implemented"); + return localCoordinates(T2); + } + /** * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 31bfd9271..b7a9fcde4 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -15,7 +15,7 @@ * @author Frank Dellaert **/ -#include +#include #include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 13c22ddc1..754da3c4a 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,15 +17,11 @@ */ #include -#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Rot2); - /* ************************************************************************* */ Rot2 Rot2::fromCosSin(double c, double s) { if (fabs(c * c + s * s - 1.0) > 1e-9) { diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 80121bf9e..000585106 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -18,10 +18,9 @@ #pragma once -#include - -#include #include +#include +#include namespace gtsam { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 6bd35f3cd..764eaffa5 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -276,15 +276,7 @@ namespace gtsam { /// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - Vector3 localCoordinates(const Rot3& t2, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> H2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const { - if (Horigin) { - CONCEPT_NOT_IMPLEMENTED; - } - if (H2) { - CONCEPT_NOT_IMPLEMENTED; - } - return localCoordinates(t2,mode); - } + /// @} /// @name Lie Group /// @{ @@ -309,6 +301,14 @@ namespace gtsam { /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector3& v); + Vector3 localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin, + OptionalJacobian<3, 3> H2, Rot3::CoordinatesMode mode = + ROT3_DEFAULT_COORDINATES_MODE) const { + if (Horigin || H2) + throw std::runtime_error("Rot3::localCoordinates derivatives not implemented"); + return localCoordinates(R2, mode); + } + /** * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index cb7236823..a7c6f6bf9 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -120,7 +120,7 @@ namespace gtsam { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; - return between_default(*this, R2); + return inverse() * R2; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index df66ad2f1..f8d8ea0ee 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index e515356f1..2f37fea3f 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -121,7 +121,7 @@ namespace gtsam { /** The difference between another point and this point */ inline StereoPoint2 between(const StereoPoint2& p2) const { - return gtsam::between_default(*this, p2); + return p2 - *this; } /// @} diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8a7a5f5e5..fd151746b 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,7 +20,7 @@ #pragma once -#include +#include #include #include #include @@ -165,12 +165,5 @@ private: // Define GTSAM traits template <> struct traits_x : public internal::Manifold {}; -//template<> -//struct GTSAM_EXPORT zero { -// static Unit3 value() { -// return Unit3(); -// } -//}; - } // namespace gtsam diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 03e3654e4..b104e5ae2 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -16,6 +16,7 @@ **/ #include +#include #include using namespace std; @@ -23,7 +24,6 @@ using namespace gtsam; typedef OptionalJacobian<3,3> SO3Jacobian; -#if 0 //****************************************************************************** TEST(SO3 , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -64,21 +64,54 @@ TEST(SO3 , Retract) { EXPECT(actual.isApprox(expected)); } -#endif - //****************************************************************************** TEST(SO3 , Compose) { - EXPECT(false); + Vector3 z_axis(0, 0, 1); + SO3 q1(Eigen::AngleAxisd(0.2, z_axis)); + SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); + + SO3 expected = q1 * q2; + Matrix actualH1, actualH2; + SO3 actual = traits_x::Compose(q1, q2, actualH1, actualH2); + EXPECT(traits_x::Equals(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(traits_x::Compose, q1, q2); + EXPECT(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(traits_x::Compose, q1, q2); + EXPECT(assert_equal(numericalH2,actualH2)); } //****************************************************************************** TEST(SO3 , Between) { - EXPECT(false); + Vector3 z_axis(0, 0, 1); + SO3 q1(Eigen::AngleAxisd(0.2, z_axis)); + SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); + + SO3 expected = q1.inverse() * q2; + Matrix actualH1, actualH2; + SO3 actual = traits_x::Between(q1, q2, actualH1, actualH2); + EXPECT(traits_x::Equals(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(traits_x::Between, q1, q2); + EXPECT(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(traits_x::Between, q1, q2); + EXPECT(assert_equal(numericalH2,actualH2)); } //****************************************************************************** TEST(SO3 , Inverse) { - EXPECT(false); + Vector3 z_axis(0, 0, 1); + SO3 q1(Eigen::AngleAxisd(0.1, z_axis)); + SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); + + Matrix actualH; + SO3 actual = traits_x::Inverse(q1, actualH); + EXPECT(traits_x::Equals(expected,actual)); + + Matrix numericalH = numericalDerivative11(traits_x::Inverse, q1); + EXPECT(assert_equal(numericalH,actualH)); } //****************************************************************************** diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 5332333f9..19b28cccd 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,8 +20,9 @@ #pragma once -#include #include +#include +#include namespace gtsam { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 9f94d9aa7..9870eae2e 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,12 +18,14 @@ #pragma once +#include +#include + #include #include #include #include #include -#include #include namespace gtsam { diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 2b014e0b8..a4008ad98 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -21,10 +21,7 @@ #include #include -#include -#include -#include -#include +#include #include #include diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index ac0120032..d3ba93d40 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -25,6 +25,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 97fa36d61..8131414d8 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -5,7 +5,7 @@ #include #include -#include + #include #include diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index 06ab633c6..92aece7e5 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -32,7 +32,7 @@ using namespace gtsam; /* ************************************************************************* */ Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) { - return between_default(r1, r2); + return r1.inverse() * r2; } /* ************************************************************************* */ diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 6860914ef..4ad9d7d47 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -32,7 +32,7 @@ using namespace gtsam; /* ************************************************************************* */ Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) { - return between_default(r1, r2); + return r1.inverse() * r2; } /* ************************************************************************* */