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

Started converting types. Into the ChartValue now.

release/4.3a0
Paul Furgale 2014-12-13 22:20:59 +01:00
parent 959716ae92
commit 58cb47a3f0
16 changed files with 406 additions and 541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

684
gtsam/base/Manifold.h
View File

@ -23,347 +23,347 @@
#include <boost/static_assert.hpp> #include <boost/static_assert.hpp>
#include <boost/type_traits.hpp> #include <boost/type_traits.hpp>
#include <string> #include <string>
//
namespace gtsam { //namespace gtsam {
//
/** ///**
* 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
* spaces may have such properties as wrapping around (as is the case with rotations), // * spaces may have such properties as wrapping around (as is the case with rotations),
* which might make linear operations on parameters not return a viable element of // * which might make linear operations on parameters not return a viable element of
* the manifold. // * the manifold.
* // *
* We perform optimization by computing a linear delta in the tangent space of the // * We perform optimization by computing a linear delta in the tangent space of the
* current estimate, and then apply this change using a retraction operation, which // * current estimate, and then apply this change using a retraction operation, which
* maps the change in tangent space back to the manifold itself. // * maps the change in tangent space back to the manifold itself.
* // *
* There may be multiple possible retractions for a given manifold, which can be chosen // * There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for // * between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be // * the creation for the retract and localCoordinates functions is that they be
* inverse operations. The new notion of a Chart guarantees that. // * inverse operations. The new notion of a Chart guarantees that.
* // *
*/ // */
//
// Traits, same style as Boost.TypeTraits //// Traits, same style as Boost.TypeTraits
// All meta-functions below ever only declare a single type //// All meta-functions below ever only declare a single type
// or a type/value/value_type //// or a type/value/value_type
namespace traits { //namespace traits {
//
// is group, by default this is false //// 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> //template<typename T>
//struct dimension: public Dynamic { //struct is_group: public boost::false_type {
//struct dimension : public boost::integral_constant<int, 1> { // just temporary fix to minimize compiler errors while refactoring
//}; //};
//
/** //// identity, no default provided, by default given by default constructor
* zero<T>::value is intended to be the origin of a canonical coordinate system //template<typename T>
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t) //struct identity {
* Below we provide the group identity as zero *in case* it is a group // static T value() {
*/ // return T();
template<typename T> struct zero: public identity<T> { // }
BOOST_STATIC_ASSERT(is_group<T>::value); //};
}; //
//// is manifold, by default this is false
// double //template<typename T>
//struct is_manifold: public boost::false_type {
template<> //};
struct is_group<double> : public boost::true_type { //
}; //// dimension, can return Eigen::Dynamic (-1) if not known at compile time
//// defaults to dynamic, TODO makes sense ?
template<> //typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
struct is_manifold<double> : public boost::true_type { ////template<typename T>
}; ////struct dimension: public Dynamic {
////struct dimension : public boost::integral_constant<int, 1> { // just temporary fix to minimize compiler errors while refactoring
template<> ////};
struct dimension<double> : public boost::integral_constant<int, 1> { //
}; ///**
// * zero<T>::value is intended to be the origin of a canonical coordinate system
template<> // * with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
struct zero<double> { // * Below we provide the group identity as zero *in case* it is a group
static double value() { // */
return 0; //template<typename T> struct zero: public identity<T> {
} // BOOST_STATIC_ASSERT(is_group<T>::value);
}; //};
//
// Fixed size Eigen::Matrix type //// double
//
template<int M, int N, int Options, int MaxRows, int MaxCols> //template<>
struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type { //struct is_group<double> : public boost::true_type {
}; //};
//
template<int M, int N, int Options, int MaxRows, int MaxCols> //template<>
struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type { //struct is_manifold<double> : public boost::true_type {
}; //};
//
template<int M, int N, int Options, int MaxRows, int MaxCols> //template<>
struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant< //struct dimension<double> : public boost::integral_constant<int, 1> {
int, //};
M == Eigen::Dynamic ? Eigen::Dynamic : //
(N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { //template<>
//TODO after switch to c++11 : the above should should be extracted to a constexpr function //struct zero<double> {
// for readability and to reduce code duplication // static double value() {
}; // return 0;
// }
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), //// Fixed size Eigen::Matrix type
"traits::zero is only supported for fixed-size matrices"); //
static Eigen::Matrix<double, M, N, Options> value() { //template<int M, int N, int Options, int MaxRows, int MaxCols>
return Eigen::Matrix<double, M, N, Options>::Zero(); //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>
template<int M, int N, int Options> //struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
struct identity<Eigen::Matrix<double, M, N, Options> > : public zero< //};
Eigen::Matrix<double, M, N, Options> > { //
}; //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<
template<typename T> struct is_chart: public boost::false_type { // int,
}; // M == Eigen::Dynamic ? Eigen::Dynamic :
// (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
} // \ namespace traits // //TODO after switch to c++11 : the above should should be extracted to a constexpr function
// // for readability and to reduce code duplication
// Chart is a map from T -> vector, retract is its inverse //};
template<typename T> //
struct DefaultChart { //template<int M, int N, int Options, int MaxRows, int MaxCols>
//BOOST_STATIC_ASSERT(traits::is_manifold<T>::value); //struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
typedef T type; // BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
typedef Eigen::Matrix<double, traits_x<T>::dimension, 1> vector; // "traits::zero is only supported for fixed-size matrices");
// static Eigen::Matrix<double, M, N, Options> value() {
static vector local(const T& origin, const T& other) { // return Eigen::Matrix<double, M, N, Options>::Zero();
return origin.localCoordinates(other); // }
} //};
static T retract(const T& origin, const vector& d) { //
return origin.retract(d); //template<int M, int N, int Options>
} //struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
static int getDimension(const T& origin) { // Eigen::Matrix<double, M, N, Options> > {
return origin.dim(); //};
} //
}; //template<typename T> struct is_chart: public boost::false_type {
//};
namespace traits { //
// populate default traits //} // \ namespace traits
template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type { //
}; //// Chart is a map from T -> vector, retract is its inverse
template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> { //template<typename T>
}; //struct DefaultChart {
} // //BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
// typedef T type;
template<class C> // typedef Eigen::Matrix<double, traits_x<T>::dimension, 1> vector;
struct ChartConcept { //
public: // static vector local(const T& origin, const T& other) {
typedef typename C::type type; // return origin.localCoordinates(other);
typedef typename C::vector vector; // }
// static T retract(const T& origin, const vector& d) {
BOOST_CONCEPT_USAGE(ChartConcept) { // return origin.retract(d);
// is_chart trait should be true // }
BOOST_STATIC_ASSERT((traits::is_chart<C>::value)); // static int getDimension(const T& origin) {
// return origin.dim();
/** // }
* Returns Retraction update of val_ //};
*/ //
type retract_ret = C::retract(val_, vec_); //namespace traits {
//// populate default traits
/** //template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
* Returns local coordinates of another object //};
*/ //template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
vec_ = C::local(val_, retract_ret); //};
//}
// a way to get the dimension that is compatible with dynamically sized types //
dim_ = C::getDimension(val_); //template<class C>
} //struct ChartConcept {
//public:
private: // typedef typename C::type type;
type val_; // typedef typename C::vector vector;
vector vec_; //
int dim_; // BOOST_CONCEPT_USAGE(ChartConcept) {
}; // // is_chart trait should be true
// BOOST_STATIC_ASSERT((traits::is_chart<C>::value));
/** //
* CanonicalChart<Chart<T> > is a chart around zero<T>::value // /**
* Canonical<T> is CanonicalChart<DefaultChart<T> > // * Returns Retraction update of val_
* An example is Canonical<Rot3> // */
*/ // type retract_ret = C::retract(val_, vec_);
template<typename C> struct CanonicalChart { //
BOOST_CONCEPT_ASSERT((ChartConcept<C>)); // /**
// * Returns local coordinates of another object
typedef C Chart; // */
typedef typename Chart::type type; // vec_ = C::local(val_, retract_ret);
typedef typename Chart::vector vector; //
// // a way to get the dimension that is compatible with dynamically sized types
// Convert t of type T into canonical coordinates // dim_ = C::getDimension(val_);
vector local(const type& t) { // }
return Chart::local(traits::zero<type>::value(), t); //
} //private:
// Convert back from canonical coordinates to T // type val_;
type retract(const vector& v) { // vector vec_;
return Chart::retract(traits::zero<type>::value(), v); // int dim_;
} //};
}; //
template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > { ///**
}; // * CanonicalChart<Chart<T> > is a chart around zero<T>::value
// * Canonical<T> is CanonicalChart<DefaultChart<T> >
// double // * An example is Canonical<Rot3>
// */
template<> //template<typename C> struct CanonicalChart {
struct DefaultChart<double> { // BOOST_CONCEPT_ASSERT((ChartConcept<C>));
typedef double type; //
typedef Eigen::Matrix<double, 1, 1> vector; // typedef C Chart;
// typedef typename Chart::type type;
static vector local(double origin, double other) { // typedef typename Chart::vector vector;
vector d; //
d << other - origin; // // Convert t of type T into canonical coordinates
return d; // vector local(const type& t) {
} // return Chart::local(traits::zero<type>::value(), t);
static double retract(double origin, const vector& d) { // }
return origin + d[0]; // // Convert back from canonical coordinates to T
} // type retract(const vector& v) {
static int getDimension(double) { // return Chart::retract(traits::zero<type>::value(), v);
return 1; // }
} //};
}; //template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
//};
// Fixed size Eigen::Matrix type //
//// double
template<int M, int N, int Options, int MaxRows, int MaxCols> //
struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > { //template<>
/** //struct DefaultChart<double> {
* 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). // typedef double type;
* Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. // typedef Eigen::Matrix<double, 1, 1> vector;
*/ //
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type; // static vector local(double origin, double other) {
typedef type T; // vector d;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector; // d << other - origin;
// return d;
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), // }
"Internal error: DefaultChart for Dynamic should be handled by template below"); // static double retract(double origin, const vector& d) {
// return origin + d[0];
static vector local(const T& origin, const T& other) { // }
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) // static int getDimension(double) {
- reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin); // return 1;
} // }
static T retract(const T& origin, const vector& d) { //};
return origin + reshape<M, N, Options>(d); //
} //// Fixed size Eigen::Matrix type
static int getDimension(const T&origin) { //
return origin.rows() * origin.cols(); //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).
// Dynamically sized Vector // * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
template<> // */
struct DefaultChart<Vector> { // typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
typedef Vector type; // typedef type T;
typedef Vector vector; // typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
static vector local(const Vector& origin, const Vector& other) { //
return other - origin; // BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
} // "Internal error: DefaultChart for Dynamic should be handled by template below");
static Vector retract(const Vector& origin, const vector& d) { //
return origin + d; // static vector local(const T& origin, const T& other) {
} // return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
static int getDimension(const Vector& origin) { // - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
return origin.size(); // }
} // static T retract(const T& origin, const vector& d) {
}; // return origin + reshape<M, N, Options>(d);
// }
// Dynamically sized Matrix // static int getDimension(const T&origin) {
template<> // return origin.rows() * origin.cols();
struct DefaultChart<Matrix> { // }
typedef Matrix type; //};
typedef Vector vector; //
static vector local(const Matrix& origin, const Matrix& other) { //// Dynamically sized Vector
Matrix d = other - origin; //template<>
return Eigen::Map<Vector>(d.data(),getDimension(d)); //struct DefaultChart<Vector> {
} // typedef Vector type;
static Matrix retract(const Matrix& origin, const vector& d) { // typedef Vector vector;
return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols()); // static vector local(const Vector& origin, const Vector& other) {
} // return other - origin;
static int getDimension(const Matrix& m) { // }
return m.size(); // static Vector retract(const Vector& origin, const vector& d) {
} // return origin + d;
}; // }
// static int getDimension(const Vector& origin) {
/** // return origin.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. //
* //// Dynamically sized Matrix
* The necessary functions to implement for Manifold are defined //template<>
* below with additional details as to the interface. The //struct DefaultChart<Matrix> {
* concept checking function in class Manifold will check whether or not // typedef Matrix type;
* the function exists and throw compile-time errors. // typedef Vector vector;
* // static vector local(const Matrix& origin, const Matrix& other) {
* Returns dimensionality of the tangent space, which may be smaller than the number // Matrix d = other - origin;
* of nonlinear parameters. // return Eigen::Map<Vector>(d.data(),getDimension(d));
* size_t dim() const; // }
* // static Matrix retract(const Matrix& origin, const vector& d) {
* Returns a new T that is a result of updating *this with the delta v after pulling // return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
* the updated value back to the manifold T. // }
* T retract(const Vector& v) const; // static int getDimension(const Matrix& m) {
* // return m.size();
* 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. // * Old Concept check class for Manifold types
*/ // * Requires a mapping between a linear tangent space and the underlying
template<class T> // * manifold, of which Lie is a specialization.
class ManifoldConcept { // *
private: // * The necessary functions to implement for Manifold are defined
/** concept checking function - implement the functions this demands */ // * below with additional details as to the interface. The
static T concept_check(const T& t) { // * concept checking function in class Manifold will check whether or not
// * the function exists and throw compile-time errors.
/** assignment */ // *
T t2 = t; // * Returns dimensionality of the tangent space, which may be smaller than the number
// * of nonlinear parameters.
/** // * size_t dim() const;
* Returns dimensionality of the tangent space // *
*/ // * Returns a new T that is a result of updating *this with the delta v after pulling
size_t dim_ret = t.dim(); // * the updated value back to the manifold T.
// * T retract(const Vector& v) const;
/** // *
* Returns Retraction update of T // * Returns the linear coordinates of lp in the tangent space centered around *this.
*/ // * Vector localCoordinates(const T& lp) const;
T retract_ret = t.retract(gtsam::zero(dim_ret)); // *
// * By convention, we use capital letters to designate a static function
/** // * @tparam T is a Lie type, like Point2, Pose3, etc.
* Returns local coordinates of another object // */
*/ //template<class T>
Vector localCoords_ret = t.localCoordinates(t2); //class ManifoldConcept {
//private:
return retract_ret; // /** concept checking function - implement the functions this demands */
} // static T concept_check(const T& t) {
}; //
// /** assignment */
} // \ namespace gtsam // T t2 = t;
//
/** // /**
* Macros for using the ManifoldConcept // * Returns dimensionality of the tangent space
* - An instantiation for use inside unit tests // */
* - A typedef for use inside generic algorithms // size_t dim_ret = t.dim();
* //
* NOTE: intentionally not in the gtsam namespace to allow for classes not in // /**
* the gtsam namespace to be more easily enforced as testable // * Returns Retraction update of T
*/ // */
#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>; // T retract_ret = t.retract(gtsam::zero(dim_ret));
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; //
// /**
// * Returns local coordinates of another object
// */
// Vector localCoords_ret = t.localCoordinates(t2);
//
// return retract_ret;
// }
//};
//
//} // \ namespace gtsam
//
///**
// * Macros for using the ManifoldConcept
// * - An instantiation for use inside unit tests
// * - A typedef for use inside generic algorithms
// *
// * NOTE: intentionally not in the gtsam namespace to allow for classes not in
// * the gtsam namespace to be more easily enforced as testable
// */
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)

