Made StereoPoint2 a vector space just like Point2, removed old-style dimensions, and fixed indentation

gtsam/geometry/StereoPoint2.h

@@ -18,8 +18,9 @@
 
 #pragma once
 
-#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/VectorSpace.h>
 #include <gtsam/geometry/Point2.h>
+#include <boost/serialization/nvp.hpp>
 
 namespace gtsam {
 
@@ -29,13 +30,12 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT StereoPoint2 {
-  public:
-    static const size_t dimension = 3;
 private:
+
   double uL_, uR_, v_;
 
 public:
-
+  enum { dimension = 3 };
   /// @name Standard Constructors
   /// @{
 
@@ -49,6 +49,10 @@ namespace gtsam {
       uL_(uL), uR_(uR), v_(v) {
   }
 
+  /// construct from 3D vector
+  StereoPoint2(const Vector3& v) :
+    uL_(v(0)), uR_(v(1)), v_(v(2)) {}
+
   /// @}
   /// @name Testable
   /// @{
@@ -58,8 +62,8 @@ namespace gtsam {
 
   /** equals */
   bool equals(const StereoPoint2& q, double tol = 1e-9) const {
-      return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_
+    return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol
-          - q.v_) < tol);
+        && fabs(v_ - q.v_) < tol);
   }
 
   /// @}
@@ -67,67 +71,38 @@ namespace gtsam {
   /// @{
 
   /// identity
-    inline static StereoPoint2 identity() { return StereoPoint2(); }
+  inline static StereoPoint2 identity() {
+    return StereoPoint2();
+  }
 
   /// inverse
-    inline StereoPoint2 inverse() const {
+  StereoPoint2 operator-() const {
-      return StereoPoint2()- (*this);
+    return StereoPoint2(-uL_, -uR_, -v_);
   }
 
-    /// "Compose", just adds the coordinates of two points.
+  /// add
-    inline StereoPoint2 compose(const StereoPoint2& p1) const {
+  inline StereoPoint2 operator +(const StereoPoint2& b) const {
-      return *this + p1;
-    }
-
-    /// add two stereo points
-    StereoPoint2 operator+(const StereoPoint2& b) const {
     return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
   }
 
-    /// subtract two stereo points
+  /// subtract
-    StereoPoint2 operator-(const StereoPoint2& b) const {
+  inline StereoPoint2 operator -(const StereoPoint2& b) const {
     return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
   }
 
   /// @}
-    /// @name Manifold
+  /// @name Vector Space
   /// @{
 
-    /// dimension of the variable - used to autodetect sizes */
+  // unit, norm, and distance don't really make sense for StereoPoint2
-    inline static size_t Dim() { return dimension; }
-
-    /// return dimensionality of tangent space, DOF = 3
-    inline size_t dim() const { return dimension; }
-
-    /// Updates a with tangent space delta
-    inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
-
-    /// Returns inverse retraction
-    inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
-
-    /// @}
-    /// @name Lie Group
-    /// @{
-
-    /** 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));
-    }
-
-    /** Log map around identity - just return the Point2 as a vector */
-    static inline Vector Logmap(const StereoPoint2& p) {
-      return p.vector();
-    }
-
-    /** The difference between another point and this point */
-    inline StereoPoint2 between(const StereoPoint2& p2) const {
-      return p2 - *this;
-    }
 
   /// @}
   /// @name Standard Interface
   /// @{
 
+  /// equality
+  inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;}
+
   /// get uL
   inline double uL() const {return uL_;}
 
@@ -152,6 +127,17 @@ namespace gtsam {
     return Point2(uR_, v_);
   }
 
+  /// @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)); }
+  /// @}
+
   /// Streaming
   GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
 
@@ -175,8 +161,8 @@ namespace gtsam {
 };
 
 template<>
-  struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
+struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
 
 template<>
-  struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
+struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
 }

gtsam/geometry/tests/testStereoPoint2.cpp

@@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
 
 //******************************************************************************
 TEST(StereoPoint2 , Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<StereoPoint2>));
   BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
+  BOOST_CONCEPT_ASSERT((IsVectorSpace<StereoPoint2>));
 }
 
 /* ************************************************************************* */