diff --git a/.cproject b/.cproject index 00535c536..3886fae80 100644 --- a/.cproject +++ b/.cproject @@ -2570,6 +2570,14 @@ true true + + make + -j4 + testLieMatrix.run + true + true + true + make -j5 diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index d7bb2f4c0..1a5cf32c2 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -41,7 +41,7 @@ struct LieMatrix : public Matrix { /// @{ enum { dimension = Eigen::Dynamic }; - /** default constructor - should be unnecessary */ + /** default constructor - only for serialize */ LieMatrix() {} /** initialize from a normal matrix */ @@ -83,111 +83,26 @@ struct LieMatrix : public Matrix { } /// @} - /// @name Manifold interface + /// @name VectorSpace requirements /// @{ /** Returns dimensionality of the tangent space */ inline size_t dim() const { return size(); } - typedef Eigen::Matrix RowMajor; - typedef const RowMajor ConstRowMajor; - + /** Convert to vector, is done row-wise - TODO why? */ inline Vector vector() const { Vector result(size()); + typedef Eigen::Matrix RowMajor; Eigen::Map(&result(0), rows(), cols()) = *this; return result; } - /** Update the LieMatrix with a tangent space update. The elements of the - * tangent space vector correspond to the matrix entries arranged in - * *row-major* order. */ - inline LieMatrix retract(const Vector& v) const { - if(v.size() != size()) - throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); - return LieMatrix(*this + Eigen::Map(&v(0), rows(), cols())); - } - - inline LieMatrix retract(const Vector& v, OptionalJacobian<-1, -1> Horigin, - OptionalJacobian<-1, -1> Hv) const { - if (Horigin || Hv) - throw std::runtime_error("LieMatrix::retract derivative not implemented"); - return retract(v); - } - - /** @return the local coordinates of another object. The elements of the - * tangent space vector correspond to the matrix entries arranged in - * *row-major* order. */ - inline Vector localCoordinates(const LieMatrix& t2) const { - Vector result(size()); - Eigen::Map(&result(0), rows(), cols()) = t2 - *this; - return result; - } - - Vector localCoordinates(const LieMatrix& ts, OptionalJacobian<-1, -1> Horigin, - OptionalJacobian<-1, -1> Hother) const { - if (Horigin || Hother) - throw std::runtime_error("LieMatrix::localCoordinates derivative not implemented"); - return localCoordinates(ts); - } - - /// @} - /// @name Group interface - /// @{ - /** identity - NOTE: no known size at compile time - so zero length */ inline static LieMatrix identity() { throw std::runtime_error("LieMatrix::identity(): Don't use this function"); return LieMatrix(); } - - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieMatrix compose(const LieMatrix& p, - OptionalJacobian<-1,-1> H1 = boost::none, - OptionalJacobian<-1,-1> H2 = boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); - - return LieMatrix(*this + p); - } - - /** between operation */ - inline LieMatrix between(const LieMatrix& l2, - OptionalJacobian<-1,-1> H1 = boost::none, - OptionalJacobian<-1,-1> H2 = boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieMatrix(l2 - *this); - } - - /** invert the object and yield a new one */ - inline LieMatrix inverse(OptionalJacobian<-1,-1> H = boost::none) const { - if(H) *H = -eye(dim()); - - return LieMatrix(-(*this)); - } - - /// @} - /// @name Lie group interface - /// @{ - - /** Expmap around identity */ - static inline LieMatrix Expmap(const Vector& v, OptionalJacobian<-1,-1> H = boost::none) { - throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); - return LieMatrix(v); } - - /** Logmap around identity */ - static inline Vector Logmap(const LieMatrix& p, OptionalJacobian<-1,-1> H = boost::none) { - if (H) throw std::runtime_error("LieMatrix::Logmap derivative not implemented"); - Vector result(p.size()); - Eigen::Map >( - result.data(), p.rows(), p.cols()) = p; - return result; - } - /// @} private: @@ -205,6 +120,18 @@ private: template<> -struct traits_x : public internal::VectorSpace {}; +struct traits_x : public internal::VectorSpace { + + // Override Retract, as the default version does not know how to initialize + static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + typedef const Eigen::Matrix RowMajor; + return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); + } + +}; } // \namespace gtsam diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 53dd6d4d5..1d386c0f3 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -30,7 +30,7 @@ TEST( LieMatrix, construction ) { Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); LieMatrix lie1(m), lie2(m); - EXPECT(lie1.dim() == 4); + EXPECT(traits_x::GetDimension(m) == 4); EXPECT(assert_equal(m, lie1.matrix())); EXPECT(assert_equal(lie1, lie2)); } @@ -50,17 +50,17 @@ TEST(LieMatrix, retract) { Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); - LieMatrix actual = init.retract(update); + LieMatrix actual = traits_x::Retract(init,update); EXPECT(assert_equal(expected, actual)); Vector expectedUpdate = update; - Vector actualUpdate = init.localCoordinates(actual); + Vector actualUpdate = traits_x::Local(init,actual); EXPECT(assert_equal(expectedUpdate, actualUpdate)); Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); - Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); + Vector actualLogmap = traits_x::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); EXPECT(assert_equal(expectedLogmap, actualLogmap)); }