diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 71adc58bc..120f8dc8b 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -34,11 +34,11 @@ namespace gtsam { private: double fx_, fy_, s_, u0_, v0_; + public: + /// @name Standard Constructors /// @{ - public: - /// Create a default calibration that leaves coordinates unchanged Cal3_S2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 9f83a3764..3a8f69899 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -177,23 +177,16 @@ public: 0., 0., 0.); } - /// @} - /// @name Standard Interface + /// @} + /// @name Group Action on Point2 /// @{ - /** return transformation matrix */ - Matrix matrix() const; - - /** - * Return point coordinates in pose coordinate frame - */ + /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** - * Return point coordinates in global frame - */ + /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const; @@ -201,6 +194,34 @@ public: /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} + /// @} + /// @name Standard Interface + /// @{ + + /// get x + inline double x() const { return t_.x(); } + + /// get y + inline double y() const { return t_.y(); } + + /// get theta + inline double theta() const { return r_.theta(); } + + /// translation + inline const Point2& t() const { return t_; } + + /// rotation + inline const Rot2& r() const { return r_; } + + /// translation + inline const Point2& translation() const { return t_; } + + /// rotation + inline const Rot2& rotation() const { return r_; } + + //// return transformation matrix + Matrix matrix() const; + /** * Calculate bearing to a landmark * @param point 2D location of landmark @@ -237,26 +258,6 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** get functions for x, y, theta */ - - /// get x - inline double x() const { return t_.x(); } - - /// get y - inline double y() const { return t_.y(); } - - /// get theta - inline double theta() const { return r_.theta(); } - - /** shorthand access functions */ - inline const Point2& t() const { return t_; } - 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_; } - private: /// @} diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 7884e7912..640016970 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -165,6 +165,21 @@ namespace gtsam { 0., 0., 0., 0.); } + /// @} + /// @name Group Action on Point3 + /// @{ + + /** 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; + /// @} /// @name Standard Interface /// @{ @@ -187,19 +202,9 @@ namespace gtsam { /** convert to 4*4 matrix */ Matrix matrix() const; + /** receives a pose in world coordinates and transforms it to local coordinates */ 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; - /** * Calculate range to a landmark * @param point 3D location of landmark @@ -230,6 +235,8 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } + /// @} + }; // Pose3 class /** @@ -243,6 +250,5 @@ namespace gtsam { inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); } - /// @} } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 7dbac4593..f3062f5c1 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -44,9 +44,6 @@ namespace gtsam { /** normalize to make sure cos and sin form unit vector */ Rot2& normalize(); - /// @name Standard Constructors - /// @{ - /** private constructor from cos/sin */ inline Rot2(double c, double s) : c_(c), s_(s) { @@ -54,6 +51,9 @@ namespace gtsam { public: + /// @name Constructors and named constructors + /// @{ + /** default constructor, zero rotation */ Rot2() : c_(1.0), s_(0.0) { @@ -171,21 +171,36 @@ namespace gtsam { return Vector_(1, r.theta()); } - /// @} - /// @name Vector Space - /// @{ + /// @} + /// @name Group Action on Point2 + /// @{ + + /** + * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ + */ + 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); + } /** - * Creates a unit vector as a Point2 + * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - inline Point2 unit() const { - return Point2(c_, s_); - } + Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; /// @} /// @name Standard Interface /// @{ + /// Creates a unit vector as a Point2 + inline Point2 unit() const { + return Point2(c_, s_); + } + /** return angle (RADIANS) */ double theta() const { return ::atan2(s_, c_); @@ -213,25 +228,6 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - 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 - */ - Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; - /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 456546a21..2e16f869c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -266,27 +266,28 @@ namespace gtsam { */ static Vector Logmap(const Rot3& R); - /// @} - /// @name Standard Interface + /// @} + /// @name Group Action on Point3 /// @{ /** - * 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 rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ + */ + 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 rotated coordinate frame to world = R*p + Point3 operator*(const Point3& p) 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; + /** + * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + */ + Point3 unrotate(const Point3& p, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; + /// @} + /// @name Standard Interface + /// @{ /** return 3*3 rotation matrix */ Matrix matrix() const;