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

57
gtsam/base/Manifold.h
View File

@ -73,7 +73,6 @@ template<typename T>
struct dimension: public Dynamic {
};
/**
* zero<T>::value is intended to be the origin of a canonical coordinate system
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
@ -111,14 +110,16 @@ struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : publi
};
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{
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
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>
@ -131,10 +132,10 @@ struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
};
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 {
};
@ -248,12 +249,16 @@ 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 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),
"DefaultChart has not been implemented yet for dynamically sized matrices");
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);
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);
@ -266,20 +271,36 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// Dynamically sized Vector
template<>
struct DefaultChart<Vector> {
typedef Vector T;
typedef T type;
typedef T vector;
static vector local(const T& origin, const T& other) {
typedef Vector type;
typedef Vector vector;
static vector local(const Vector& origin, const Vector& other) {
return other - origin;
}
static T retract(const T& origin, const vector& d) {
static Vector retract(const Vector& origin, const vector& d) {
return origin + d;
}
static int getDimension(const T& origin) {
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

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

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