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

TangentVector meta-function

release/4.3a0
dellaert 2014-10-18 14:29:40 +02:00
parent ec69949f43
commit 10cfd47404
1 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -339,7 +339,15 @@ struct is_manifold: public false_type {
template<typename T> template<typename T>
struct dimension; struct dimension;
// TangentVector is eigen::Matrix type in tangent space
template<typename T>
struct TangentVector {
BOOST_STATIC_ASSERT(is_manifold<T>::value);
typedef Eigen::Matrix<double, dimension<T>::value, 1> type;
};
// Fixed size Eigen::Matrix type // Fixed size Eigen::Matrix type
template<int M, int N, int Options> template<int M, int N, int Options>
struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public true_type { struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public true_type {
}; };
@ -363,12 +371,11 @@ struct dimension<Point2> : public integral_constant<size_t, 2> {
template<typename T> template<typename T>
struct manifold_traits { struct manifold_traits {
typedef T type; typedef T type;
static const size_t dim = dimension<T>::value; static typename TangentVector<T>::type localCoordinates(const T& t1,
typedef Eigen::Matrix<double, dim, 1> tangent; const T& t2) {
static tangent localCoordinates(const T& t1, const T& t2) {
return t1.localCoordinates(t2); return t1.localCoordinates(t2);
} }
static type retract(const type& t, const tangent& d) { static type retract(const type& t, const typename TangentVector<T>::type& d) {
return t.retract(d); return t.retract(d);
} }
}; };
@ -378,13 +385,14 @@ template<int M, int N, int Options>
struct manifold_traits<Eigen::Matrix<double, M, N, Options> > { struct manifold_traits<Eigen::Matrix<double, M, N, Options> > {
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic);
typedef Eigen::Matrix<double, M, N, Options> type; typedef Eigen::Matrix<double, M, N, Options> type;
static const size_t dim = dimension<type>::value; static typename TangentVector<type>::type localCoordinates(const type& t1,
typedef Eigen::Matrix<double, dim, 1> tangent; const type& t2) {
static tangent localCoordinates(const type& t1, const type& t2) {
type diff = t2 - t1; type diff = t2 - t1;
return tangent(Eigen::Map<tangent>(diff.data())); return typename TangentVector<type>::type(
Eigen::Map<typename TangentVector<type>::type>(diff.data()));
} }
static type retract(const type& t, const tangent& d) { static type retract(const type& t,
const typename TangentVector<type>::type& d) {
type sum = t + Eigen::Map<const type>(d.data()); type sum = t + Eigen::Map<const type>(d.data());
return sum; return sum;
} }