fixed formatting issues

gtsam/geometry/OrientedPlane3.cpp

@@ -33,49 +33,48 @@ void OrientedPlane3::print(const std::string& s) const {
 
 
 /* ************************************************************************* */
-  OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane,
+OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane,
                             const gtsam::Pose3& xr,
                             OptionalJacobian<3, 6> Hr,
-                            OptionalJacobian<3, 3> Hp)
+                            OptionalJacobian<3, 3> Hp) {
-  {
+  Matrix n_hr;
-    Matrix n_hr;
+  Matrix n_hp;
-    Matrix n_hp;
+  Unit3 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 ();
-    double pred_d = n_unit.dot (xr.translation ().vector ()) + plane.d_;
-    OrientedPlane3 transformed_plane (unit_vec (0), unit_vec (1), unit_vec (2), pred_d);
 
-    if (Hr)
+  Vector n_unit = plane.n_.unitVector ();
-    {
+  Vector unit_vec = n_rotated.unitVector ();
-      *Hr = gtsam::zeros (3, 6);
+  double pred_d = n_unit.dot (xr.translation ().vector ()) + plane.d_;
-      (*Hr).block<2,3> (0,0) = n_hr;
+  OrientedPlane3 transformed_plane (unit_vec (0), unit_vec (1), unit_vec (2), pred_d);
-      (*Hr).block<1,3> (2,3) = unit_vec;
+
-    }
+  if (Hr)
-    if (Hp)
+  {
-    {
+    *Hr = gtsam::zeros (3, 6);
-      Vector xrp = xr.translation ().vector ();
+    (*Hr).block<2,3> (0,0) = n_hr;
-      Matrix n_basis = plane.n_.basis();
+    (*Hr).block<1,3> (2,3) = unit_vec;
-      Vector hpp = n_basis.transpose() * xrp;
-      *Hp = gtsam::zeros (3,3);
-      (*Hp).block<2,2> (0,0) = n_hp;
-      (*Hp).block<1,2> (2,0) = hpp;
-      (*Hp) (2,2) = 1;
-    }
-    
-    return (transformed_plane);
   }
+  if (Hp)
+  {
+    Vector xrp = xr.translation ().vector ();
+    Matrix n_basis = plane.n_.basis();
+    Vector hpp = n_basis.transpose() * xrp;
+    *Hp = gtsam::zeros (3,3);
+    (*Hp).block<2,2> (0,0) = n_hp;
+    (*Hp).block<1,2> (2,0) = hpp;
+    (*Hp) (2,2) = 1;
+  }
+    
+  return (transformed_plane);
+}
   
 /* ************************************************************************* */
-  Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const
+Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const
-  {
+{
-    Vector2 n_error = -n_.localCoordinates (plane.n_);
+  Vector2 n_error = -n_.localCoordinates (plane.n_);
-    double d_error = d_ - plane.d_;
+  double d_error = d_ - plane.d_;
-    Vector3 e;
+  Vector3 e;
-    e << n_error (0), n_error (1), d_error;
+  e << n_error (0), n_error (1), d_error;
-    return (e);
+  return (e);
-  }
+}
 
 /* ************************************************************************* */
 OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
@@ -96,13 +95,13 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
 }
 
 /* ************************************************************************* */
-  Vector3 OrientedPlane3::planeCoefficients () const
+Vector3 OrientedPlane3::planeCoefficients () const
-  {
+{
-    Vector unit_vec = n_.unitVector ();
+  Vector unit_vec = n_.unitVector ();
-    Vector3 a;
+  Vector3 a;
-    a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
+  a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
-    return a;
+  return a;
-  }
+}
 
 /* ************************************************************************* */
 

gtsam/geometry/OrientedPlane3.h

@@ -31,8 +31,8 @@ class OrientedPlane3: public DerivedValue<OrientedPlane3> {
 
 private:
 
-    Unit3 n_; /// The direction of the planar normal
+  Unit3 n_; /// The direction of the planar normal
-    double d_; /// The perpendicular distance to this plane
+  double d_; /// The perpendicular distance to this plane
 
 public:
   enum { dimension = 3 };
@@ -46,16 +46,16 @@ public:
   }
 
   /// Construct from a Unit3 and a distance
-    OrientedPlane3 (const Unit3& s, double d)
+  OrientedPlane3 (const Unit3& s, double d)
-      : n_ (s),
+    : n_ (s),
-        d_ (d)
+      d_ (d)
-    {}    
+  {}
   
-    OrientedPlane3 (Vector vec) 
+  OrientedPlane3 (Vector vec)
-      : n_ (vec (0), vec (1), vec (2)),
+    : n_ (vec (0), vec (1), vec (2)),
-        d_ (vec (3))
+      d_ (vec (3))
-    {
+  {
-    }
+  }
   
 
   /// Construct from a, b, c, d
