Started converting types. Into the ChartValue now.
Paul Furgale
parent
959716ae92
commit
58cb47a3f0
|
|
@ -23,347 +23,347 @@
|
|||
#include <boost/static_assert.hpp>
|
||||
#include <boost/type_traits.hpp>
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A manifold defines a space in which there is a notion of a linear tangent space
|
||||
* that can be centered around a given point on the manifold. These nonlinear
|
||||
* spaces may have such properties as wrapping around (as is the case with rotations),
|
||||
* which might make linear operations on parameters not return a viable element of
|
||||
* the manifold.
|
||||
*
|
||||
* We perform optimization by computing a linear delta in the tangent space of the
|
||||
* current estimate, and then apply this change using a retraction operation, which
|
||||
* maps the change in tangent space back to the manifold itself.
|
||||
*
|
||||
* There may be multiple possible retractions for a given manifold, which can be chosen
|
||||
* between depending on the computational complexity. The important criteria for
|
||||
* the creation for the retract and localCoordinates functions is that they be
|
||||
* inverse operations. The new notion of a Chart guarantees that.
|
||||
*
|
||||
*/
|
||||
|
||||
// Traits, same style as Boost.TypeTraits
|
||||
// All meta-functions below ever only declare a single type
|
||||
// or a type/value/value_type
|
||||
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;
|
||||
//
|
||||
//namespace gtsam {
|
||||
//
|
||||
///**
|
||||
// * A manifold defines a space in which there is a notion of a linear tangent space
|
||||
// * that can be centered around a given point on the manifold. These nonlinear
|
||||
// * spaces may have such properties as wrapping around (as is the case with rotations),
|
||||
// * which might make linear operations on parameters not return a viable element of
|
||||
// * the manifold.
|
||||
// *
|
||||
// * We perform optimization by computing a linear delta in the tangent space of the
|
||||
// * current estimate, and then apply this change using a retraction operation, which
|
||||
// * maps the change in tangent space back to the manifold itself.
|
||||
// *
|
||||
// * There may be multiple possible retractions for a given manifold, which can be chosen
|
||||
// * between depending on the computational complexity. The important criteria for
|
||||
// * the creation for the retract and localCoordinates functions is that they be
|
||||
// * inverse operations. The new notion of a Chart guarantees that.
|
||||
// *
|
||||
// */
|
||||
//
|
||||
//// Traits, same style as Boost.TypeTraits
|
||||
//// All meta-functions below ever only declare a single type
|
||||
//// or a type/value/value_type
|
||||
//namespace traits {
|
||||
//
|
||||
//// is group, by default this is false
|
||||
//template<typename T>
|
||||
//struct dimension: public Dynamic {
|
||||
//struct dimension : public boost::integral_constant<int, 1> { // just temporary fix to minimize compiler errors while refactoring
|
||||
//struct is_group: public boost::false_type {
|
||||
//};
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
||||
/**
|
||||
* 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) template class gtsam::ManifoldConcept<T>;
|
||||
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;
|
||||
//
|
||||
//// 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
|
||||
//
|
||||
///**
|
||||
// * 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)
|
||||
|
|
|
|||
|
|
@ -206,6 +206,36 @@ struct LieGroup {
|
|||
|
||||
} // 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
|
||||
template<typename T>
|
||||
|
|
|
|||
|
|
@ -37,6 +37,8 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -169,24 +171,7 @@ private:
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Cal3Bundler> : public boost::true_type{
|
||||
};
|
||||
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -43,6 +43,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 9 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -107,18 +109,8 @@ private:
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Cal3DS2> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Cal3DS2> : public boost::integral_constant<int, 9>{
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ private:
|
|||
double xi_; // mirror parameter
|
||||
|
||||
public:
|
||||
enum { dimension = 10 };
|
||||
|
||||
Vector vector() const ;
|
||||
|
||||
|
|
@ -138,23 +139,8 @@ private:
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Cal3Unified> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Cal3Unified> : public boost::integral_constant<int, 10>{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT zero<Cal3Unified> {
|
||||
static Cal3Unified value() { return Cal3Unified();}
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ private:
|
|||
double fx_, fy_, s_, u0_, v0_;
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 5 };
|
||||
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
|
||||
|
||||
/// @name Standard Constructors
|
||||
|
|
@ -224,22 +224,7 @@ private:
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Cal3_S2> : public boost::true_type{
|
||||
};
|
||||
|
||||
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();}
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -44,6 +44,8 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -199,23 +201,8 @@ private:
|
|||
/// @}
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
|
||||
int, 6> {
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<CalibratedCamera> : public internal::LieGroup<CalibratedCamera> {};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,6 +30,8 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 5 };
|
||||
|
||||
/// Static function to convert Point2 to homogeneous coordinates
|
||||
static Vector Homogeneous(const Point2& p) {
|
||||
return (Vector(3) << p.x(), p.y(), 1).finished();
|
||||
|
|
@ -196,23 +198,8 @@ private:
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<EssentialMatrix> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<EssentialMatrix> : public boost::integral_constant<int, 5>{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT zero<EssentialMatrix> {
|
||||
static EssentialMatrix value() { return EssentialMatrix();}
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<EssentialMatrix> : public internal::LieGroup<EssentialMatrix> {};
|
||||
|
||||
} // gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ private:
|
|||
double x_, y_;
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -245,22 +245,8 @@ private:
|
|||
/// multiply with scalar
|
||||
inline Point2 operator*(double s, const Point2& p) {return p*s;}
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_group<Point2> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Point2> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Point2> : public boost::integral_constant<int, 2>{
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Point2> : public internal::LieGroup<Point2> {};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ namespace gtsam {
|
|||
double x_, y_, z_;
|
||||
|
||||
public:
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -248,20 +249,6 @@ namespace gtsam {
|
|||
/// Syntactic sugar for multiplying coordinates by a scalar s*p
|
||||
inline Point3 operator*(double s, const Point3& p) { return p*s;}
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_group<Point3> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Point3> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Point3> : public boost::integral_constant<int, 3>{
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Point3> : public internal::LieGroup<Point3> {};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -47,6 +47,8 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -298,18 +300,8 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
|
|||
typedef std::pair<Point2,Point2> Point2Pair;
|
||||
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Pose2> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Pose2> : public boost::integral_constant<int, 3>{
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Pose2> : public internal::LieGroup<Pose2> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -53,6 +53,8 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -354,21 +356,7 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
|
|||
typedef std::pair<Point3, Point3> Point3Pair;
|
||||
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_group<Pose3> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Pose3> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Pose3> : public boost::integral_constant<int, 6>{
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<Pose3> : public internal::LieGroup<Pose3> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -241,20 +241,7 @@ namespace gtsam {
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -44,6 +44,8 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -150,22 +152,7 @@ private:
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<StereoCamera> : public boost::true_type{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<StereoCamera> : public boost::integral_constant<int, 6>{
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT zero<StereoCamera> {
|
||||
static StereoCamera value() { return StereoCamera();}
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<StereoCamera> : public internal::LieGroup<StereoCamera> {};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -174,20 +174,6 @@ namespace gtsam {
|
|||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct is_group<StereoPoint2> : public boost::true_type {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct is_manifold<StereoPoint2> : public boost::true_type {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
|
||||
};
|
||||
|
||||
}
|
||||
struct traits_x<StereoPoint2> : public internal::LieGroup<StereoPoint2> {};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,9 +28,9 @@ namespace gtsam {
|
|||
template<typename F, typename T, typename A1, typename A2>
|
||||
class AdaptAutoDiff {
|
||||
|
||||
static const int N = traits::dimension<T>::value;
|
||||
static const int M1 = traits::dimension<A1>::value;
|
||||
static const int M2 = traits::dimension<A2>::value;
|
||||
static const int N = traits:_x<T>:dimension;
|
||||
static const int M1 = traits_x<A1>::dimension;
|
||||
static const int M2 = traits_x<A2>::dimension;
|
||||
|
||||
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
|
||||
typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
|
||||
|
|
|
|||
Loading…
Reference in New Issue