Formatting

gtsam/nonlinear/NonlinearEquality.h

@@ -77,11 +77,12 @@ namespace gtsam {
    */
   bool (*compare_)(const T& a, const T& b);
 
-
   /** default constructor - only for serialization */
-    NonlinearEquality() {}
+  NonlinearEquality() {
+  }
 
-    virtual ~NonlinearEquality() {}
+  virtual ~NonlinearEquality() {
+  }
 
   /// @name Standard Constructors
   /// @{
@@ -89,26 +90,28 @@ namespace gtsam {
   /**
    * Constructor - forces exact evaluation
    */
-    NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
+  NonlinearEquality(Key j, const T& feasible,
-      Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
+      bool (*_compare)(const T&, const T&) = compare<T>) :
-      allow_error_(false), error_gain_(0.0),
+      Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
-      compare_(_compare) {
+          feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) {
   }
 
   /**
    * Constructor - allows inexact evaluation
    */
-    NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
+  NonlinearEquality(Key j, const T& feasible, double error_gain,
-      Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
+      bool (*_compare)(const T&, const T&) = compare<T>) :
-      allow_error_(true), error_gain_(error_gain),
+      Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(
-      compare_(_compare) {
+          feasible), allow_error_(true), error_gain_(error_gain), compare_(
+          _compare) {
   }
 
   /// @}
   /// @name Testable
   /// @{
 
-    virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  virtual void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
     gtsam::print(feasible_, "Feasible Point:\n");
     std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
@@ -117,8 +120,8 @@ namespace gtsam {
   /** Check if two factors are equal */
   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This* e = dynamic_cast<const This*>(&f);
-      return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
+    return e && Base::equals(f) && feasible_.equals(e->feasible_, tol)
-          fabs(error_gain_ - e->error_gain_) < tol;
+        && fabs(error_gain_ - e->error_gain_) < tol;
   }
 
   /// @}
@@ -137,17 +140,22 @@ namespace gtsam {
   }
 
   /** error function */
-    Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const T& xj,
+      boost::optional<Matrix&> H = boost::none) const {
     size_t nj = feasible_.dim();
     if (allow_error_) {
-        if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
+      if (H)
+        *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
       return xj.localCoordinates(feasible_);
     } else if (compare_(feasible_, xj)) {
-        if (H) *H = eye(nj);
+      if (H)
+        *H = eye(nj);
       return zero(nj); // set error to zero if equal
     } else {
-        if (H) throw std::invalid_argument(
+      if (H)
-            "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!");
+        throw std::invalid_argument(
+            "Linearization point not feasible for "
+                + DefaultKeyFormatter(this->key()) + "!");
       return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
     }
   }
@@ -158,13 +166,15 @@ namespace gtsam {
     Matrix A;
     Vector b = evaluateError(xj, A);
     SharedDiagonal model = noiseModel::Constrained::All(b.size());
-      return GaussianFactor::shared_ptr(new JacobianFactor(this->key(), A, b, model));
+    return GaussianFactor::shared_ptr(
+        new JacobianFactor(this->key(), A, b, model));
   }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
 
   /// @}
 
@@ -174,14 +184,16 @@ namespace gtsam {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor1",
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(feasible_);
     ar & BOOST_SERIALIZATION_NVP(allow_error_);
     ar & BOOST_SERIALIZATION_NVP(error_gain_);
   }
 
-  }; // \class NonlinearEquality
+};
+// \class NonlinearEquality
 
 /* ************************************************************************* */
 /**
@@ -198,39 +210,48 @@ namespace gtsam {
   typedef NonlinearEquality1<VALUE> This;
 
   /** default constructor to allow for serialization */
-    NonlinearEquality1() {}
+  NonlinearEquality1() {
+  }
 
   X value_; /// fixed value for variable
 
-    GTSAM_CONCEPT_MANIFOLD_TYPE(X);
+  GTSAM_CONCEPT_MANIFOLD_TYPE(X)
-    GTSAM_CONCEPT_TESTABLE_TYPE(X);
+  ;GTSAM_CONCEPT_TESTABLE_TYPE(X)
+  ;
 
 public:
 
   typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
 
   ///TODO: comment
-    NonlinearEquality1(const X& value, Key key1, double mu = 1000.0)
+  NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) :
-      : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
+      Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(
+          value) {
+  }
 