30
gtsam/base/concepts.h
View File

@ -206,6 +206,36 @@ struct LieGroup {
} // namespace internal } // namespace internal
template<typename ManifoldType>
struct Canonical {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<group_tag, typename traits_x<ManifoldType>::structure_category_tag>::value),
"This type's trait does not assert it is a group (or derived)");
typedef traits_x<ManifoldType> Traits;
typedef typename Traits::TangentVector TangentVector;
enum { dimension = Traits::dimension };
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(const ManifoldType& other) {
return Traits::Local(Traits::Identity(), other);
}
static ManifoldType Retract(const TangentVector& v) {
return Traits::Retract(Traits::Identity(), v);
}
static TangentVector Local(const ManifoldType& other,
ChartJacobian Hother) {
return Traits::Local(Traits::Identity(), other, boost::none, Hother);
}
static ManifoldType Retract(const ManifoldType& origin,
const TangentVector& v,
ChartJacobian Horigin,
ChartJacobian Hv) {
return Traits::Retract(Traits::Identity(), v, boost::none, Hv);
}
};
/// Check invariants for Manifold type /// Check invariants for Manifold type
template<typename T> template<typename T>

21
gtsam/geometry/Cal3Bundler.h
View File

@ -37,6 +37,8 @@ private:
public: public:
enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -169,24 +171,7 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<Cal3Bundler> : public boost::true_type{ struct traits_x<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
};
template<>
struct GTSAM_EXPORT dimension<Cal3Bundler> : public boost::integral_constant<int, 3>{
};
template<>
struct GTSAM_EXPORT zero<Cal3Bundler> {
static Cal3Bundler value() {
return Cal3Bundler(0, 0, 0);
}
};
}
} // namespace gtsam } // namespace gtsam

