diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 7c167a46b..90b7207a2 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -31,7 +31,7 @@ namespace gtsam { /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as * we can directly add double, Vector, and Matrix into values now, because of * gtsam::traits. */ @@ -83,6 +83,26 @@ struct LieMatrix : public Matrix { } /// @} + + /// @name Group + /// @{ + LieMatrix compose(const LieMatrix& q) { return (*this)+q;} + LieMatrix between(const LieMatrix& q) { return q-(*this);} + LieMatrix inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} + LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieMatrix& p) {return p.vector();} + static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} + /// @} + /// @name VectorSpace requirements /// @{ diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 2e4be834f..9f6c56b28 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -49,17 +49,36 @@ namespace gtsam { /** Automatic conversion to underlying value */ operator double() const { return d_; } + /** convert vector */ + Vector1 vector() const { Vector1 v; v<