diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 724306159..b276d9dce 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -33,7 +33,6 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point2 { - private: double x_, y_; @@ -118,7 +117,10 @@ public: } /// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity() - inline Point2 inverse() const { return Point2(-x_, -y_); } + inline Point2 inverse(OptionalJacobian<2,2> H=boost::none) const { + if (H) *H = -I_2x2; + return Point2(-x_, -y_); + } /// syntactic sugar for inverse, i.e., -p == inverse(p) inline Point2 operator- () const {return Point2(-x_,-y_);} @@ -159,9 +161,17 @@ public: /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } + inline Point2 retract(const Vector& v, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const { + CONCEPT_NOT_IMPLEMENTED; + return *this + Point2(v); + } /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } + inline Vector localCoordinates(const Point2& t2, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const { + CONCEPT_NOT_IMPLEMENTED; + return Logmap(between(t2)); + } /// @} /// @name Lie Group @@ -169,9 +179,17 @@ public: /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } + static Point2 Expmap(const Vector& v, OptionalJacobian<2,2> H) { + CONCEPT_NOT_IMPLEMENTED; + } /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); } + static Vector Logmap(const Point2& dp, OptionalJacobian<2,2> H) { + CONCEPT_NOT_IMPLEMENTED; + return (Vector(2) << dp.x(), dp.y()).finished(); + } + /// @} /// @name Vector Space diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 05359ff1f..6075c2643 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -87,7 +87,10 @@ namespace gtsam { } /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() - inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } + inline Point3 inverse(OptionalJacobian<3,3> H=boost::none) const { + if (H) *H = -I_3x3; + return Point3(-x_, -y_, -z_); + } /// syntactic sugar for inverse, i.e., -p == inverse(p) Point3 operator - () const { return Point3(-x_,-y_,-z_);} @@ -128,6 +131,10 @@ namespace gtsam { /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return Point3(*this + v); } + inline Point3 retract(const Vector& v, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> Hv) const { + CONCEPT_NOT_IMPLEMENTED; + return Point3(*this + v); + } /// Returns inverse retraction inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } @@ -141,10 +148,16 @@ namespace gtsam { /// @{ /** Exponential map at identity - just create a Point3 from x,y,z */ - static inline Point3 Expmap(const Vector& v) { return Point3(v); } + static inline Point3 Expmap(const Vector& v, OptionalJacobian<3,3> H=boost::none) { + if (H) *H = I_3x3; + return Point3(v); + } /** Log map at identity - return the x,y,z of this point */ - static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } + static inline Vector3 Logmap(const Point3& dp, OptionalJacobian<3,3> H=boost::none) { + if (H) *H = I_3x3; + return Vector3(dp.x(), dp.y(), dp.z()); + } /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector& v) {