14
gtsam/geometry/Cal3DS2.h
View File

@ -43,6 +43,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
public: public:
enum { dimension = 9 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -107,18 +109,8 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<Cal3DS2> : public boost::true_type{ struct traits_x<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
};
template<>
struct GTSAM_EXPORT dimension<Cal3DS2> : public boost::integral_constant<int, 9>{
};
}
} }

18
gtsam/geometry/Cal3Unified.h
View File

@ -50,6 +50,7 @@ private:
double xi_; // mirror parameter double xi_; // mirror parameter
public: public:
enum { dimension = 10 };
Vector vector() const ; Vector vector() const ;
@ -138,23 +139,8 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<Cal3Unified> : public boost::true_type{ struct traits_x<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
};
template<>
struct GTSAM_EXPORT dimension<Cal3Unified> : public boost::integral_constant<int, 10>{
};
template<>
struct GTSAM_EXPORT zero<Cal3Unified> {
static Cal3Unified value() { return Cal3Unified();}
};
}
} }

19
gtsam/geometry/Cal3_S2.h
View File

@ -36,7 +36,7 @@ private:
double fx_, fy_, s_, u0_, v0_; double fx_, fy_, s_, u0_, v0_;
public: public:
enum { dimension = 5 };
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
/// @name Standard Constructors /// @name Standard Constructors
@ -224,22 +224,7 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<Cal3_S2> : public boost::true_type{ struct traits_x<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
};
template<>
struct GTSAM_EXPORT dimension<Cal3_S2> : public boost::integral_constant<int, 5>{
};
template<>
struct GTSAM_EXPORT zero<Cal3_S2> {
static Cal3_S2 value() { return Cal3_S2();}
};
}
} // \ namespace gtsam } // \ namespace gtsam

