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

Now dynamically sized matrices live in manifolds, too.

release/4.3a0
dellaert 2014-11-29 15:06:26 +01:00
parent ea070353d6
commit 63918ff7de
2 changed files with 44 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/base/Manifold.h
View File

@ -73,7 +73,6 @@ template<typename T>
struct dimension: public Dynamic { struct dimension: public Dynamic {
}; };
/** /**
* zero<T>::value is intended to be the origin of a canonical coordinate system * zero<T>::value is intended to be the origin of a canonical coordinate system
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t) * with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
@ -115,8 +114,10 @@ struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : pu
}; };
template<int M, int N, int Options, int MaxRows, int MaxCols> 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, struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
M == Eigen::Dynamic ? Eigen::Dynamic : (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { 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 //TODO after switch to c++11 : the above should should be extracted to a constexpr function
// for readability and to reduce code duplication // for readability and to reduce code duplication
}; };
@ -131,10 +132,10 @@ struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
}; };
template<int M, int N, int Options> template<int M, int N, int Options>
struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<Eigen::Matrix<double, M, N, 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 { template<typename T> struct is_chart: public boost::false_type {
}; };
@ -250,10 +251,14 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
*/ */
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type; typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
typedef type T; typedef type T;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
"DefaultChart has not been implemented yet for dynamically sized matrices");
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) { static vector local(const T& origin, const T& other) {
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin); return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
- reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
} }
static T retract(const T& origin, const vector& d) { static T retract(const T& origin, const vector& d) {
return origin + reshape<M, N, Options>(d); return origin + reshape<M, N, Options>(d);
@ -266,20 +271,36 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// Dynamically sized Vector // Dynamically sized Vector
template<> template<>
struct DefaultChart<Vector> { struct DefaultChart<Vector> {
typedef Vector T; typedef Vector type;
typedef T type; typedef Vector vector;
typedef T vector; static vector local(const Vector& origin, const Vector& other) {
static vector local(const T& origin, const T& other) {
return other - origin; return other - origin;
} }
static T retract(const T& origin, const vector& d) { static Vector retract(const Vector& origin, const vector& d) {
return origin + d; return origin + d;
} }
static int getDimension(const T& origin) { static int getDimension(const Vector& origin) {
return origin.size(); 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 * 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

8
gtsam/nonlinear/tests/testValues.cpp
View File

@ -12,6 +12,8 @@
/** /**
* @file testValues.cpp * @file testValues.cpp
* @author Richard Roberts * @author Richard Roberts
* @author Frank Dellaert
* @author Mike Bosse
*/ */
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
@ -168,9 +170,9 @@ TEST(Values, basic_functions)
Values values; Values values;
const Values& values_c = values; const Values& values_c = values;
values.insert(2, Vector3()); values.insert(2, Vector3());
values.insert(4, Vector3()); values.insert(4, Vector(3));
values.insert(6, Vector3()); values.insert(6, Matrix23());
values.insert(8, Vector3()); values.insert(8, Matrix(2,3));
// find // find
EXPECT_LONGS_EQUAL(4, values.find(4)->key); EXPECT_LONGS_EQUAL(4, values.find(4)->key);