diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 46eac05d7..506ae59c9 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -139,10 +139,10 @@ public: /// @name Lie Group /// @{ - /// Exponential map from Lie algebra se(2) to SE(2) + ///Exponential map at identity - create a rotation from canonical coordinates static Pose2 Expmap(const Vector& xi); - /// Exponential map from SE(2) to Lie algebra se(2) + ///Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose2& p); /// @} diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 612111b61..48933a691 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -32,6 +32,7 @@ namespace gtsam { /** * A 3D pose (R,t) : (Rot3,Point3) * @ingroup geometry + * \nosubgrouping */ class Pose3 { public: @@ -47,6 +48,9 @@ namespace gtsam { public: + /// @name Standard Constructors + /// @{ + /** Default constructor is origin */ Pose3() {} @@ -64,16 +68,7 @@ namespace gtsam { R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - 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; - + /// @} /// @name Testable /// @{ @@ -134,13 +129,25 @@ namespace gtsam { /// @name Lie Group /// @{ - /// Exponential map from Lie algebra se(3) to SE(3) + /// Exponential map at identity - create a rotation from canonical coordinates static Pose3 Expmap(const Vector& xi); - /// Exponential map from SE(3) to Lie algebra se(3) + /// 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); } @@ -203,6 +210,10 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// @} + /// @name Advanced Interface + /// @{ + private: /** Serialization function */ friend class boost::serialization::access; @@ -224,5 +235,6 @@ 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 67acc0bc1..eb6d1f1e9 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -155,7 +155,7 @@ namespace gtsam { /// @name Lie Group /// @{ - /// Expmap around identity - create a rotation from an angle + ///Exponential map at identity - create a rotation from canonical coordinates static Rot2 Expmap(const Vector& v) { if (zero(v)) return (Rot2()); @@ -163,7 +163,7 @@ namespace gtsam { return Rot2::fromAngle(v(0)); } - /// Logmap around identity - return the angle of the rotation + ///Log map at identity - return the canonical coordinates of this rotation static inline Vector Logmap(const Rot2& r) { return Vector_(1, r.theta()); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 502734c3e..e7eb1a889 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -281,6 +281,8 @@ namespace gtsam { static Vector Logmap(const Rot3& R); /// @} + /// @name Standard Interface + /// @{ /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -336,6 +338,10 @@ namespace gtsam { */ inline double yaw() const { return ypr()(0); } + /// @} + /// @name Advanced Interface + /// @{ + /** Compute the quaternion representation of this rotation. * @return The quaternion */ @@ -357,6 +363,8 @@ namespace gtsam { } }; + /// @} + /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 4cf46d159..0bdf37e0e 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -132,6 +132,7 @@ namespace gtsam { return Point2(uL_, v_); } + ///TODO comment inline StereoPoint2 between(const StereoPoint2& p2) const { return gtsam::between_default(*this, p2); }