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

traits for eigen matrices

release/4.3a0
Mike Bosse 2014-12-14 23:24:19 +01:00
parent 0706caf173
commit 0857c1069c
3 changed files with 85 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/LieMatrix.h
View File

@ -71,7 +71,7 @@ struct LieMatrix : public Matrix {
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** get the underlying vector */ /** get the underlying matrix */
inline Matrix matrix() const { inline Matrix matrix() const {
return static_cast<Matrix>(*this); return static_cast<Matrix>(*this);
} }

13
gtsam/base/LieVector.h
View File

@ -31,6 +31,8 @@ namespace gtsam {
*/ */
struct LieVector : public Vector { struct LieVector : public Vector {
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */ /** default constructor - should be unnecessary */
LieVector() {} LieVector() {}
@ -131,17 +133,8 @@ private:
} }
}; };
// Define GTSAM traits
namespace traits {
template<> template<>
struct is_manifold<LieVector> : public boost::true_type { struct traits_x<LieVector> : public internal::LieGroup<LieVector> {};
};
template<>
struct dimension<LieVector> : public Dynamic {
};
}
} // \namespace gtsam } // \namespace gtsam

169
gtsam/base/concepts.h
View File

@ -231,7 +231,7 @@ struct ScalarTraits {
// For Testable // For Testable
static void Print(Scalar m, const std::string& str = "") { static void Print(Scalar m, const std::string& str = "") {
m.print(); gtsam::print(m,str);
} }
static bool Equals(Scalar m1, static bool Equals(Scalar m1,
Scalar m2, Scalar m2,
@ -252,7 +252,7 @@ struct ScalarTraits {
static TangentVector Local(Scalar origin, static TangentVector Local(Scalar origin,
Scalar other, Scalar other,
ChartJacobian Horigin, ChartJacobian Horigin,
ChartJacobian Hother) { ChartJacobian Hother = boost::none) {
if(Horigin) { if(Horigin) {
(*Horigin)[0] = -1.0; (*Horigin)[0] = -1.0;
} }
@ -265,7 +265,7 @@ struct ScalarTraits {
static Scalar Retract(Scalar origin, static Scalar Retract(Scalar origin,
const TangentVector& v, const TangentVector& v,
ChartJacobian Horigin, ChartJacobian Horigin,
ChartJacobian Hv) { ChartJacobian Hv = boost::none) {
if(Horigin) { if(Horigin) {
(*Horigin)[0] = 1.0; (*Horigin)[0] = 1.0;
} }
@ -295,7 +295,7 @@ struct ScalarTraits {
static Scalar Compose(Scalar m1, static Scalar Compose(Scalar m1,
Scalar m2, Scalar m2,
ChartJacobian H1, ChartJacobian H1,
ChartJacobian H2) { ChartJacobian H2 = boost::none) {
if(H1) { if(H1) {
(*H1)[0] = 1.0; (*H1)[0] = 1.0;
} }
@ -308,7 +308,7 @@ struct ScalarTraits {
static Scalar Between(Scalar m1, static Scalar Between(Scalar m1,
Scalar m2, Scalar m2,
ChartJacobian H1, ChartJacobian H1,
ChartJacobian H2) { ChartJacobian H2 = boost::none) {
if(H1) { if(H1) {
(*H1)[0] = -1.0; (*H1)[0] = -1.0;
} }
@ -360,7 +360,7 @@ struct traits_x<double> : public internal::ScalarTraits<double> {};
template<> template<>
struct traits_x<float> : public internal::ScalarTraits<float> {}; struct traits_x<float> : public internal::ScalarTraits<float> {};
// traits for any double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols> template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > { struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// Typedefs required by all manifold types. // Typedefs required by all manifold types.
@ -370,6 +370,13 @@ struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> ManifoldType; typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> ManifoldType;
static int GetDimension(const ManifoldType& m){ return m.rows()*m.cols(); }
static Eigen::Matrix<double, dimension, dimension> Eye(const ManifoldType& m) {
int dim = GetDimension(m);
return Eigen::Matrix<double, dimension, dimension>::Identity(dim,dim);
}
// For Testable // For Testable
static void Print(const ManifoldType& m, const std::string& str = "") { static void Print(const ManifoldType& m, const std::string& str = "") {
gtsam::print(m, str); gtsam::print(m, str);
@ -380,88 +387,74 @@ struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
return equal_with_abs_tol(m1, m2, 1e-9); return equal_with_abs_tol(m1, m2, 1e-9);
} }
// static TangentVector Local(const ManifoldType& origin, static TangentVector Local(const ManifoldType& origin,
// const ManifoldType& other) { const ManifoldType& other,
// TangentVector result(GetDimension(origin)); ChartJacobian Horigin = boost::none,
// Eigen::Map<Eigen::Matrix<double, M, N, Eigen::RowMajor> >( ChartJacobian Hother = boost::none) {
// &result(0), origin.rows(), origin.cols()) = other - origin; if (Horigin) *Horigin = -Eye(origin);
// return result; if (Hother) *Hother = Eye(origin);
// } TangentVector result(GetDimension(origin));
// Eigen::Map<Eigen::Matrix<double, M, N> >(
// static ManifoldType Retract(const ManifoldType& origin, result.data(), origin.rows(), origin.cols()) = other - origin;
// const TangentVector& v) { return result;
// return origin.retract(v); }
// }
// static ManifoldType Retract(const ManifoldType& origin,
// static TangentVector Local(const ManifoldType& origin, const TangentVector& v,
// const ManifoldType& other, ChartJacobian Horigin = boost::none,
// ChartJacobian Horigin, ChartJacobian Hv = boost::none) {
// ChartJacobian Hother) { if (Horigin) *Horigin = Eye(origin);
// return origin.localCoordinates(other, Horigin, Hother); if (Hv) *Hv = Eye(origin);
// } return origin +
// Eigen::Map<Eigen::Matrix<double, M, N> >(v.data(), origin.rows(), origin.cols());
// static ManifoldType Retract(const ManifoldType& origin, }
// const TangentVector& v,
// ChartJacobian Horigin, static ManifoldType Compose(const ManifoldType& m1,
// ChartJacobian Hv) { const ManifoldType& m2,
// return origin.retract(v, Horigin, Hv); ChartJacobian H1 = boost::none,
// } ChartJacobian H2 = boost::none) {
// if (H1) *H1 = Eye(m1);
// static int GetDimension(const ManifoldType& m){ return m.dim(); } if (H2) *H2 = Eye(m1);
//
// // For Group. Only implemented for groups return m1+m2;
// static ManifoldType Compose(const ManifoldType& m1, }
// const ManifoldType& m2) {
// return m1.compose(m2); static ManifoldType Between(const ManifoldType& m1,
// } const ManifoldType& m2,
// ChartJacobian H1 = boost::none,
// static ManifoldType Between(const ManifoldType& m1, ChartJacobian H2 = boost::none) {
// const ManifoldType& m2) { if (H1) *H1 = -Eye(m1);
// return m1.between(m2); if (H2) *H2 = Eye(m1);
// }
// return m2-m1;
// static ManifoldType Inverse(const ManifoldType& m) { }
// return m.inverse();
// } static ManifoldType Inverse(const ManifoldType& m,
// ChartJacobian H = boost::none) {
// static ManifoldType Compose(const ManifoldType& m1, if (H) *H = -Eye(m);
// const ManifoldType& m2, return -m;
// ChartJacobian H1, }
// ChartJacobian H2) {
// return m1.compose(m2, H1, H2); static ManifoldType Identity() {
// } //FIXME: this won't work for dynamic matrices, but where to get the size???
// return ManifoldType::Zero();
// static ManifoldType Between(const ManifoldType& m1, }
// const ManifoldType& m2,
// ChartJacobian H1, static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) {
// ChartJacobian H2) { if (Hm) *Hm = Eye(m);
// return m1.between(m2, H1, H2); TangentVector result(GetDimension(m));
// } Eigen::Map<Eigen::Matrix<double, M, N> >(
// result.data(), m.rows(), m.cols()) = m;
// static ManifoldType Inverse(const ManifoldType& m, return result;
// ChartJacobian H) { }
// return m.inverse(H);
// } //FIXME: this also does not work for dynamic matrices
// static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
// static ManifoldType Identity() { ManifoldType m; m.setZero();
// return ManifoldType::identity(); if (Hv) *Hv = Eye(m);
// } return m +
// Eigen::Map<Eigen::Matrix<double, M, N> >(v.data(), m.rows(), m.cols());
// static TangentVector Logmap(const ManifoldType& m) { }
// return ManifoldType::Logmap(m);
// }
//
// static ManifoldType Expmap(const TangentVector& v) {
// return ManifoldType::Expmap(v);
// }
//
// static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) {
// return ManifoldType::Logmap(m, Hm);
// }
//
// static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) {
// return ManifoldType::Expmap(v, Hv);
// }
}; };