-    virtual ~NonlinearEquality1() {}
+  virtual ~NonlinearEquality1() {
+  }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
 
   /** g(x) with optional derivative */
-    Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const X& x1,
-      if (H) (*H) = eye(x1.dim());
+      boost::optional<Matrix&> H = boost::none) const {
+    if (H)
+      (*H) = eye(x1.dim());
     // manifold equivalent of h(x)-z -> log(z,h(x))
     return value_.localCoordinates(x1);
   }
 
   /** Print */
-    virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  virtual void print(const std::string& s = "",
-      std::cout << s << ": NonlinearEquality1("
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-          << keyFormatter(this->key()) << "),"<< "\n";
+    std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
+        << ")," << "\n";
     this->noiseModel_->print();
     value_.print("Value");
   }
@@ -241,11 +262,13 @@ namespace gtsam {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor1",
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(value_);
   }
-  }; // \NonlinearEquality1
+};
+// \NonlinearEquality1
 
 /* ************************************************************************* */
 /**
@@ -261,32 +284,38 @@ namespace gtsam {
   typedef NoiseModelFactor2<VALUE, VALUE> Base;
   typedef NonlinearEquality2<VALUE> This;
 
-    GTSAM_CONCEPT_MANIFOLD_TYPE(X);
+  GTSAM_CONCEPT_MANIFOLD_TYPE(X)
+  ;
 
   /** default constructor to allow for serialization */
-    NonlinearEquality2() {}
+  NonlinearEquality2() {
+  }
 
 public:
 
   typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
 
   ///TODO: comment
-    NonlinearEquality2(Key key1, Key key2, double mu = 1000.0)
+  NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) :
-      : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
+      Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {
-    virtual ~NonlinearEquality2() {}
+  }
+  virtual ~NonlinearEquality2() {
+  }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
 
   /** g(x) with optional derivative2 */
-    Vector evaluateError(const X& x1, const X& x2,
+  Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
-        boost::optional<Matrix&> H1 = boost::none,
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
-        boost::optional<Matrix&> H2 = boost::none) const {
     const size_t p = X::Dim();
-      if (H1) *H1 = -eye(p);
+    if (H1)
-      if (H2) *H2 = eye(p);
+      *H1 = -eye(p);
+    if (H2)
+      *H2 = eye(p);
     return x1.localCoordinates(x2);
   }
 
@@ -296,10 +325,12 @@ namespace gtsam {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor2",
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor2",
             boost::serialization::base_object<Base>(*this));
   }
-  }; // \NonlinearEquality2
+};
+// \NonlinearEquality2
 
 }// namespace gtsam
 

tests/testNonlinearEquality.cpp

