12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Split up concepts into Group/Manifold/Lie/VectorSpace. Also fixed some implementations and put in exceptions elsewhere, rather than CONCEPT_NOT_IMPLEMENTED macro.

release/4.3a0
dellaert 2014-12-22 00:12:08 +01:00
parent bc6af85a9c
commit 99a50a2f87
36 changed files with 727 additions and 1084 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
gtsam/base/Group.h
View File

@ -19,43 +19,93 @@
#pragma once #pragma once
#include <gtsam/global_includes.h> #include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp>
namespace gtsam { 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 <typename T> struct traits_x;
/** /**
* This concept check enforces a Group structure on a variable type, * Group Concept
* in which we require the existence of basic algebraic operations.
*/ */
template<class T> template<typename G>
class GroupConcept { class IsGroup {
private: public:
static T concept_check(const T& t) { typedef typename traits_x<G>::structure_category structure_category_tag;
/** assignment */ typedef typename traits_x<G>::group_flavor flavor_tag;
T t2 = t; //typedef typename traits_x<G>::identity::value_type identity_value_type;
/** identity */ BOOST_CONCEPT_USAGE(IsGroup) {
T identity = T::identity(); BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<group_tag, structure_category_tag>::value),
/** compose with another object */ "This type's structure_category trait does not assert it as a group (or derived)");
T compose_ret = identity.compose(t2); e = traits_x<G>::Identity();
e = traits_x<G>::Compose(g, h);
/** invert the object and yield a new one */ e = traits_x<G>::Between(g, h);
T inverse_ret = compose_ret.inverse(); e = traits_x<G>::Inverse(g);
operator_usage(flavor);
return inverse_ret; // 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<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits_x<G>::Identity();
return traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::Inverse(a)), e, tol)
&& traits_x<G>::Equals(traits_x<G>::Between(a, b), traits_x<G>::Compose(traits_x<G>::Inverse(a), b), tol)
&& traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::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 * - An instantiation for use inside unit tests
* - A typedef for use inside generic algorithms * - A typedef for use inside generic algorithms
* *
* NOTE: intentionally not in the gtsam namespace to allow for classes not in * NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable * the gtsam namespace to be more easily enforced as testable
*/ */
#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept<T>; #define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup<T>;
#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; #define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup<T> _gtsam_IsGroup_##T;

28
gtsam/base/Lie-inl.h
View File

@ -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 <gtsam/base/Lie.h>
#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&);

198
gtsam/base/Lie.h
View File

@ -24,6 +24,105 @@
namespace gtsam { 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<Type> : public LieGroup<Type> { };
template<typename T>
struct LieGroup : Testable<T> {
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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> 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 * These core global functions can be specialized by new Lie types
* for better performance. * for better performance.
@ -31,75 +130,53 @@ namespace gtsam {
/** Compute l0 s.t. l2=l1*l0 */ /** Compute l0 s.t. l2=l1*l0 */
template<class T> template<class T>
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 */ /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
template<class T> template<class T>
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) */ /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
template<class T> template<class T>
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 * Lie Group Concept
*
* 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.
*/ */
template <class T> template<typename LG>
class LieConcept { class IsLieGroup: public IsGroup<LG>, public IsManifold<LG> {
private: public:
/** concept checking function - implement the functions this demands */ typedef typename traits_x<LG>::structure_category structure_category_tag;
static T concept_check(const T& t) { typedef typename traits_x<LG>::ManifoldType ManifoldType;
typedef typename traits_x<LG>::TangentVector TangentVector;
typedef typename traits_x<LG>::ChartJacobian ChartJacobian;
/** assignment */ BOOST_CONCEPT_USAGE(IsLieGroup) {
T t2 = t; BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)");
/** // group opertations with Jacobians
* Returns dimensionality of the tangent space g = traits_x<LG>::Compose(g, h, Hg, Hh);
*/ g = traits_x<LG>::Between(g, h, Hg, Hh);
size_t dim_ret = t.dim(); g = traits_x<LG>::Inverse(g, Hg);
// log and exp map without Jacobians
/** expmap around identity */ g = traits_x<LG>::Expmap(v);
T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); v = traits_x<LG>::Logmap(g);
// log and exp map with Jacobians
/** Logmap around identity */ //g = traits_x<LG>::Expmap(v, Hg);
Vector logmap_identity_ret = T::Logmap(t); //v = traits_x<LG>::Logmap(g, Hg);
/** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
T between_ret = expmap_identity_ret.between(t2);
return between_ret;
} }
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 * NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable * the gtsam namespace to be more easily enforced as testable
*/ */
#define GTSAM_CONCEPT_LIE_INST(T) \ #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
template class gtsam::IsLieGroup<T>; #define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T;
#define GTSAM_CONCEPT_LIE_TYPE(T) \
typedef gtsam::IsLieGroup<T> _gtsam_LieConcept_##T;

14
gtsam/base/LieMatrix.h
View File

@ -102,8 +102,10 @@ struct LieMatrix : public Matrix {
Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >( Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&v(0), this->rows(), this->cols())); &v(0), this->rows(), this->cols()));
} }
inline LieMatrix retract(const Vector& v, OptionalJacobian<-1,-1> Horigin, OptionalJacobian<-1,-1> Hv) const { inline LieMatrix retract(const Vector& v, OptionalJacobian<-1, -1> Horigin,
CONCEPT_NOT_IMPLEMENTED; OptionalJacobian<-1, -1> Hv) const {
if (Horigin || Hv)
throw std::runtime_error("LieMatrix::retract derivative not implemented");
return retract(v); return retract(v);
} }
/** @return the local coordinates of another object. The elements of the /** @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; &result(0), this->rows(), this->cols()) = t2 - *this;
return result; return result;
} }
Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1,-1> Horigin, OptionalJacobian<-1,-1> Hother) const { Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1, -1> Horigin,
CONCEPT_NOT_IMPLEMENTED; OptionalJacobian<-1, -1> Hother) const {
if (Horigin || Hother)
throw std::runtime_error("LieMatrix::localCoordinates derivative not implemented");
return localCoordinates(ts); return localCoordinates(ts);
} }
@ -171,7 +175,7 @@ struct LieMatrix : public Matrix {
/** Logmap around identity */ /** Logmap around identity */
static inline Vector Logmap(const LieMatrix& p, OptionalJacobian<-1,-1> H = boost::none) { 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()); Vector result(p.size());
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >( Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
result.data(), p.rows(), p.cols()) = p; result.data(), p.rows(), p.cols()) = p;

