From 4e2d7a69bd15ba23c59c18ac436d30760731d7de Mon Sep 17 00:00:00 2001 From: Nick Barrash Date: Tue, 24 Jan 2012 02:27:44 +0000 Subject: [PATCH] fixed some comment groupings in geometry --- gtsam/geometry/Pose2.h | 48 ++++++++++++++++++------------------- gtsam/geometry/Pose3.h | 54 +++++++++++++++++++++--------------------- gtsam/geometry/Rot3.h | 29 ++++++++++++----------- 3 files changed, 66 insertions(+), 65 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 506ae59c9..337c8cb81 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -145,6 +145,30 @@ public: ///Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose2& p); + /** + * Calculate Adjoint map + * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) + */ + Matrix AdjointMap() const; + inline Vector Adjoint(const Vector& xi) const { + assert(xi.size() == 3); + return AdjointMap()*xi; + } + + /** + * wedge for SE(2): + * @param xi 3-dim twist (v,omega) where + * omega is angular velocity + * v (vx,vy) = 2D velocity + * @return xihat, 3*3 element of Lie algebra that can be exponentiated + */ + static inline Matrix wedge(double vx, double vy, double w) { + return Matrix_(3,3, + 0.,-w, vx, + w, 0., vy, + 0., 0., 0.); + } + /// @} /// @name Standard Interface /// @{ @@ -212,30 +236,6 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** - * Calculate Adjoint map - * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) - */ - Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); - return AdjointMap()*xi; - } - - /** - * wedge for SE(2): - * @param xi 3-dim twist (v,omega) where - * omega is angular velocity - * v (vx,vy) = 2D velocity - * @return xihat, 3*3 element of Lie algebra that can be exponentiated - */ - static inline Matrix wedge(double vx, double vy, double w) { - return Matrix_(3,3, - 0.,-w, vx, - w, 0., vy, - 0., 0., 0.); - } - /** get functions for x, y, theta */ /// get x diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 48933a691..d1560c52c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -135,33 +135,6 @@ namespace gtsam { /// Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose3& p); - /// @} - /// @name Standard Interface - /// @{ - - const Rot3& rotation() const { return R_; } - const Point3& translation() const { return t_; } - - double x() const { return t_.x(); } - double y() const { return t_.y(); } - 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; - - /** 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; - /** * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives @@ -192,6 +165,33 @@ namespace gtsam { 0., 0., 0., 0.); } + /// @} + /// @name Standard Interface + /// @{ + + const Rot3& rotation() const { return R_; } + const Point3& translation() const { return t_; } + + double x() const { return t_.x(); } + double y() const { return t_.y(); } + 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; + + /** 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; + /** * Calculate range to a landmark * @param point 3D location of landmark diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index a0775c449..85911a181 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -213,20 +213,6 @@ namespace gtsam { /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - Point3 rotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** - * rotate point from world to rotated - * frame = R'*p - */ - Point3 unrotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// @} /// @name Manifold /// @{ @@ -287,6 +273,21 @@ namespace gtsam { /// @name Standard Interface /// @{ + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point3 rotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point3 unrotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** return 3*3 rotation matrix */ Matrix matrix() const;