BORG formatting

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -24,32 +24,38 @@
 using namespace boost::assign;
 using namespace gtsam;
 using namespace std;
+using boost::none;
 
 GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
 GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
 
 //*******************************************************************************
-TEST (OrientedPlane3, transform)
+TEST (OrientedPlane3, transform) {
-{
   // 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 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,
+      none, none);
   EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9));
 
   // Test the jacobians of transform
   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, none, none), pose);
 
-    OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none);
+    OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
+        none);
     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, none, none), plane);
 
-    OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2);
+    OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
+        actualH2);
     EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
   }
 

gtsam/slam/OrientedPlane3Factor.cpp

@@ -5,7 +5,6 @@
  *      Author: Natesh Srinivasan
  */
 
-
 #include "OrientedPlane3Factor.h"
 
 using namespace std;
@@ -32,7 +31,8 @@ void OrientedPlane3DirectionPrior::print(const string& s,
 bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
     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);
+  return e != NULL && Base::equals(*e, tol)
+      && this->measured_p_.equals(e->measured_p_, tol);
 }
 
 //***************************************************************************

gtsam/slam/OrientedPlane3Factor.h

@@ -31,18 +31,14 @@ protected:
 public:
 
   /// Constructor
-  OrientedPlane3Factor ()
+  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& pose, const Symbol& landmark) :
-                        const Symbol& landmark)
+      Base(noiseModel, pose, landmark), poseSymbol_(pose), landmarkSymbol_(
-    : Base (noiseModel, pose, landmark),
+          landmark), measured_coeffs_(z) {
-      poseSymbol_ (pose),
-      landmarkSymbol_ (landmark),
-      measured_coeffs_ (z)
-  {
     measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
   }
 
@@ -52,14 +48,15 @@ public:
 
   /// evaluateError
   virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
-                               boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-                                boost::optional<Matrix&> H2 = boost::none) const
+          boost::none) const {
-  {
+    OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
-    OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2);
+        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
@@ -72,15 +69,13 @@ public:
 
   typedef OrientedPlane3DirectionPrior This;
   /// Constructor
-  OrientedPlane3DirectionPrior ()
+  OrientedPlane3DirectionPrior() {
-  {}
+  }
 
   /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
   OrientedPlane3DirectionPrior(Key key, const Vector&z,
-                                const SharedGaussian& noiseModel)
+      const SharedGaussian& noiseModel) :
-    : Base (noiseModel, key),
+      Base(noiseModel, key), landmarkKey_(key) {
-      landmarkKey_ (key)
-  {
     measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
   }
 

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -34,8 +34,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
 GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
 
 // *************************************************************************
-TEST (OrientedPlane3Factor, lm_translation_error)
+TEST (OrientedPlane3Factor, lm_translation_error) {
-{
   // Tests one pose, two measurements of the landmark that differ in range only.
   // Normal along -x, 3m away
   gtsam::Symbol lm_sym('p', 0);
@@ -51,7 +50,8 @@ TEST (OrientedPlane3Factor, lm_translation_error)
       gtsam::Point3(0.0, 0.0, 0.0));
   gtsam::Vector sigmas(6);
   sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
-  gtsam::PriorFactor<gtsam::Pose3> pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas (sigmas) );
+  gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
+      gtsam::noiseModel::Diagonal::Sigmas(sigmas));
   new_values.insert(init_sym, init_pose);
   new_graph.add(pose_prior);
 
@@ -61,17 +61,20 @@ TEST (OrientedPlane3Factor, lm_translation_error)
   sigmas3 << 0.1, 0.1, 0.1;
   gtsam::Vector test_meas0_mean(4);
   test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
-  gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym);
+  gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
+      gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
   new_graph.add(test_meas0);
   gtsam::Vector test_meas1_mean(4);
   test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
-  gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym);
+  gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
+      gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
   new_graph.add(test_meas1);
 
   // Optimize
   gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
   gtsam::Values result_values = isam2.calculateEstimate();
-  gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym);
+  gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
+      gtsam::OrientedPlane3>(lm_sym);
 
   // Given two noisy measurements of equal weight, expect result between the two
   gtsam::OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
@@ -79,8 +82,7 @@ TEST (OrientedPlane3Factor, lm_translation_error)
 }
 
 // *************************************************************************
-TEST (OrientedPlane3Factor, lm_rotation_error)
+TEST (OrientedPlane3Factor, lm_rotation_error) {
-{
   // Tests one pose, two measurements of the landmark that differ in angle only.
   // Normal along -x, 3m away
   gtsam::Symbol lm_sym('p', 0);
@@ -94,7 +96,9 @@ TEST (OrientedPlane3Factor, lm_rotation_error)
   gtsam::Symbol init_sym('x', 0);
   gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0),
       gtsam::Point3(0.0, 0.0, 0.0));
-  gtsam::PriorFactor<gtsam::Pose3> pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas ((Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
+  gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
+      gtsam::noiseModel::Diagonal::Sigmas(
+          (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
   new_values.insert(init_sym, init_pose);
   new_graph.add(pose_prior);
 
@@ -102,20 +106,26 @@ TEST (OrientedPlane3Factor, lm_rotation_error)
   new_values.insert(lm_sym, test_lm0);
   Vector test_meas0_mean(4);
   test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
-  gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym);
+  gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
+      gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
+      lm_sym);
   new_graph.add(test_meas0);
   Vector test_meas1_mean(4);
   test_meas1_mean << 0.0, -1.0, 0.0, 3.0;
-  gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym);
+  gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
+      gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
+      lm_sym);
   new_graph.add(test_meas1);
 
   // Optimize
   gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
   gtsam::Values result_values = isam2.calculateEstimate();
-  gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym);
+  gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
+      gtsam::OrientedPlane3>(lm_sym);
 
   // Given two noisy measurements of equal weight, expect result between the two
-  gtsam::OrientedPlane3 expected_plane_landmark (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0);
+  gtsam::OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0,
+      -sqrt(2.0) / 2.0, 0.0, 3.0);
   EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
 }
 
@@ -130,7 +140,8 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
 
   // Factor
   Key key(1);
-  SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0));
+  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
@@ -138,21 +149,22 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
   Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0);
   Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0);
 
-
   OrientedPlane3 T1(theta1);
   OrientedPlane3 T2(theta2);
   OrientedPlane3 T3(theta3);
 
-
   // Calculate numerical derivatives
   Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
-      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1);
+      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
+          boost::none), T1);
 
   Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
-      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2);
+      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
+          boost::none), T2);
 
   Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
-      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3);
+      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
+          boost::none), T3);
 
   // Use the factor to calculate the derivative
   Matrix actualH1, actualH2, actualH3;