diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 395fb88d9..1b9559e67 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,165 +18,145 @@ #pragma once -#include #include +#include +#include namespace gtsam { - /** - * A 2D stereo point, v will be same for rectified images - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT StereoPoint2 { - public: - static const size_t dimension = 3; - private: - double uL_, uR_, v_; +/** + * A 2D stereo point, v will be same for rectified images + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT StereoPoint2 { +private: - public: + double uL_, uR_, v_; - /// @name Standard Constructors - /// @{ +public: + enum { dimension = 3 }; + /// @name Standard Constructors + /// @{ - /** default constructor */ - StereoPoint2() : + /** default constructor */ + StereoPoint2() : uL_(0), uR_(0), v_(0) { - } + } - /** constructor */ - StereoPoint2(double uL, double uR, double v) : + /** constructor */ + StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { - } + } - /// @} - /// @name Testable - /// @{ + /// construct from 3D vector + StereoPoint2(const Vector3& v) : + uL_(v(0)), uR_(v(1)), v_(v(2)) {} - /** print */ - void print(const std::string& s="") const; + /// @} + /// @name Testable + /// @{ - /** equals */ - bool equals(const StereoPoint2& q, double tol=1e-9) const { - return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_ - - q.v_) < tol); - } + /** print */ + void print(const std::string& s = "") const; - /// @} - /// @name Group - /// @{ + /** equals */ + bool equals(const StereoPoint2& q, double tol = 1e-9) const { + return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol + && fabs(v_ - q.v_) < tol); + } - /// identity - inline static StereoPoint2 identity() { return StereoPoint2(); } + /// @} + /// @name Group + /// @{ - /// inverse - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); - } + /// identity + inline static StereoPoint2 identity() { + return StereoPoint2(); + } - /// "Compose", just adds the coordinates of two points. - inline StereoPoint2 compose(const StereoPoint2& p1) const { - return *this + p1; - } + /// inverse + StereoPoint2 operator-() const { + return StereoPoint2(-uL_, -uR_, -v_); + } - /// add two stereo points - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } + /// add + inline StereoPoint2 operator +(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); + } - /// subtract two stereo points - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /// @} - /// @name Manifold - /// @{ + /// subtract + inline StereoPoint2 operator -(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); + } - /// dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Standard Interface + /// @{ - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } + /// equality + inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;} - /// Updates a with tangent space delta - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /// get uL + inline double uL() const {return uL_;} - /// Returns inverse retraction - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /// get uR + inline double uR() const {return uR_;} - /// @} - /// @name Lie Group - /// @{ + /// get v + inline double v() const {return v_;} - /** Exponential map around identity - just create a Point2 from a vector */ - static inline StereoPoint2 Expmap(const Vector& d) { - return StereoPoint2(d(0), d(1), d(2)); - } + /** convert to vector */ + Vector3 vector() const { + return Vector3(uL_, uR_, v_); + } - /** Log map around identity - just return the Point2 as a vector */ - static inline Vector Logmap(const StereoPoint2& p) { - return p.vector(); - } + /** convenient function to get a Point2 from the left image */ + Point2 point2() const { + return Point2(uL_, v_); + } - /** The difference between another point and this point */ - inline StereoPoint2 between(const StereoPoint2& p2) const { - return p2 - *this; - } + /** convenient function to get a Point2 from the right image */ + Point2 right() const { + return Point2(uR_, v_); + } - /// @} - /// @name Standard Interface - /// @{ + /// @name Deprecated + /// @{ + inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);} + inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;} + inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; } + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); } + static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); } + /// @} - /// get uL - inline double uL() const {return uL_;} + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); - /// get uR - inline double uR() const {return uR_;} +private: - /// get v - inline double v() const {return v_;} + /// @} + /// @name Advanced Interface + /// @{ - /** convert to vector */ - Vector3 vector() const { - return Vector3(uL_, uR_, v_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(uL_); + ar & BOOST_SERIALIZATION_NVP(uR_); + ar & BOOST_SERIALIZATION_NVP(v_); + } - /** convenient function to get a Point2 from the left image */ - Point2 point2() const { - return Point2(uL_, v_); - } + /// @} - /** convenient function to get a Point2 from the right image */ - Point2 right() const { - return Point2(uR_, v_); - } +}; - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); +template<> +struct traits : public internal::VectorSpace {}; - private: - - /// @} - /// @name Advanced Interface - /// @{ - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(uL_); - ar & BOOST_SERIALIZATION_NVP(uR_); - ar & BOOST_SERIALIZATION_NVP(v_); - } - - /// @} - - }; - - template<> - struct traits : public internal::Manifold {}; - - template<> - struct traits : public internal::Manifold {}; +template<> +struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 5496b8aac..ddcc9238a 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) //****************************************************************************** TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); } /* ************************************************************************* */