@@ -44,7 +44,7 @@ typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
 
 static Symbol key('x', 1);
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST ( NonlinearEquality, linearization ) {
   Pose2 value = Pose2(2.1, 1.0, 2.0);
   Values linearize;
@@ -60,7 +60,7 @@ TEST ( NonlinearEquality, linearization ) {
   EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
 }
 
-/* ********************************************************************** */
+//******************************************************************************
 TEST ( NonlinearEquality, linearization_pose ) {
 
   Symbol key('x', 1);
@@ -75,7 +75,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
   EXPECT(true);
 }
 
-/* ********************************************************************** */
+//******************************************************************************
 TEST ( NonlinearEquality, linearization_fail ) {
   Pose2 value = Pose2(2.1, 1.0, 2.0);
   Pose2 wrong = Pose2(2.1, 3.0, 4.0);
@@ -89,12 +89,11 @@ TEST ( NonlinearEquality, linearization_fail ) {
   CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
 }
 
-/* ********************************************************************** */
+//******************************************************************************
 TEST ( NonlinearEquality, linearization_fail_pose ) {
 
   Symbol key('x', 1);
-  Pose2 value(2.0, 1.0, 2.0),
+  Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0);
-      wrong(2.0, 3.0, 4.0);
   Values bad_linearize;
   bad_linearize.insert(key, wrong);
 
@@ -105,12 +104,11 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
   CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
 }
 
-/* ********************************************************************** */
+//******************************************************************************
 TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
 
   Symbol key('x', 1);
-  Pose2 value,
+  Pose2 value, wrong(2.0, 3.0, 4.0);
-      wrong(2.0, 3.0, 4.0);
   Values bad_linearize;
   bad_linearize.insert(key, wrong);
 
@@ -121,7 +119,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
   CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument);
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST ( NonlinearEquality, error ) {
   Pose2 value = Pose2(2.1, 1.0, 2.0);
   Pose2 wrong = Pose2(2.1, 3.0, 4.0);
@@ -137,10 +135,11 @@ TEST ( NonlinearEquality, error ) {
   EXPECT(assert_equal(actual, zero(3)));
 
   actual = nle->unwhitenedError(bad_linearize);
-  EXPECT(assert_equal(actual, repeat(3, std::numeric_limits<double>::infinity())));
+  EXPECT(
+      assert_equal(actual, repeat(3, std::numeric_limits<double>::infinity())));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST ( NonlinearEquality, equals ) {
   Pose2 value1 = Pose2(2.1, 1.0, 2.0);
   Pose2 value2 = Pose2(2.1, 3.0, 4.0);
@@ -151,12 +150,15 @@ TEST ( NonlinearEquality, equals ) {
   shared_poseNLE nle3(new PoseNLE(key, value2));
 
   // verify
-  EXPECT(nle1->equals(*nle2));  // basic equality = true
+  EXPECT(nle1->equals(*nle2));
-  EXPECT(nle2->equals(*nle1));  // test symmetry of equals()
+  // basic equality = true
-  EXPECT(!nle1->equals(*nle3)); // test config
+  EXPECT(nle2->equals(*nle1));
+  // test symmetry of equals()
+  EXPECT(!nle1->equals(*nle3));
+  // test config
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST ( NonlinearEquality, allow_error_pose ) {
   Symbol key1('x', 1);
   Pose2 feasible1(1.0, 2.0, 3.0);
@@ -180,11 +182,12 @@ TEST ( NonlinearEquality, allow_error_pose ) {
   Matrix A1 = eye(3, 3);
   Vector b = expVec;
   SharedDiagonal model = noiseModel::Constrained::All(3);
-  GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(key1, A1, b, model));
+  GaussianFactor::shared_ptr expLinFactor(
+      new JacobianFactor(key1, A1, b, model));
   EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST ( NonlinearEquality, allow_error_optimize ) {
   Symbol key1('x', 1);
   Pose2 feasible1(1.0, 2.0, 3.0);
@@ -211,7 +214,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
   EXPECT(assert_equal(expected, result));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 
   // create a hard constraint
@@ -245,11 +248,11 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
   EXPECT(assert_equal(expected, actual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 static SharedDiagonal hard_model = noiseModel::Constrained::All(2);
 static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST( testNonlinearEqualityConstraint, unary_basics ) {
   Point2 pt(1.0, 2.0);
   Symbol key1('x', 1);
@@ -267,12 +270,14 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
   Point2 ptBad1(2.0, 2.0);
   config2.insert(key, ptBad1);
   EXPECT(constraint.active(config2));
-  EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol));
+  EXPECT(
-  EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol));
+      assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol));
+  EXPECT(
+      assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol));
   EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol);
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST( testNonlinearEqualityConstraint, unary_linearization ) {
   Point2 pt(1.0, 2.0);
   Symbol key1('x', 1);
@@ -282,18 +287,20 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
   Values config1;
   config1.insert(key, pt);
   GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
-  GaussianFactor::shared_ptr expected1(new JacobianFactor(key, eye(2,2), zero(2), hard_model));
+  GaussianFactor::shared_ptr expected1(
+      new JacobianFactor(key, eye(2, 2), zero(2), hard_model));
   EXPECT(assert_equal(*expected1, *actual1, tol));
 
   Values config2;
   Point2 ptBad(2.0, 2.0);
   config2.insert(key, ptBad);
   GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
-  GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), Vector2(-1.0,0.0), hard_model));
+  GaussianFactor::shared_ptr expected2(
+      new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model));
   EXPECT(assert_equal(*expected2, *actual2, tol));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
   // create a single-node graph with a soft and hard constraint to
   // ensure that the hard constraint overrides the soft constraint