@@ -75,9 +75,9 @@ public:
 
   /// Transforms a plane to the specified pose
   static OrientedPlane3 Transform (const gtsam::OrientedPlane3& plane,
-                            const gtsam::Pose3& xr,
+                                   const gtsam::Pose3& xr,
-                            OptionalJacobian<3, 6> Hr = boost::none,
+                                   OptionalJacobian<3, 6> Hr = boost::none,
-                            OptionalJacobian<3, 3> Hp = boost::none);
+                                   OptionalJacobian<3, 3> Hp = boost::none);
 
   /// Computes the error between two poses
   Vector3 error (const gtsam::OrientedPlane3& plane) const;
@@ -104,7 +104,7 @@ public:
   inline Unit3 normal () const {
     return n_;
   }
-    
+
   /// @}
 };
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -42,30 +42,30 @@ GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
 
 //*******************************************************************************
 TEST (OrientedPlane3, transform)
- {
+{
-   // Test transforming a plane to a pose
+  // Test transforming a plane to a pose
-   gtsam::Pose3 pose(gtsam::Rot3::ypr (-M_PI/4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0));
+  gtsam::Pose3 pose(gtsam::Rot3::ypr (-M_PI/4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0));
-   OrientedPlane3 plane (-1 , 0, 0, 5);
+  OrientedPlane3 plane (-1 , 0, 0, 5);
-   OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3);
+  OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3);
-   OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none);
+  OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none);
-   EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9));
+  EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9));
 
-   // Test the jacobians of transform
+  // Test the jacobians of transform
-   Matrix actualH1, expectedH1, actualH2, expectedH2;
+  Matrix actualH1, expectedH1, actualH2, expectedH2;
-   {
+  {
-      expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose);
+    expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose);
 
-      OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none);
+    OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none);
-      EXPECT (assert_equal (expectedH1, actualH1, 1e-9));
+    EXPECT (assert_equal (expectedH1, actualH1, 1e-9));
-   }
+  }
-   {
+  {
-     expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3> (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane);
+    expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3> (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane);
-     
+
-     OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2);
+    OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2);
-     EXPECT (assert_equal (expectedH2, actualH2, 1e-9));
+    EXPECT (assert_equal (expectedH2, actualH2, 1e-9));
-   }
+  }
-                                                    
+
- }
+}
 
 //*******************************************************************************
 // Returns a random vector -- copied from testUnit3.cpp
@@ -78,9 +78,9 @@ inline static Vector randomVector(const Vector& minLimits,
 
   // Create the random vector
   for (size_t i = 0; i < numDims; i++) {
-    double range = maxLimits(i) - minLimits(i);
+      double range = maxLimits(i) - minLimits(i);
-    vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
+      vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
-  }
+    }
   return vector;
 }
 
@@ -102,7 +102,7 @@ TEST(OrientedPlane3, localCoordinates_retract) {
     // Create a Plane
     OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit));
     Vector v12 = randomVector(minXiLimit, maxXiLimit);
-    
+
     // Magnitude of the rotation can be at most pi
     if (v12.segment<3>(0).norm () > M_PI)
       v12.segment<3>(0) = v12.segment<3>(0) / M_PI;

gtsam/slam/OrientedPlane3Factor.cpp

@@ -23,7 +23,7 @@ void OrientedPlane3DirectionPrior::print(const string& s) const {
 //***************************************************************************
 
 bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
-    double tol) const {
+  double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
   return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol);
 }
@@ -33,21 +33,21 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
 Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
     boost::optional<Matrix&> H) const {
 
-  if(H) {
+if(H) {
-    Matrix H_p;
+  Matrix H_p;
-    Unit3 n_hat_p = measured_p_.normal();
+  Unit3 n_hat_p = measured_p_.normal();
-    Unit3 n_hat_q = plane.normal();
+  Unit3 n_hat_q = plane.normal();
-    Vector e = n_hat_p.error(n_hat_q,H_p);
+  Vector e = n_hat_p.error(n_hat_q,H_p);
-    H->resize(2,3);
+  H->resize(2,3);
-    H->block <2,2>(0,0) << H_p;
+  H->block <2,2>(0,0) << H_p;
-    H->block <2,1>(0,2) << Matrix::Zero(2, 1);
+  H->block <2,1>(0,2) << Matrix::Zero(2, 1);
-    return e;
+  return e;
-  } else {
+} else {
-    Unit3 n_hat_p = measured_p_.normal();
+  Unit3 n_hat_p = measured_p_.normal();
-    Unit3 n_hat_q = plane.normal();
+  Unit3 n_hat_q = plane.normal();
-    Vector e = n_hat_p.error(n_hat_q);
+  Vector e = n_hat_p.error(n_hat_q);
-    return e;
+  return e;
-  }
+}
 
 }
 }

