BORG-style comments

gtsam/geometry/OrientedPlane3.cpp

@@ -31,11 +31,9 @@ void OrientedPlane3::print(const std::string& s) const {
   cout << s << " : " << coeffs << endl;
 }
 
-
 /* ************************************************************************* */
 OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane,
-                            const gtsam::Pose3& xr,
+    const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr,
-                            OptionalJacobian<3, 6> Hr,
     OptionalJacobian<3, 3> Hp) {
   Matrix n_hr;
   Matrix n_hp;
@@ -44,16 +42,15 @@ OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane,
   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);
+  OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2),
+      pred_d);
 
-  if (Hr)
+  if (Hr) {
-  {
     *Hr = gtsam::zeros(3, 6);
     (*Hr).block<2, 3>(0, 0) = n_hr;
     (*Hr).block<1, 3>(2, 3) = unit_vec;
   }
-  if (Hp)
+  if (Hp) {
-  {
     Vector xrp = xr.translation().vector();
     Matrix n_basis = plane.n_.basis();
     Vector hpp = n_basis.transpose() * xrp;
@@ -67,8 +64,7 @@ OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane,
 }
 
 /* ************************************************************************* */
-Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const
+Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
-{
   Vector2 n_error = -n_.localCoordinates(plane.n_);
   double d_error = d_ - plane.d_;
   Vector3 e;
@@ -95,8 +91,7 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
 }
 
 /* ************************************************************************* */
-Vector3 OrientedPlane3::planeCoefficients () const
+Vector3 OrientedPlane3::planeCoefficients() const {
-{
   Vector unit_vec = n_.unitVector();
   Vector3 a;
   a << unit_vec[0], unit_vec[1], unit_vec[2], d_;

gtsam/geometry/OrientedPlane3.h

@@ -35,28 +35,25 @@ private:
   double d_; /// The perpendicular distance to this plane
 
 public:
-  enum { dimension = 3 };
+  enum {
+    dimension = 3
+  };
   /// @name Constructors
   /// @{
 
   /// Default constructor
   OrientedPlane3() :
-    n_(),
+      n_(), d_(0.0) {
-    d_ (0.0){
   }
 
   /// 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)
-    : n_ (vec (0), vec (1), vec (2)),
-      d_ (vec (3))
-  {
   }
 
+  OrientedPlane3(Vector vec) :
+      n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
+  }
 
   /// Construct from a, b, c, d
   OrientedPlane3(double a, double b, double c, double d) {
@@ -75,8 +72,7 @@ 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);
 
   /// Computes the error between two poses
@@ -108,9 +104,13 @@ public:
   /// @}
 };
 
-template <> struct traits<OrientedPlane3> : public internal::Manifold<OrientedPlane3> {};
+template<> struct traits<OrientedPlane3> : public internal::Manifold<
+    OrientedPlane3> {
+};
 
-template <> struct traits<const OrientedPlane3> : public internal::Manifold<OrientedPlane3> {};
+template<> struct traits<const OrientedPlane3> : public internal::Manifold<
+    OrientedPlane3> {
+};
 
 } // namespace gtsam
 

gtsam/geometry/Unit3.h

@@ -38,7 +38,9 @@ private:
 
 public:
 
-  enum { dimension = 2 };
+  enum {
+    dimension = 2
+  };
 
   /// @name Constructors
   /// @{
@@ -65,8 +67,8 @@ public:
   }
 
   /// Named constructor from Point3 with optional Jacobian
-  static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
+  static Unit3 FromPoint3(const Point3& point, //
-      boost::none);
+      OptionalJacobian<2, 3> H = boost::none);
 
   /// Random direction, using boost::uniform_on_sphere
   static Unit3 Random(boost::mt19937 & rng);
@@ -118,12 +120,10 @@ public:
   }
 
   /// Signed, vector-valued error between two directions
-  Vector error(const Unit3& q,
+  Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
-      OptionalJacobian<2,2> H = boost::none) const;
 
   /// Distance between two directions
-  double distance(const Unit3& q,
+  double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
-      OptionalJacobian<1,2> H = boost::none) const;
 
   /// @}
 
@@ -176,9 +176,11 @@ private:
 };
 
 // Define GTSAM traits
-template <> struct traits<Unit3> : public internal::Manifold<Unit3> {};
+template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
+};
 
-template <> struct traits<const Unit3> : public internal::Manifold<Unit3> {};
+template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
+};
 
 } // namespace gtsam