19
gtsam/geometry/CalibratedCamera.h
View File

@ -44,6 +44,8 @@ private:
public: public:
enum { dimension = 6 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -199,23 +201,8 @@ private:
/// @} /// @}
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type { struct traits_x<CalibratedCamera> : public internal::LieGroup<CalibratedCamera> {};
};
template<>
struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
};
template<>
struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
int, 6> {
};
}
} }

19
gtsam/geometry/EssentialMatrix.h
View File

@ -30,6 +30,8 @@ private:
public: public:
enum { dimension = 5 };
/// Static function to convert Point2 to homogeneous coordinates /// Static function to convert Point2 to homogeneous coordinates
static Vector Homogeneous(const Point2& p) { static Vector Homogeneous(const Point2& p) {
return (Vector(3) << p.x(), p.y(), 1).finished(); return (Vector(3) << p.x(), p.y(), 1).finished();
@ -196,23 +198,8 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<EssentialMatrix> : public boost::true_type{ struct traits_x<EssentialMatrix> : public internal::LieGroup<EssentialMatrix> {};
};
template<>
struct GTSAM_EXPORT dimension<EssentialMatrix> : public boost::integral_constant<int, 5>{
};
template<>
struct GTSAM_EXPORT zero<EssentialMatrix> {
static EssentialMatrix value() { return EssentialMatrix();}
};
}
} // gtsam } // gtsam

