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

zero for double and fixed matrices

release/4.3a0
dellaert 2014-10-21 10:42:05 +02:00
parent bf16446f92
commit 13b433ad89
1 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/base/Manifold.h
View File

@ -81,6 +81,10 @@ template<typename T> struct zero: public identity<T> {
// double // double
template<>
struct is_group<double> : public std::true_type {
};
template<> template<>
struct is_manifold<double> : public std::true_type { struct is_manifold<double> : public std::true_type {
}; };
@ -89,8 +93,17 @@ template<>
struct dimension<double> : public std::integral_constant<int, 1> { struct dimension<double> : public std::integral_constant<int, 1> {
}; };
template<>
struct zero<double> {
static double value() { return 0;}
};
// Fixed size Eigen::Matrix type // Fixed size Eigen::Matrix type
template<int M, int N, int Options>
struct is_group<Eigen::Matrix<double, M, N, Options> > : public std::true_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 std::true_type { struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
}; };
@ -119,6 +132,15 @@ struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_c
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic);
}; };
template<int M, int N, int Options>
struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
int, M * N> {
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic);
static Eigen::Matrix<double, M, N, Options> value() {
return Eigen::Matrix<double, M, N, Options>::Zero();
}
};
} // \ namespace traits } // \ namespace traits
// Chart is a map from T -> vector, retract is its inverse // Chart is a map from T -> vector, retract is its inverse
@ -126,6 +148,7 @@ template<typename T>
struct DefaultChart { struct DefaultChart {
BOOST_STATIC_ASSERT(traits::is_manifold<T>::value); BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector; typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
T const & t_;
DefaultChart(const T& t) : DefaultChart(const T& t) :
t_(t) { t_(t) {
} }
@ -135,19 +158,16 @@ struct DefaultChart {
T retract(const vector& d) { T retract(const vector& d) {
return t_.retract(d); return t_.retract(d);
} }
private:
T const & t_;
}; };
/** /**
* Canonical<T>::value is a chart around zero<T>::value * Canonical<T>::value is a chart around zero<T>::value
* An example is Canonical<Rot3> * An example is Canonical<Rot3>
*/ */
template<typename T> class Canonical { template<typename T> struct Canonical {
DefaultChart<T> chart;
public:
typedef T type; typedef T type;
typedef typename DefaultChart<T>::vector vector; typedef typename DefaultChart<T>::vector vector;
DefaultChart<T> chart;
Canonical() : Canonical() :
chart(traits::zero<T>::value()) { chart(traits::zero<T>::value()) {
} }