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));
}