18
gtsam/geometry/Point2.h
View File

@ -39,7 +39,7 @@ private:
double x_, y_; double x_, y_;
public: public:
enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -245,22 +245,8 @@ private:
/// multiply with scalar /// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;} inline Point2 operator*(double s, const Point2& p) {return p*s;}
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<Point2> : public boost::true_type{ struct traits_x<Point2> : public internal::LieGroup<Point2> {};
};
template<>
struct GTSAM_EXPORT is_manifold<Point2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Point2> : public boost::integral_constant<int, 2>{
};
}
} }

17
gtsam/geometry/Point3.h
View File

@ -43,6 +43,7 @@ namespace gtsam {
double x_, y_, z_; double x_, y_, z_;
public: public:
enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -248,20 +249,6 @@ namespace gtsam {
/// Syntactic sugar for multiplying coordinates by a scalar s*p /// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;} inline Point3 operator*(double s, const Point3& p) { return p*s;}
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<Point3> : public boost::true_type{ struct traits_x<Point3> : public internal::LieGroup<Point3> {};
};
template<>
struct GTSAM_EXPORT is_manifold<Point3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Point3> : public boost::integral_constant<int, 3>{
};
}
} }

14
gtsam/geometry/Pose2.h
View File

@ -47,6 +47,8 @@ private:
public: public:
enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -298,18 +300,8 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair; typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs); GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<Pose2> : public boost::true_type{ struct traits_x<Pose2> : public internal::LieGroup<Pose2> {};
};
template<>
struct GTSAM_EXPORT dimension<Pose2> : public boost::integral_constant<int, 3>{
};
}
} // namespace gtsam } // namespace gtsam

