From 5010f016c7b4178fcf0b8d9486b376235733a203 Mon Sep 17 00:00:00 2001 From: Nick Barrash Date: Tue, 24 Jan 2012 04:13:16 +0000 Subject: [PATCH] geometry doc fixes 2.0 --- gtsam/geometry/Point3.h | 54 +++++++++++++++++++++-------------------- gtsam/geometry/Pose2.h | 18 ++++++++------ gtsam/geometry/Pose3.h | 30 ++++++++++++++--------- gtsam/geometry/Rot2.h | 24 +++++++++--------- gtsam/geometry/Rot3.h | 16 ++++++------ 5 files changed, 77 insertions(+), 65 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 269331490..c2667d3b4 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -84,6 +84,9 @@ namespace gtsam { /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } + /// syntactic sugar for inverse, i.e., -p == inverse(p) + Point3 operator - () const { return Point3(-x_,-y_,-z_);} + /// "Compose" - just adds coordinates of two points inline Point3 compose(const Point3& p2, boost::optional H1=boost::none, @@ -93,6 +96,21 @@ namespace gtsam { return *this + p2; } + ///syntactic sugar for adding two points, i.e., p+q == compose(p,q) + Point3 operator + (const Point3& q) const; + + /** Between using the default implementation */ + inline Point3 between(const Point3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(3); + if(H2) *H2 = eye(3); + return p2 - *this; + } + + /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + Point3 operator - (const Point3& q) const; + /// @} /// @name Manifold /// @{ @@ -120,25 +138,13 @@ namespace gtsam { static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } /// @} - /// @name Vector Operators + /// @name Vector Space /// @{ - ///TODO: comment - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - - ///TODO: comment - bool operator ==(const Point3& q) const; - - ///TODO: comment - Point3 operator + (const Point3& q) const; - - ///TODO: comment - Point3 operator - (const Point3& q) const; - - ///TODO: comment + ///multiply with a scalar Point3 operator * (double s) const; - ///TODO: comment + ///divide by a scalar Point3 operator / (double s) const; /** distance between two points */ @@ -159,14 +165,8 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** Between using the default implementation */ - inline Point3 between(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(3); - if(H2) *H2 = eye(3); - return p2 - *this; - } + /// equality + bool operator ==(const Point3& q) const; /** return vectorized form (column-wise)*/ Vector vector() const { @@ -191,9 +191,10 @@ namespace gtsam { Point3 sub (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// @} + private: - /// @} /// @name Advanced Interface /// @{ @@ -206,11 +207,12 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(z_); } + + /// @} + }; /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} - /// @} - } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 337c8cb81..9f83a3764 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -71,7 +71,7 @@ public: r_(Rot2::fromAngle(theta)), t_(t) { } - ///TODO: comment + /** construct from r,t */ Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ @@ -119,6 +119,14 @@ public: return Pose2(r_*p2.r(), t_ + r_*p2.t()); } + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// @} /// @name Manifold /// @{ @@ -176,13 +184,6 @@ public: /** return transformation matrix */ Matrix matrix() const; - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - /** * Return point coordinates in pose coordinate frame */ @@ -252,6 +253,7 @@ public: inline const Rot2& r() const { return r_; } /** full access functions required by Pose concept */ + inline const Point2& translation() const { return t_; } inline const Rot2& rotation() const { return r_; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d1560c52c..7884e7912 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -98,6 +98,14 @@ namespace gtsam { return Pose3(R_*T.R_, t_ + R_*T.t_); } + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives + */ + Pose3 between(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + /// @} /// @name Manifold /// @{ @@ -135,14 +143,6 @@ namespace gtsam { /// Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose3& p); - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - /** * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) @@ -169,25 +169,33 @@ namespace gtsam { /// @name Standard Interface /// @{ + /// get rotation const Rot3& rotation() const { return R_; } + + /// get translation const Point3& translation() const { return t_; } + /// get x double x() const { return t_.x(); } + + /// get y double y() const { return t_.y(); } + + /// get z double z() const { return t_.z(); } /** convert to 4*4 matrix */ Matrix matrix() const; - /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - Pose3 transform_to(const Pose3& pose) const; /** receives the point in Pose coordinates and transforms it to world coordinates */ Point3 transform_from(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } + /** receives the point in world coordinates and transforms it to Pose coordinates */ Point3 transform_to(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index eb6d1f1e9..7dbac4593 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -126,9 +126,12 @@ namespace gtsam { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } - /** syntactic sugar for rotate */ - inline Point2 operator*(const Point2& p) const { - return rotate(p); + /** Between using the default implementation */ + inline Rot2 between(const Rot2& p2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + if (H1) *H1 = -eye(1); + if (H2) *H2 = eye(1); + return between_default(*this, p2); } /// @} @@ -169,7 +172,7 @@ namespace gtsam { } /// @} - /// @name Vector Operators + /// @name Vector Space /// @{ /** @@ -204,14 +207,6 @@ namespace gtsam { return s_; } - /** Between using the default implementation */ - inline Rot2 between(const Rot2& p2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); - return between_default(*this, p2); - } - /** return 2*2 rotation matrix */ Matrix matrix() const; @@ -225,6 +220,11 @@ namespace gtsam { Point2 rotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + /** syntactic sugar for rotate */ + inline Point2 operator*(const Point2& p) const { + return rotate(p); + } + /** * rotate point from world to rotated * frame = R'*p diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 85911a181..456546a21 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -193,15 +193,15 @@ namespace gtsam { return Rot3(); } + /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity + Rot3 inverse(boost::optional H1=boost::none) const; + /// Compose two rotations i.e., R= (*this) * R2 Rot3 compose(const Rot3& R2, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// rotate point from rotated coordinate frame to world = R*p - Point3 operator*(const Point3& p) const; - - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; + /** compose two rotations */ + Rot3 operator*(const Rot3& R2) const; /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' @@ -210,9 +210,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** compose two rotations */ - Rot3 operator*(const Rot3& R2) const; - /// @} /// @name Manifold /// @{ @@ -280,6 +277,9 @@ namespace gtsam { Point3 rotate(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// rotate point from rotated coordinate frame to world = R*p + Point3 operator*(const Point3& p) const; + /** * rotate point from world to rotated * frame = R'*p