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

DefaultChart for dynamically sized Vector

release/4.3a0
dellaert 2014-10-21 18:04:19 +02:00
parent fcbc1e90cf
commit f46aa7cd8c
1 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/base/Manifold.h
View File

@ -95,7 +95,9 @@ struct dimension<double> : public std::integral_constant<int, 1> {
template<> template<>
struct zero<double> { struct zero<double> {
static double value() { return 0;} static double value() {
return 0;
}
}; };
// Fixed size Eigen::Matrix type // Fixed size Eigen::Matrix type
@ -118,24 +120,22 @@ struct dimension<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Options>
template<int M, int Options> template<int M, int Options>
struct dimension<Eigen::Matrix<double, M, Eigen::Dynamic, Options> > : public Dynamic { struct dimension<Eigen::Matrix<double, M, Eigen::Dynamic, Options> > : public Dynamic {
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic);
}; };
template<int N, int Options> template<int N, int Options>
struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dynamic { struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dynamic {
BOOST_STATIC_ASSERT(N!=Eigen::Dynamic);
}; };
template<int M, int N, int Options> template<int M, int N, int Options>
struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant< struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
int, M * N> { int, M * N> {
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic);
}; };
template<int M, int N, int Options> template<int M, int N, int Options>
struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant< struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
int, M * N> { int, M * N> {
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic); 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() { static Eigen::Matrix<double, M, N, Options> value() {
return Eigen::Matrix<double, M, N, Options>::Zero(); return Eigen::Matrix<double, M, N, Options>::Zero();
} }
@ -206,6 +206,8 @@ template<int M, int N, int Options>
struct DefaultChart<Eigen::Matrix<double, M, N, Options> > { struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> T; typedef Eigen::Matrix<double, M, N, Options> T;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector; typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"DefaultChart has not been implemented yet for dynamically sized matrices");
T t_; T t_;
DefaultChart(const T& t) : DefaultChart(const T& t) :
t_(t) { t_(t) {
@ -221,6 +223,23 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
} }
}; };
// Dynamically sized Vector
template<>
struct DefaultChart<Vector> {
typedef Vector T;
typedef T vector;
T t_;
DefaultChart(const T& t) :
t_(t) {
}
vector apply(const T& other) {
return other - t_;
}
T retract(const vector& d) {
return t_ + d;
}
};
/** /**
* Old Concept check class for Manifold types * Old Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying * Requires a mapping between a linear tangent space and the underlying