18
gtsam/geometry/Pose3.h
View File

@ -53,6 +53,8 @@ private:
public: public:
enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -354,21 +356,7 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
typedef std::pair<Point3, Point3> Point3Pair; typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs); GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<Pose3> : public boost::true_type{ struct traits_x<Pose3> : public internal::LieGroup<Pose3> {};
};
template<>
struct GTSAM_EXPORT is_manifold<Pose3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Pose3> : public boost::integral_constant<int, 6>{
};
}
} // namespace gtsam } // namespace gtsam

15
gtsam/geometry/Rot2.h
View File

@ -241,20 +241,7 @@ namespace gtsam {
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<Rot2> : public boost::true_type{ struct traits_x<Rot2> : public internal::LieGroup<Rot2> {};
};
template<>
struct GTSAM_EXPORT is_manifold<Rot2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Rot2> : public boost::integral_constant<int, 1>{
};
}
} // gtsam } // gtsam

19
gtsam/geometry/StereoCamera.h
View File

@ -44,6 +44,8 @@ private:
public: public:
enum { dimension = 6 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -150,22 +152,7 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct GTSAM_EXPORT is_manifold<StereoCamera> : public boost::true_type{ struct traits_x<StereoCamera> : public internal::LieGroup<StereoCamera> {};
};
template<>
struct GTSAM_EXPORT dimension<StereoCamera> : public boost::integral_constant<int, 6>{
};
template<>
struct GTSAM_EXPORT zero<StereoCamera> {
static StereoCamera value() { return StereoCamera();}
};
}
} }

16
gtsam/geometry/StereoPoint2.h
View File

@ -174,20 +174,6 @@ namespace gtsam {
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct is_group<StereoPoint2> : public boost::true_type { struct traits_x<StereoPoint2> : public internal::LieGroup<StereoPoint2> {};
};
template<>
struct is_manifold<StereoPoint2> : public boost::true_type {
};
template<>
struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
};
}
} }

6
gtsam/nonlinear/AdaptAutoDiff.h
View File

@ -28,9 +28,9 @@ namespace gtsam {
template<typename F, typename T, typename A1, typename A2> template<typename F, typename T, typename A1, typename A2>
class AdaptAutoDiff { class AdaptAutoDiff {
static const int N = traits::dimension<T>::value; static const int N = traits:_x<T>:dimension;
static const int M1 = traits::dimension<A1>::value; static const int M1 = traits_x<A1>::dimension;
static const int M2 = traits::dimension<A2>::value; static const int M2 = traits_x<A2>::dimension;
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1; typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2; typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;