gtsam/slam/OrientedPlane3Factor.h

@@ -16,85 +16,80 @@
 
 namespace gtsam {
 
-/**
+  /**
  * Factor to measure a planar landmark from a given pose
  */
-  class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
+class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
 
-    protected:
+protected:
-      Symbol poseSymbol_;
+  Symbol poseSymbol_;
-      Symbol landmarkSymbol_;
+  Symbol landmarkSymbol_;
-      Vector3 measured_coeffs_;
+  Vector3 measured_coeffs_;
-      OrientedPlane3 measured_p_;
+  OrientedPlane3 measured_p_;
-      
-      typedef NoiseModelFactor2<Pose3, OrientedPlane3 > Base;
 
-    public:
+  typedef NoiseModelFactor2<Pose3, OrientedPlane3 > Base;
-      
-      /// Constructor
-      OrientedPlane3Factor ()
-      {}
-      
-      /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
-      OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel,
-                   const Symbol& pose,
-                   const Symbol& landmark)
-        : Base (noiseModel, pose, landmark),
-          poseSymbol_ (pose),
-          landmarkSymbol_ (landmark),
-          measured_coeffs_ (z)
-      {
-        measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
-      }
 
-      /// print
+public:
-      void print(const std::string& s="PlaneFactor") const;
+
-      
+  /// Constructor
-      virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
+  OrientedPlane3Factor ()
-                                   boost::optional<Matrix&> H1 = boost::none,
+  {}
-                                   boost::optional<Matrix&> H2 = boost::none) const 
+
-      {
+  /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
-        OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2);
+  OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel,
-        Vector err(3);
+                        const Symbol& pose,
-        err << predicted_plane.error (measured_p_);
+                        const Symbol& landmark)
-        return (err);
+    : Base (noiseModel, pose, landmark),
-      };
+      poseSymbol_ (pose),
+      landmarkSymbol_ (landmark),
+      measured_coeffs_ (z)
+  {
+    measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
+  }
+
+  /// print
+  void print(const std::string& s="PlaneFactor") const;
+  virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
+                               boost::optional<Matrix&> H1 = boost::none,
+                                boost::optional<Matrix&> H2 = boost::none) const
+  {
+    OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2);
+    Vector err(3);
+    err << predicted_plane.error (measured_p_);
+    return (err);
   };
+};
 
-  // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
+// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
-  class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
+class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
+protected:
+  OrientedPlane3 measured_p_; /// measured plane parameters
+  Key landmarkKey_;
+  typedef NoiseModelFactor1<OrientedPlane3 > Base;
+public:
 
-    protected:
+  typedef OrientedPlane3DirectionPrior This;
-      OrientedPlane3 measured_p_; /// measured plane parameters
+  /// Constructor
-      Key landmarkKey_;
+  OrientedPlane3DirectionPrior ()
+  {}
 
-      typedef NoiseModelFactor1<OrientedPlane3 > Base;
+  /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
+  OrientedPlane3DirectionPrior (Key key, const Vector&z,
+                                const SharedGaussian& noiseModel)
+    : Base (noiseModel, key),
+      landmarkKey_ (key)
+  {
+    measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
+  }
+  /// print
+  void print(const std::string& s) const;
 
-    public:
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
 
-      typedef OrientedPlane3DirectionPrior This;
+  virtual Vector evaluateError(const OrientedPlane3& plane,
-      /// Constructor
+                               boost::optional<Matrix&> H = boost::none) const;
-      OrientedPlane3DirectionPrior ()
+};
-      {}
-
-      /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
-      OrientedPlane3DirectionPrior (Key key, const Vector&z,
-          const SharedGaussian& noiseModel)
-        : Base (noiseModel, key),
-          landmarkKey_ (key)
-      {
-        measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
-      }
-
-      /// print
-      void print(const std::string& s) const;
-
-      /** equals */
-      virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-
-      virtual Vector evaluateError(const OrientedPlane3& plane,
-                                   boost::optional<Matrix&> H = boost::none) const;
-  };
 
 } // gtsam
 

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -141,7 +141,7 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) {
   SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0));
   OrientedPlane3DirectionPrior factor(key, planeOrientation, model);
 
-//   Create a linearization point at the zero-error point
+  // Create a linearization point at the zero-error point
   Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0);
   Vector theta2 = Vector4(0.0, 0.1, - 0.8, 10.0);
   Vector theta3 = Vector4(0.0, 0.2,  -0.9, 10.0);