@@ -326,7 +333,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
   EXPECT(assert_equal(expected, actual, tol));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST( testNonlinearEqualityConstraint, odo_basics ) {
   Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
   Symbol key1('x', 1), key2('x', 2);
@@ -347,12 +354,14 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
   config2.insert(key1, x1bad);
   config2.insert(key2, x2bad);
   EXPECT(constraint.active(config2));
-  EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
+  EXPECT(
-  EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol));
+      assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
+  EXPECT(
+      assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol));
   EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST( testNonlinearEqualityConstraint, odo_linearization ) {
   Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
   Symbol key1('x', 1), key2('x', 2);
@@ -364,8 +373,8 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
   config1.insert(key2, x2);
   GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
   GaussianFactor::shared_ptr expected1(
-      new JacobianFactor(key1, -eye(2,2), key2,
+      new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2),
-          eye(2,2), zero(2), hard_model));
+          hard_model));
   EXPECT(assert_equal(*expected1, *actual1, tol));
 
   Values config2;
@@ -375,12 +384,12 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
   config2.insert(key2, x2bad);
   GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
   GaussianFactor::shared_ptr expected2(
-      new JacobianFactor(key1, -eye(2,2), key2,
+      new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0),
-          eye(2,2), Vector2(1.0, 1.0), hard_model));
+          hard_model));
   EXPECT(assert_equal(*expected2, *actual2, tol));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
   // create a two-node graph, connected by an odometry constraint, with
   // a hard prior on one variable, and a conflicting soft prior
@@ -399,8 +408,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
 
   // odometry constraint
   eq2D::OdoEqualityConstraint::shared_ptr constraint2(
-      new eq2D::OdoEqualityConstraint(
+      new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1,
-          truth_pt1.between(truth_pt2), key1, key2));
+          key2));
 
   NonlinearFactorGraph graph;
   graph.push_back(constraint1);
@@ -418,7 +427,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
   CHECK(assert_equal(expected, actual, tol));
 }
 
-/* ********************************************************************* */
+//******************************************************************************
 TEST (testNonlinearEqualityConstraint, two_pose ) {
   /*
    * Determining a ground truth linear system
@@ -430,8 +439,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
 
   Symbol x1('x', 1), x2('x', 2);
   Symbol l1('l', 1), l2('l', 2);
-  Point2 pt_x1(1.0, 1.0),
+  Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0);
-       pt_x2(5.0, 6.0);
   graph += eq2D::UnaryEqualityConstraint(pt_x1, x1);
   graph += eq2D::UnaryEqualityConstraint(pt_x2, x2);
 
@@ -450,7 +458,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
   initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth
   initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
 
-  Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
+  Values actual =
+      LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
 
   Values expected;
   expected.insert(x1, pt_x1);
@@ -460,7 +469,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
   CHECK(assert_equal(expected, actual, 1e-5));
 }
 
-/* ********************************************************************* */
+//******************************************************************************
 TEST (testNonlinearEqualityConstraint, map_warp ) {
   // get a graph
   NonlinearFactorGraph graph;
@@ -494,7 +503,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
   initialEstimate.insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
 
   // optimize
-  Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
+  Values actual =
+      LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
 
   Values expected;
   expected.insert(x1, Point2(1.0, 1.0));
@@ -516,14 +526,12 @@ typedef NonlinearFactorGraph VGraph;
 // factors for visual slam
 typedef NonlinearEquality2<Point3> Point3Equality;
 
-/* ********************************************************************* */
+//******************************************************************************
 TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 
   // create initial estimates
-  Rot3 faceDownY((Matrix)(Matrix(3,3) <<
+  Rot3 faceDownY(
-      1.0, 0.0, 0.0,
+      (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished());
-      0.0, 0.0, 1.0,
-      0.0, -1.0, 0.0).finished());
   Pose3 pose1(faceDownY, Point3()); // origin, left camera
   SimpleCamera camera1(pose1, K);
   Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
@@ -543,8 +551,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
 
   // create  factors
   SharedDiagonal vmodel = noiseModel::Unit::Create(2);
-  graph += GenericProjectionFactor<Pose3,Point3,Cal3_S2>(camera1.project(landmark), vmodel, x1, l1, shK);
+  graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
-  graph += GenericProjectionFactor<Pose3,Point3,Cal3_S2>(camera2.project(landmark), vmodel, x2, l2, shK);
+      camera1.project(landmark), vmodel, x1, l1, shK);
+  graph += GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
+      camera2.project(landmark), vmodel, x2, l2, shK);
 
   // add equality constraint
   graph += Point3Equality(l1, l2);
@@ -573,6 +583,9 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
   CHECK(assert_equal(truthValues, actual, 1e-5));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
-/* ************************************************************************* */
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************