testOrientedPlane3 works. Still have to change boost::optional to Optional Jacobian

gtsam/geometry/OrientedPlane3.cpp

@@ -40,7 +40,7 @@ void OrientedPlane3::print(const std::string& s) const {
   {
     Matrix n_hr;
     Matrix n_hp;
-    Sphere2 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, n_hp);
+    Unit3 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, n_hp);
     
     Vector n_unit = plane.n_.unitVector ();
     Vector unit_vec = n_rotated.unitVector ();
@@ -68,34 +68,40 @@ void OrientedPlane3::print(const std::string& s) const {
   }
   
 /* ************************************************************************* */
-  Vector OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const
+  Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const
   {
-    Vector n_error = -n_.localCoordinates (plane.n_, Sphere2::EXPMAP);
+    Vector2 n_error = -n_.localCoordinates (plane.n_);
     double d_error = d_ - plane.d_;
-    return (Vector (3) << n_error (0), n_error (1), d_error);
+    Vector3 e;
+    e << n_error (0), n_error (1), d_error;
+    return (e);
   }
 
 /* ************************************************************************* */
 OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
-  // Retract the Sphere2
+  // Retract the Unit3
   Vector2 n_v (v (0), v (1));
-  Sphere2 n_retracted = n_.retract (n_v, Sphere2::EXPMAP);
+  Unit3 n_retracted = n_.retract (n_v);
   double d_retracted = d_ + v (2);
   return OrientedPlane3 (n_retracted, d_retracted);
 }
 
 /* ************************************************************************* */
-Vector OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
+Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
-  Vector n_local = n_.localCoordinates (y.n_, Sphere2::EXPMAP);
+  Vector n_local = n_.localCoordinates (y.n_);
   double d_local = d_ - y.d_;
-  return Vector (3) << n_local (0), n_local (1), -d_local;
+  Vector3 e;
+  e << n_local (0), n_local (1), -d_local;
+  return e;
 }
 
 /* ************************************************************************* */
-  Vector OrientedPlane3::planeCoefficients () const 
+  Vector3 OrientedPlane3::planeCoefficients () const
   {
     Vector unit_vec = n_.unitVector ();
-    return Vector (4) << unit_vec[0], unit_vec[1], unit_vec[2], d_;
+    Vector3 a;
+    a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
+    return a;
   }
 
 /* ************************************************************************* */

gtsam/geometry/OrientedPlane3.h

@@ -20,7 +20,7 @@
 #pragma once
 
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/geometry/Sphere2.h>
+#include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/DerivedValue.h>
 
@@ -31,11 +31,11 @@ class OrientedPlane3: public DerivedValue<OrientedPlane3> {
 
 private:
 
-    Sphere2 n_; /// The direction of the planar normal
+    Unit3 n_; /// The direction of the planar normal
     double d_; /// The perpendicular distance to this plane
 
 public:
-
+  enum { dimension = 3 };
   /// @name Constructors
   /// @{
 
@@ -45,8 +45,8 @@ public:
     d_ (0.0){
   }
 
-  /// Construct from a Sphere2 and a distance
+  /// Construct from a Unit3 and a distance
-    OrientedPlane3 (const Sphere2& s, double d)
+    OrientedPlane3 (const Unit3& s, double d)
       : n_ (s),
         d_ (d)
     {}    
@@ -61,7 +61,7 @@ public:
   /// Construct from a, b, c, d
   OrientedPlane3(double a, double b, double c, double d) {
     Point3 p (a, b, c);
-    n_ = Sphere2(p);
+    n_ = Unit3(p);
     d_ = d;
   }
 
@@ -80,7 +80,7 @@ public:
                             boost::optional<Matrix&> Hp);
 
   /// Computes the error between two poses
-  Vector error (const gtsam::OrientedPlane3& plane) const;
+  Vector3 error (const gtsam::OrientedPlane3& plane) const;
 
   /// Dimensionality of tangent space = 3 DOF
   inline static size_t Dim() {
@@ -96,17 +96,21 @@ public:
   OrientedPlane3 retract(const Vector& v) const;
 
   /// The local coordinates function
-  Vector localCoordinates(const OrientedPlane3& s) const;
+  Vector3 localCoordinates(const OrientedPlane3& s) const;
 
   /// Returns the plane coefficients (a, b, c, d)
-  Vector planeCoefficients () const;
+  Vector3 planeCoefficients () const;
 
-  inline Sphere2 normal () const {
+  inline Unit3 normal () const {
     return n_;
   }
     
   /// @}
 };
 
+template <> struct traits<OrientedPlane3> : public internal::Manifold<OrientedPlane3> {};
+
+template <> struct traits<const OrientedPlane3> : public internal::Manifold<OrientedPlane3> {};
+
 } // namespace gtsam
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -16,7 +16,7 @@
  * @brief Tests the OrientedPlane3 class
  */
 
-#include <gtsam/geometry/Sphere2.h>
+#include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/geometry/Pose3.h>
@@ -68,7 +68,7 @@ TEST (OrientedPlane3, transform)
  }
 
 //*******************************************************************************
-/// Returns a random vector -- copied from testSphere2.cpp
+/// Returns a random vector -- copied from testUnit3.cpp
 inline static Vector randomVector(const Vector& minLimits,
     const Vector& maxLimits) {
 
@@ -88,9 +88,13 @@ inline static Vector randomVector(const Vector& minLimits,
 TEST(OrientedPlane3, localCoordinates_retract) {
   
   size_t numIterations = 10000;
-  Vector minPlaneLimit = Vector_(4, -1.0, -1.0, -1.0, 0.01), maxPlaneLimit =
+  Vector minPlaneLimit, maxPlaneLimit;
-      Vector_(4, 1.0, 1.0, 10.0);
+  minPlaneLimit << 4, -1.0, -1.0, -1.0, 0.01;
-  Vector minXiLimit = Vector_(3, -M_PI, -M_PI, -10.0), maxXiLimit = Vector_(3, M_PI, M_PI, 10.0);
+  maxPlaneLimit << 4, 1.0, 1.0, 10.0;
+
+  Vector minXiLimit,maxXiLimit;
+  minXiLimit <<  -M_PI, -M_PI, -10.0;
+  maxXiLimit <<   M_PI, M_PI, 10.0;
   for (size_t i = 0; i < numIterations; i++) {
 
     sleep(0);