3
gtsam/base/LieScalar.h
View File

@ -24,8 +24,7 @@
#endif #endif
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Lie.h>
namespace gtsam { namespace gtsam {

12
gtsam/base/LieVector.h
View File

@ -125,14 +125,18 @@ struct LieVector : public Vector {
// Lie functions // Lie functions
/** Expmap around identity */ /** Expmap around identity */
static LieVector Expmap(const Vector& v, OptionalJacobian<-1,-1> H = boost::none) { static LieVector Expmap(const Vector& v, OptionalJacobian<-1, -1> H =
if (H) { CONCEPT_NOT_IMPLEMENTED; } boost::none) {
if (H)
throw std::runtime_error("LieVector::Expmap derivative not implemented");
return LieVector(v); return LieVector(v);
} }
/** Logmap around identity - just returns with default cast back */ /** Logmap around identity - just returns with default cast back */
static Vector Logmap(const LieVector& p, OptionalJacobian<-1,-1> H = boost::none) { static Vector Logmap(const LieVector& p, OptionalJacobian<-1, -1> H =
if (H) { CONCEPT_NOT_IMPLEMENTED; } boost::none) {
if (H)
throw std::runtime_error("LieVector::Logmap derivative not implemented");
return p; return p;
} }

436
gtsam/base/Manifold.h
View File

@ -14,18 +14,24 @@
* @brief Base class and basic functions for Manifold types * @brief Base class and basic functions for Manifold types
* @author Alex Cunningham * @author Alex Cunningham
* @author Frank Dellaert * @author Frank Dellaert
* @author Mike Bosse
*/ */
#pragma once #pragma once
#include <gtsam/base/concepts.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <boost/static_assert.hpp> #include <gtsam/base/Testable.h>
#include <boost/type_traits.hpp> #include <gtsam/base/OptionalJacobian.h>
#include <string>
// #include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
namespace gtsam { 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 * 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 * that can be centered around a given point on the manifold. These nonlinear
@ -44,322 +50,112 @@ namespace gtsam {
* *
*/ */
template <typename T> 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<Type> : public Manifold<Type> { };
template<typename _ManifoldType>
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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> 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<typename T>
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typename traits_x<T>::TangentVector v0 = traits_x<T>::Local(a,a);
typename traits_x<T>::TangentVector v = traits_x<T>::Local(a,b);
T c = traits_x<T>::Retract(a,v);
return v0.norm() < tol && traits_x<T>::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<int, DIM> {};\
typedef TANGENT_VECTOR TangentVector;\
typedef OptionalJacobian<dimension, dimension> 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<typename M>
class IsManifold {
public:
typedef typename traits_x<M>::structure_category structure_category_tag;
static const size_t dim = traits_x<M>::dimension;
typedef typename traits_x<M>::ManifoldType ManifoldType;
typedef typename traits_x<M>::TangentVector TangentVector;
typedef typename traits_x<M>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, structure_category_tag>::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<M>::Local(p,q);
q = traits_x<M>::Retract(p,v);
// and the versions with Jacobians.
//v = traits_x<M>::Local(p,q,Hp,Hq);
//q = traits_x<M>::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<typename T>
//struct is_group: public boost::false_type {
//};
//
//// identity, no default provided, by default given by default constructor
//template<typename T>
//struct identity {
// static T value() {
// return T();
// }
//};
//
//// is manifold, by default this is false
//template<typename T>
//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<int, Eigen::Dynamic> Dynamic;
////template<typename T>
////struct dimension: public Dynamic {
////struct dimension : public boost::integral_constant<int, 1> { // just temporary fix to minimize compiler errors while refactoring
////};
//
///**
// * zero<T>::value is intended to be the origin of a canonical coordinate system
// * with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
// * Below we provide the group identity as zero *in case* it is a group
// */
//template<typename T> struct zero: public identity<T> {
// BOOST_STATIC_ASSERT(is_group<T>::value);
//};
//
//// double
//
//template<>
//struct is_group<double> : public boost::true_type {
//};
//
//template<>
//struct is_manifold<double> : public boost::true_type {
//};
//
//template<>
//struct dimension<double> : public boost::integral_constant<int, 1> {
//};
//
//template<>
//struct zero<double> {
// static double value() {
// return 0;
// }
//};
//
//// Fixed size Eigen::Matrix type
//
//template<int M, int N, int Options, int MaxRows, int MaxCols>
//struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
//};
//
//template<int M, int N, int Options, int MaxRows, int MaxCols>
//struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
//};
//
//template<int M, int N, int Options, int MaxRows, int MaxCols>
//struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : 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<int M, int N, int Options, int MaxRows, int MaxCols>
//struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
// "traits::zero is only supported for fixed-size matrices");
// static Eigen::Matrix<double, M, N, Options> value() {
// return Eigen::Matrix<double, M, N, Options>::Zero();
// }
//};
//
//template<int M, int N, int Options>
//struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
// Eigen::Matrix<double, M, N, Options> > {
//};
//
//template<typename T> struct is_chart: public boost::false_type {
//};
//
//} // \ namespace traits
//
//// Chart is a map from T -> vector, retract is its inverse
//template<typename T>
//struct DefaultChart {
// //BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
// typedef T type;
// typedef Eigen::Matrix<double, traits_x<T>::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<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
//};
//template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
//};
//}
//
//template<class C>
//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<C>::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<Chart<T> > is a chart around zero<T>::value
// * Canonical<T> is CanonicalChart<DefaultChart<T> >
// * An example is Canonical<Rot3>
// */
//template<typename C> struct CanonicalChart {
// BOOST_CONCEPT_ASSERT((ChartConcept<C>));
//
// 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<type>::value(), t);
// }
// // Convert back from canonical coordinates to T
// type retract(const vector& v) {
// return Chart::retract(traits::zero<type>::value(), v);
// }
//};
//template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
//};
//
//// double
//
//template<>
//struct DefaultChart<double> {
// typedef double type;
// typedef Eigen::Matrix<double, 1, 1> 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<int M, int N, int Options, int MaxRows, int MaxCols>
//struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// /**
// * 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<double, M, N, Options, MaxRows, MaxCols> type;
// typedef type T;
// typedef Eigen::Matrix<double, traits::dimension<T>::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<vector::RowsAtCompileTime, 1, vector::Options>(other)
// - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
// }
// static T retract(const T& origin, const vector& d) {
// return origin + reshape<M, N, Options>(d);
// }
// static int getDimension(const T&origin) {
// return origin.rows() * origin.cols();
// }
//};
//
//// Dynamically sized Vector
//template<>
//struct DefaultChart<Vector> {
// 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<Matrix> {
// typedef Matrix type;
// typedef Vector vector;
// static vector local(const Matrix& origin, const Matrix& other) {
// Matrix d = other - origin;
// return Eigen::Map<Vector>(d.data(),getDimension(d));
// }
// static Matrix retract(const Matrix& origin, const vector& d) {
// return origin + Eigen::Map<const Matrix>(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 T>
//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 } // \ namespace gtsam
//
// TODO(ASL) Remove these and fix the compiler errors.
///** ///**
// * Macros for using the ManifoldConcept // * Macros for using the ManifoldConcept
// * - An instantiation for use inside unit tests // * - 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 // * NOTE: intentionally not in the gtsam namespace to allow for classes not in
// * the gtsam namespace to be more easily enforced as testable // * the gtsam namespace to be more easily enforced as testable
// */ // */
#define GTSAM_CONCEPT_MANIFOLD_INST(T) #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T;

1
gtsam/base/Matrix.h
View File

@ -22,7 +22,6 @@
#pragma once #pragma once
#include <gtsam/base/concepts.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>

237
gtsam/base/VectorSpace.h Normal file
View File

@ -0,0 +1,237 @@
/*
* VectorSpace.h
*
* @date December 21, 2014
* @author Mike Bosse
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/base/Lie.h>
namespace gtsam {
/// tag to assert a type is a vector space
struct vector_space_tag: public lie_group_tag {};
template <typename T> 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<Type> : public ScalarTraits<Type> { };
template<typename Scalar>
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<double, 1, 1> 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<double> : public internal::ScalarTraits<double> {};
/// float
template<> struct traits_x<float> : public internal::ScalarTraits<float> {};
// traits for any double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// 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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> ManifoldType;
static int GetDimension(const ManifoldType& m){ return m.rows()*m.cols(); }
static Eigen::Matrix<double, dimension, dimension> Eye(const ManifoldType& m) {
int dim = GetDimension(m);
return Eigen::Matrix<double, dimension, dimension>::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<Eigen::Matrix<double, M, N> >(
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<const Eigen::Matrix<double, M, N> >(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<Eigen::Matrix<double, M, N> >(
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<Eigen::Matrix<double, M, N> >(v.data());
}
};
/// Vector Space concept
template<typename V>
class IsVectorSpace: public IsLieGroup<V> {
public:
typedef typename traits_x<V>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<vector_space_tag, structure_category_tag>::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

526
gtsam/base/concepts.h
View File

@ -1,16 +1,18 @@
/* /*
* concepts.h * concepts.h
* *
* @data Dec 4, 2014 * @date Dec 4, 2014
* @author Mike Bosse * @author Mike Bosse
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#pragma once #pragma once
#include <gtsam/base/Group.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <boost/concept_check.hpp> #include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp> #include <boost/concept/requires.hpp>
#include <boost/static_assert.hpp> #include <boost/static_assert.hpp>
@ -31,357 +33,8 @@
namespace gtsam { namespace gtsam {
/**
* @name Algebraic Structure Tags
* @brief Possible values for traits::structure_category<T>::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 <typename T> struct traits_x; template <typename T> 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<Type> : public Manifold<Type> { };
template<typename _ManifoldType>
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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> 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<Type> : public LieGroup<Type> { };
template<typename _ManifoldType,typename _group_flavor = additive_group_tag>
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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> 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<Type> : public ScalarTraits<Type> { };
template<typename Scalar>
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<double, 1, 1> 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<double> : public internal::ScalarTraits<double> {};
template<>
struct traits_x<float> : public internal::ScalarTraits<float> {};
// traits for any double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// 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<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> ManifoldType;
static int GetDimension(const ManifoldType& m){ return m.rows()*m.cols(); }
static Eigen::Matrix<double, dimension, dimension> Eye(const ManifoldType& m) {
int dim = GetDimension(m);
return Eigen::Matrix<double, dimension, dimension>::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<Eigen::Matrix<double, M, N> >(
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<const Eigen::Matrix<double, M, N> >(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<Eigen::Matrix<double, M, N> >(
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<Eigen::Matrix<double, M, N> >(v.data());
}
};
template<typename ManifoldType> template<typename ManifoldType>
struct Canonical { struct Canonical {
BOOST_STATIC_ASSERT_MSG( BOOST_STATIC_ASSERT_MSG(
@ -413,178 +66,5 @@ struct Canonical {
} }
}; };
/// Check invariants for Manifold type
template<typename T>
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typename traits_x<T>::TangentVector v0 = traits_x<T>::Local(a,a);
typename traits_x<T>::TangentVector v = traits_x<T>::Local(a,b);
T c = traits_x<T>::Retract(a,v);
return v0.norm() < tol && traits_x<T>::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<int, DIM> {};\
typedef TANGENT_VECTOR TangentVector;\
typedef OptionalJacobian<dimension, dimension> 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<typename M>
class IsManifold {
public:
typedef typename traits_x<M>::structure_category structure_category_tag;
static const size_t dim = traits_x<M>::dimension;
typedef typename traits_x<M>::ManifoldType ManifoldType;
typedef typename traits_x<M>::TangentVector TangentVector;
typedef typename traits_x<M>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, structure_category_tag>::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<M>::Local(p,q);
q = traits_x<M>::Retract(p,v);
// and the versions with Jacobians.
//v = traits_x<M>::Local(p,q,Hp,Hq);
//q = traits_x<M>::Retract(p,v,Hp,Hv);
}
private:
ManifoldType p,q;
ChartJacobian Hp,Hq,Hv;
TangentVector v;
bool b;
};
/**
* Group Concept
*/
template<typename G>
class IsGroup {
public:
typedef typename traits_x<G>::structure_category structure_category_tag;
typedef typename traits_x<G>::group_flavor flavor_tag;
//typedef typename traits_x<G>::identity::value_type identity_value_type;
BOOST_CONCEPT_USAGE(IsGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<group_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a group (or derived)");
e = traits_x<G>::Identity();
e = traits_x<G>::Compose(g, h);
e = traits_x<G>::Between(g, h);
e = traits_x<G>::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<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits_x<G>::Identity();
return traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::Inverse(a)), e, tol)
&& traits_x<G>::Equals(traits_x<G>::Between(a, b), traits_x<G>::Compose(traits_x<G>::Inverse(a), b), tol)
&& traits_x<G>::Equals(traits_x<G>::Compose(a, traits_x<G>::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<typename LG>
class IsLieGroup: public IsGroup<LG>, public IsManifold<LG> {
public:
typedef typename traits_x<LG>::structure_category structure_category_tag;
typedef typename traits_x<LG>::ManifoldType ManifoldType;
typedef typename traits_x<LG>::TangentVector TangentVector;
typedef typename traits_x<LG>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsLieGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)");
// group opertations with Jacobians
g = traits_x<LG>::Compose(g, h, Hg, Hh);
g = traits_x<LG>::Between(g, h, Hg, Hh);
g = traits_x<LG>::Inverse(g, Hg);
// log and exp map without Jacobians
g = traits_x<LG>::Expmap(v);
v = traits_x<LG>::Logmap(g);
// log and exp map with Jacobians
//g = traits_x<LG>::Expmap(v, Hg);
//v = traits_x<LG>::Logmap(g, Hg);
}
private:
LG g, h;
TangentVector v;
ChartJacobian Hg, Hh;
};
template<typename V>
class IsVectorSpace: public IsLieGroup<V> {
public:
typedef typename traits_x<V>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<vector_space_tag, structure_category_tag>::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 } // namespace gtsam

4
gtsam/base/numericalDerivative.h
View File

@ -28,12 +28,10 @@
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
#endif #endif
#include <gtsam/base/concepts.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Lie.h>
namespace gtsam { namespace gtsam {

21
gtsam/geometry/Cyclic.h
View File

@ -15,8 +15,11 @@
* @author Frank Dellaert * @author Frank Dellaert
**/ **/
#include <gtsam/base/concepts.h> #include <gtsam/base/Group.h>
#include <cstddef> #include <cstddef>
#include <string>
#include <iostream>
#include <assert.h>
namespace gtsam { namespace gtsam {
@ -28,7 +31,7 @@ public:
/// Constructor /// Constructor
Cyclic(size_t i) : Cyclic(size_t i) :
i_(i) { i_(i) {
assert(i<N); assert(i < N);
} }
/// Idenity element /// Idenity element
static Cyclic Identity() { static Cyclic Identity() {
@ -63,13 +66,17 @@ public:
/// Define cyclic group traits to be a model of the Group concept /// Define cyclic group traits to be a model of the Group concept
template<size_t N> template<size_t N>
struct traits_x<Cyclic<N> > { struct traits_x<Cyclic<N> > {
typedef group_tag structure_category; typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic<N>)
GTSAM_ADDITIVE_GROUP(Cyclic<N>) static Cyclic<N> Identity() {
static Cyclic<N> Identity() { return Cyclic<N>::Identity();} return Cyclic<N>::Identity();
static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b, double tol = 1e-9) { }
static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b,
double tol = 1e-9) {
return a.equals(b, tol); return a.equals(b, tol);
} }
static void Print(const Cyclic<N>& c, const std::string &s = "") {c.print(s);} static void Print(const Cyclic<N>& c, const std::string &s = "") {
c.print(s);
}
}; };
} // \namespace gtsam } // \namespace gtsam

4
gtsam/geometry/Point2.cpp
View File

@ -16,7 +16,6 @@
*/ */
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <cmath> #include <cmath>
@ -24,9 +23,6 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2);
/* ************************************************************************* */ /* ************************************************************************* */
void Point2::print(const string& s) const { void Point2::print(const string& s) const {
cout << s << *this << endl; cout << s << *this << endl;

46
gtsam/geometry/Point2.h
View File

@ -17,12 +17,9 @@
#pragma once #pragma once
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Lie.h>
namespace gtsam { namespace gtsam {
/** /**
@ -158,17 +155,14 @@ public:
inline size_t dim() const { return 2; } inline size_t dim() const { return 2; }
/// Updates a with tangent space delta /// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); } inline Point2 retract(const Vector& v) const {
inline Point2 retract(const Vector& v, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const {
CONCEPT_NOT_IMPLEMENTED;
return *this + Point2(v); return *this + Point2(v);
} }
/// Local coordinates of manifold neighborhood around current value /// 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) const {
inline Vector localCoordinates(const Point2& t2, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const { Point2 dp = t2 - *this;
CONCEPT_NOT_IMPLEMENTED; return Vector2(dp.x(), dp.y());
return Logmap(between(t2));
} }
/// @} /// @}
@ -176,20 +170,17 @@ public:
/// @{ /// @{
/// Exponential map around identity - just create a Point2 from a vector /// 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) {
static Point2 Expmap(const Vector2& v, OptionalJacobian<2,2> H) { if (H) *H = I_2x2;
CONCEPT_NOT_IMPLEMENTED; return Point2(v);
} }
/// Logmap around identity /// 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()); 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 /// Left-trivialized derivative of the exponential map
static Matrix2 dexpL(const Vector2& v) { static Matrix2 dexpL(const Vector2& v) {
return I_2x2; return I_2x2;
@ -200,6 +191,23 @@ public:
return I_2x2; 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 /// @name Vector Space
/// @{ /// @{

4
gtsam/geometry/Point3.cpp
View File

@ -15,15 +15,11 @@
*/ */
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/Lie-inl.h>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point3);
/* ************************************************************************* */ /* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const { bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol

29
gtsam/geometry/Point3.h
View File

@ -21,12 +21,8 @@
#pragma once #pragma once
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <cmath> #include <cmath>
namespace gtsam { namespace gtsam {
@ -131,18 +127,10 @@ namespace gtsam {
/// Updates a with tangent space delta /// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return Point3(*this + v); } 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 /// Returns inverse retraction
inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } 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 /// @name Lie Group
/// @{ /// @{
@ -159,6 +147,21 @@ namespace gtsam {
return Vector3(dp.x(), dp.y(), dp.z()); 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 /// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector& v) { static Matrix3 dexpL(const Vector& v) {
return I_3x3; return I_3x3;

12
gtsam/geometry/Pose2.cpp
View File

@ -16,7 +16,6 @@
#include <gtsam/geometry/concepts.h> #include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <cmath> #include <cmath>
@ -27,9 +26,6 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2); 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 /// 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 { Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis,
CONCEPT_NOT_IMPLEMENTED; OptionalJacobian<3, 3> Hother) const {
if (Hthis || Hother)
throw std::runtime_error(
"Pose2::localCoordinates derivatives not implemented");
return localCoordinates(p2);
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/geometry/Pose2.h
View File

@ -20,11 +20,9 @@
#pragma once #pragma once
#include <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
namespace gtsam { namespace gtsam {

5
gtsam/geometry/Pose3.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h> #include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <iostream> #include <iostream>
#include <cmath> #include <cmath>
@ -26,9 +26,6 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3);

17
gtsam/geometry/Pose3.h
View File

@ -25,9 +25,9 @@
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP #define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP
#endif #endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h>
namespace gtsam { 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 /// 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; 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 /// @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 /// 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); 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 * 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) * 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)

2
gtsam/geometry/Quaternion.h
View File

@ -15,7 +15,7 @@
* @author Frank Dellaert * @author Frank Dellaert
**/ **/
#include <gtsam/base/concepts.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>

4
gtsam/geometry/Rot2.cpp
View File

@ -17,15 +17,11 @@
*/ */
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie-inl.h>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot2);
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 Rot2::fromCosSin(double c, double s) { Rot2 Rot2::fromCosSin(double c, double s) {
if (fabs(c * c + s * s - 1.0) > 1e-9) { if (fabs(c * c + s * s - 1.0) > 1e-9) {

5
gtsam/geometry/Rot2.h
View File

@ -18,10 +18,9 @@
#pragma once #pragma once
#include <boost/optional.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie.h>
#include <boost/optional.hpp>
namespace gtsam { namespace gtsam {

18
gtsam/geometry/Rot3.h
View File

@ -276,15 +276,7 @@ namespace gtsam {
/// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation /// 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, 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 /// @name Lie Group
/// @{ /// @{
@ -309,6 +301,14 @@ namespace gtsam {
/// Left-trivialized derivative inverse of the exponential map /// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v); 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 * 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. * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.

2
gtsam/geometry/Rot3Q.cpp
View File

@ -120,7 +120,7 @@ namespace gtsam {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix()); if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3; if (H2) *H2 = I3;
return between_default(*this, R2); return inverse() * R2;
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/geometry/SO3.h
View File

@ -19,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/Lie.h>
#include <cmath> #include <cmath>

2
gtsam/geometry/StereoPoint2.h
View File

@ -121,7 +121,7 @@ namespace gtsam {
/** The difference between another point and this point */ /** The difference between another point and this point */
inline StereoPoint2 between(const StereoPoint2& p2) const { inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2); return p2 - *this;
} }
/// @} /// @}

9
gtsam/geometry/Unit3.h
View File

@ -20,7 +20,7 @@
#pragma once #pragma once
#include <gtsam/base/concepts.h> #include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp> #include <boost/random/mersenne_twister.hpp>
@ -165,12 +165,5 @@ private:
// Define GTSAM traits // Define GTSAM traits
template <> struct traits_x<Unit3> : public internal::Manifold<Unit3> {}; template <> struct traits_x<Unit3> : public internal::Manifold<Unit3> {};
//template<>
//struct GTSAM_EXPORT zero<Unit3> {
// static Unit3 value() {
// return Unit3();
// }
//};
} // namespace gtsam } // namespace gtsam

45
gtsam/geometry/tests/testSO3.cpp
View File

@ -16,6 +16,7 @@
**/ **/
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SO3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
@ -23,7 +24,6 @@ using namespace gtsam;
typedef OptionalJacobian<3,3> SO3Jacobian; typedef OptionalJacobian<3,3> SO3Jacobian;
#if 0
//****************************************************************************** //******************************************************************************
TEST(SO3 , Concept) { TEST(SO3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >)); BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
@ -64,21 +64,54 @@ TEST(SO3 , Retract) {
EXPECT(actual.isApprox(expected)); EXPECT(actual.isApprox(expected));
} }
#endif
//****************************************************************************** //******************************************************************************
TEST(SO3 , Compose) { 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<SO3>::Compose(q1, q2, actualH1, actualH2);
EXPECT(traits_x<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits_x<SO3>::Compose, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits_x<SO3>::Compose, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Between) { 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<SO3>::Between(q1, q2, actualH1, actualH2);
EXPECT(traits_x<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits_x<SO3>::Between, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits_x<SO3>::Between, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Inverse) { 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<SO3>::Inverse(q1, actualH);
EXPECT(traits_x<SO3>::Equals(expected,actual));
Matrix numericalH = numericalDerivative11(traits_x<SO3>::Inverse, q1);
EXPECT(assert_equal(numericalH,actualH));
} }
//****************************************************************************** //******************************************************************************

3
gtsam/linear/GaussianFactor.h
View File

@ -20,8 +20,9 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/Factor.h> #include <gtsam/inference/Factor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
namespace gtsam { namespace gtsam {

4
gtsam/linear/NoiseModel.h
View File

@ -18,12 +18,14 @@
#pragma once #pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/serialization/extended_type_info.hpp> #include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp> #include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp> #include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp> #include <boost/serialization/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <cmath> #include <cmath>
namespace gtsam { namespace gtsam {

5
gtsam/nonlinear/Expression-inl.h
View File

@ -21,10 +21,7 @@
#include <gtsam_unstable/nonlinear/CallRecord.h> #include <gtsam_unstable/nonlinear/CallRecord.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>

1
gtsam/nonlinear/Values.h
View File

@ -25,6 +25,7 @@
#pragma once #pragma once
#include <gtsam/base/ChartValue.h> #include <gtsam/base/ChartValue.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -5,7 +5,7 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>

2
timing/timePose2.cpp
View File

@ -32,7 +32,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) { Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) {
return between_default(r1, r2); return r1.inverse() * r2;
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
timing/timeRot2.cpp
View File

@ -32,7 +32,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) { Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) {
return between_default(r1, r2); return r1.inverse() * r2;
} }
/* ************************************************************************* */ /* ************************************************************************* */