[REFACTOR] Ran Eclipse Code Formatter on all Added files.

gtsam_unstable/linear/ActiveSetSolver.h

@@ -43,8 +43,7 @@ public:
    * Dual Jacobians used to build a dual factor graph.
    */
   template<typename FACTOR>
-  TermsContainer collectDualJacobians(Key key,
+  TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
-      const FactorGraph<FACTOR>& graph,
       const VariableIndex& variableIndex) const {
     /*
      * Iterates through each factor in the factor graph and checks 
@@ -53,43 +52,44 @@ public:
      */
     TermsContainer Aterms;
     if (variableIndex.find(key) != variableIndex.end()) {
-    for(size_t factorIx: variableIndex[key]) {
+      for (size_t factorIx : variableIndex[key]) {
-      typename FACTOR::shared_ptr factor = graph.at(factorIx);
+        typename FACTOR::shared_ptr factor = graph.at(factorIx);
-      if (!factor->active()) continue;
+        if (!factor->active())
-      Matrix Ai = factor->getA(factor->find(key)).transpose();
+          continue;
-      Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
+        Matrix Ai = factor->getA(factor->find(key)).transpose();
+        Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
+      }
     }
+    return Aterms;
   }
-  return Aterms;
+  /**
-}
+   * Identifies active constraints that shouldn't be active anymore.
-/**
+   */
- * Identifies active constraints that shouldn't be active anymore.
+  int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
- */
+      const VectorValues& lambdas) const;
-int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
-    const VectorValues& lambdas) const;
 
-/**
+  /**
- * Builds a dual graph from the current working set.
+   * Builds a dual graph from the current working set.
- */
+   */
-GaussianFactorGraph::shared_ptr buildDualGraph(
+  GaussianFactorGraph::shared_ptr buildDualGraph(
-    const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
+      const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
 
 protected:
-/**
+  /**
- * Protected constructor because this class doesn't have any meaning without
+   * Protected constructor because this class doesn't have any meaning without
- * a concrete Programming problem to solve.
+   * a concrete Programming problem to solve.
- */
+   */
-ActiveSetSolver() :
+  ActiveSetSolver() :
-    constrainedKeys_() {
+      constrainedKeys_() {
-}
+  }
 
-/**
+  /**
- * Computes the distance to move from the current point being examined to the next 
+   * Computes the distance to move from the current point being examined to the next 
- * location to be examined by the graph. This should only be used where there are less 
+   * location to be examined by the graph. This should only be used where there are less 
- * than two constraints active.
+   * than two constraints active.
- */
+   */
-boost::tuple<double, int> computeStepSize(
+  boost::tuple<double, int> computeStepSize(
-    const InequalityFactorGraph& workingSet, const VectorValues& xk,
+      const InequalityFactorGraph& workingSet, const VectorValues& xk,
-    const VectorValues& p, const double& startAlpha) const;
+      const VectorValues& p, const double& startAlpha) const;
 };
 } // namespace gtsam

gtsam_unstable/linear/EqualityFactorGraph.h

@@ -26,7 +26,7 @@ namespace gtsam {
  * This class is used to represent an equality constraint on
  * a Programming problem of the form Ax = b.
  */
-class EqualityFactorGraph : public FactorGraph<LinearEquality> {
+class EqualityFactorGraph: public FactorGraph<LinearEquality> {
 public:
   typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
 
@@ -36,8 +36,8 @@ public:
    */
   double error(const VectorValues& x) const {
     double total_error = 0.;
-    for(const sharedFactor& factor: *this){
+    for (const sharedFactor& factor : *this) {
-      if(factor)
+      if (factor)
         total_error += factor->error(x);
     }
     return total_error;

gtsam_unstable/linear/InequalityFactorGraph.h

@@ -47,10 +47,10 @@ public:
    * Compute error of a guess.
    * Infinity error if it violates an inequality; zero otherwise. */
   double error(const VectorValues& x) const {
-    for(const sharedFactor& factor: *this) {
+    for (const sharedFactor& factor : *this) {
-      if(factor)
+      if (factor)
-      if (factor->error(x) > 0)
+        if (factor->error(x) > 0)
-      return std::numeric_limits<double>::infinity();
+          return std::numeric_limits<double>::infinity();
     }
     return 0.0;
   }

gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h

@@ -5,7 +5,6 @@
  * @date     1/24/16
  */
 
-
 namespace gtsam {
 
 class InfeasibleOrUnboundedProblem: public ThreadsafeException<

gtsam_unstable/linear/LPInitSolver.h

@@ -128,7 +128,7 @@ private:
   /// Collect all terms of a factor into a container.
   std::vector<std::pair<Key, Matrix> > collectTerms(
       const LinearInequality& factor) const {
-    std::vector<std::pair<Key, Matrix> > terms;
+    std::vector < std::pair<Key, Matrix> > terms;
     for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
       terms.push_back(make_pair(*it, factor.getA(it)));
     }
@@ -140,7 +140,7 @@ private:
       const InequalityFactorGraph& inequalities) const {
     InequalityFactorGraph slackInequalities;
     for (const auto& factor : lp_.inequalities) {
-      std::vector<std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
+      std::vector < std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
       terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y
       double d = factor->getb()[0];
       slackInequalities.push_back(

gtsam_unstable/linear/LPSolver.cpp

@@ -109,7 +109,7 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
         lp_.cost.end(), std::inserter(difference, difference.end()));
     for (Key k : difference) {
       size_t dim = keysDim_.at(k);
-      graph->push_back(JacobianFactor(k, Matrix::Identity(dim,dim), xk.at(k)));
+      graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
     }
   }
   return graph;
@@ -136,10 +136,10 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
     const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
   // Transpose the A matrix of constrained factors to have the jacobian of the
   // dual key
-  TermsContainer Aterms = collectDualJacobians<LinearEquality>(key,
+  TermsContainer Aterms = collectDualJacobians < LinearEquality
-      lp_.equalities, equalityVariableIndex_);
+      > (key, lp_.equalities, equalityVariableIndex_);
-  TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
+  TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
-      key, workingSet, inequalityVariableIndex_);
+      > (key, workingSet, inequalityVariableIndex_);
   Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
       AtermsInequalities.end());
 
@@ -149,7 +149,7 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
     Factor::const_iterator it = lp_.cost.find(key);
     if (it != lp_.cost.end())
       b = lp_.cost.getA(it).transpose();
-    return boost::make_shared<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables
+    return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
   } else {
     return boost::make_shared<JacobianFactor>();
   }

gtsam_unstable/linear/LinearCost.h

@@ -30,90 +30,89 @@ typedef Eigen::RowVectorXd RowVector;
  */
 class LinearCost: public JacobianFactor {
 public:
-	typedef LinearCost This; ///< Typedef to this class
+  typedef LinearCost This; ///< Typedef to this class
-	typedef JacobianFactor Base; ///< Typedef to base class
+  typedef JacobianFactor Base; ///< Typedef to base class
-	typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
+  typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
 public:
-	/** default constructor for I/O */
+  /** default constructor for I/O */
-	LinearCost() :
+  LinearCost() :
-			Base() {
+      Base() {
-	}
+  }
 
-	/** Conversion from HessianFactor */
+  /** Conversion from HessianFactor */
-	explicit LinearCost(const HessianFactor& hf) {
+  explicit LinearCost(const HessianFactor& hf) {
-		throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
+    throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
-	}
+  }
 
-	/** Conversion from JacobianFactor */
+  /** Conversion from JacobianFactor */
-	explicit LinearCost(const JacobianFactor& jf) :
+  explicit LinearCost(const JacobianFactor& jf) :
-			Base(jf) {
+      Base(jf) {
-		if (jf.isConstrained()) {
+    if (jf.isConstrained()) {
-			throw std::runtime_error(
+      throw std::runtime_error(
-					"Cannot convert a constrained JacobianFactor to LinearCost");
+          "Cannot convert a constrained JacobianFactor to LinearCost");
-		}
+    }
 
-		if (jf.get_model()->dim() != 1) {
+    if (jf.get_model()->dim() != 1) {
-			throw std::runtime_error(
+      throw std::runtime_error(
-					"Only support single-valued linear cost factor!");
+          "Only support single-valued linear cost factor!");
-		}
+    }
-	}
+  }
 
-	/** Construct unary factor */
+  /** Construct unary factor */
-	LinearCost(Key i1, const RowVector& A1) :
+  LinearCost(Key i1, const RowVector& A1) :
-			Base(i1, A1, Vector1::Zero()) {
+      Base(i1, A1, Vector1::Zero()) {
-	}
+  }
 
-	/** Construct binary factor */
+  /** Construct binary factor */
-	LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
+  LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) :
-			double b) :
+      Base(i1, A1, i2, A2, Vector1::Zero()) {
-			Base(i1, A1, i2, A2, Vector1::Zero()) {
+  }
-	}
 
-	/** Construct ternary factor */
+  /** Construct ternary factor */
-	LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
+  LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
-			const RowVector& A3) :
+      const RowVector& A3) :
-			Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) {
+      Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) {
-	}
+  }
 
-	/** Construct an n-ary factor
+  /** Construct an n-ary factor
-	 * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
+   * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
-	 *         collection of keys and matrices making up the factor. */
+   *         collection of keys and matrices making up the factor. */
-	template<typename TERMS>
+  template<typename TERMS>
-	LinearCost(const TERMS& terms) :
+  LinearCost(const TERMS& terms) :
-			Base(terms, Vector1::Zero()) {
+      Base(terms, Vector1::Zero()) {
-	}
+  }
 
-	/** Virtual destructor */
+  /** Virtual destructor */
-	virtual ~LinearCost() {
+  virtual ~LinearCost() {
-	}
+  }
 
-	/** equals */
+  /** equals */
-	virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
+  virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
-		return Base::equals(lf, tol);
+    return Base::equals(lf, tol);
-	}
+  }
 
-	/** print */
+  /** print */
-	virtual void print(const std::string& s = "",
+  virtual void print(const std::string& s = "", const KeyFormatter& formatter =
-			const KeyFormatter& formatter = DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const {
-		Base::print(s + " LinearCost: ", formatter);
+    Base::print(s + " LinearCost: ", formatter);
-	}
+  }
 
-	/** Clone this LinearCost */
+  /** Clone this LinearCost */
-	virtual GaussianFactor::shared_ptr clone() const {
+  virtual GaussianFactor::shared_ptr clone() const {
-		return boost::static_pointer_cast<GaussianFactor>(
+    return boost::static_pointer_cast < GaussianFactor
-				boost::make_shared<LinearCost>(*this));
+        > (boost::make_shared < LinearCost > (*this));
-	}
+  }
 
-	/** Special error_vector for constraints (A*x-b) */
+  /** Special error_vector for constraints (A*x-b) */
-	Vector error_vector(const VectorValues& c) const {
+  Vector error_vector(const VectorValues& c) const {
-		return unweighted_error(c);
+    return unweighted_error(c);
-	}
+  }
 
-	/** Special error for single-valued inequality constraints. */
+  /** Special error for single-valued inequality constraints. */
-	virtual double error(const VectorValues& c) const {
+  virtual double error(const VectorValues& c) const {
-		return error_vector(c)[0];
+    return error_vector(c)[0];
-	}
+  }
 };
 // \ LinearCost
 

gtsam_unstable/linear/LinearEquality.h

@@ -44,13 +44,14 @@ public:
   /**
    * Construct from a constrained noisemodel JacobianFactor with a dual key.
    */
-  explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey){
+  explicit LinearEquality(const JacobianFactor& jf, Key dualKey) :
+      Base(jf), dualKey_(dualKey) {
     if (!jf.isConstrained()) {
-      throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality");
+      throw std::runtime_error(
+          "Cannot convert an unconstrained JacobianFactor to LinearEquality");
     }
   }
 
-
   /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
   explicit LinearEquality(const HessianFactor& hf) {
     throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
@@ -100,15 +101,19 @@ public:
 
   /** Clone this LinearEquality */
   virtual GaussianFactor::shared_ptr clone() const {
-    return boost::static_pointer_cast<GaussianFactor>(
+    return boost::static_pointer_cast < GaussianFactor
-        boost::make_shared<LinearEquality>(*this));
+        > (boost::make_shared < LinearEquality > (*this));
   }
 
   /// dual key
-  Key dualKey() const { return dualKey_; }
+  Key dualKey() const {
+    return dualKey_;
+  }
 
   /// for active set method: equality constraints are always active
-  bool active() const { return true; }
+  bool active() const {
+    return true;
+  }
 
   /** Special error_vector for constraints (A*x-b) */
   Vector error_vector(const VectorValues& c) const {
@@ -123,11 +128,12 @@ public:
     return 0.0;
   }
 
-}; // \ LinearEquality
+};
-
+// \ LinearEquality
 
 /// traits
-template<> struct traits<LinearEquality> : public Testable<LinearEquality> {};
+template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
+};
 
 } // \ namespace gtsam
 

gtsam_unstable/linear/LinearInequality.h

@@ -52,9 +52,11 @@ public:
   }
 
   /** Conversion from JacobianFactor */
-  explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey), active_(true) {
+  explicit LinearInequality(const JacobianFactor& jf, Key dualKey) :
+      Base(jf), dualKey_(dualKey), active_(true) {
     if (!jf.isConstrained()) {
-      throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearInequality");
+      throw std::runtime_error(
+          "Cannot convert an unconstrained JacobianFactor to LinearInequality");
     }
 
     if (jf.get_model()->dim() != 1) {
@@ -64,20 +66,20 @@ public:
 
   /** Construct unary factor */
   LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
-      Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
+      Base(i1, A1, (Vector(1) << b).finished(),
-          dualKey), active_(true) {
+          noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
   }
 
   /** Construct binary factor */
-  LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b,
+  LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
-      Key dualKey) :
+      double b, Key dualKey) :
-      Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
+      Base(i1, A1, i2, A2, (Vector(1) << b).finished(),
-          dualKey), active_(true) {
+          noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
   }
 
   /** Construct ternary factor */
-  LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
+  LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
-      const RowVector& A3, double b, Key dualKey) :
+      Key i3, const RowVector& A3, double b, Key dualKey) :
       Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(),
           noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
   }
@@ -112,21 +114,29 @@ public:
 
   /** Clone this LinearInequality */
   virtual GaussianFactor::shared_ptr clone() const {
-    return boost::static_pointer_cast<GaussianFactor>(
+    return boost::static_pointer_cast < GaussianFactor
-        boost::make_shared<LinearInequality>(*this));
+        > (boost::make_shared < LinearInequality > (*this));
   }
 
   /// dual key
-  Key dualKey() const { return dualKey_; }
+  Key dualKey() const {
+    return dualKey_;
+  }
 
   /// return true if this constraint is active
-  bool active() const { return active_; }
+  bool active() const {
+    return active_;
+  }
 
   /// Make this inequality constraint active
-  void activate() { active_ = true; }
+  void activate() {
+    active_ = true;
+  }
 
   /// Make this inequality constraint inactive
-  void inactivate() { active_ = false; }
+  void inactivate() {
+    active_ = false;
+  }
 
   /** Special error_vector for constraints (A*x-b) */
   Vector error_vector(const VectorValues& c) const {
@@ -149,10 +159,12 @@ public:
     return aTp;
   }
 
-}; // \ LinearInequality
+};
+// \ LinearInequality
 
 /// traits
-template<> struct traits<LinearInequality> : public Testable<LinearInequality> {};
+template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
+};
 
 } // \ namespace gtsam
 

gtsam_unstable/linear/QP.h

@@ -42,7 +42,8 @@ struct QP {
   QP(const GaussianFactorGraph& _cost,
       const EqualityFactorGraph& _linearEqualities,
       const InequalityFactorGraph& _linearInequalities) :
-      cost(_cost), equalities(_linearEqualities), inequalities(_linearInequalities) {
+      cost(_cost), equalities(_linearEqualities), inequalities(
+          _linearInequalities) {
   }
 
   /** print */

gtsam_unstable/linear/RawQP.h

@@ -36,9 +36,7 @@ private:
 
 public:
   RawQP() :
-      row_to_constraint_v(), E(), IG(), IL(), varNumber(1),
+      row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() {
-      b(), g(), varname_to_key(), H(), f(),
-      obj_name(), name_(), up(), lo(), Free() {
   }
 
   void setName(

gtsam_unstable/linear/tests/testLinearEquality.cpp

@@ -28,20 +28,20 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
+GTSAM_CONCEPT_TESTABLE_INST (LinearEquality)
 
 namespace {
-  namespace simple {
+namespace simple {
-    // Terms we'll use
+// Terms we'll use
-    const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
+const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix>
-      (make_pair(5, Matrix3::Identity()))
+    > (make_pair(5, Matrix3::Identity()))(
-      (make_pair(10, 2*Matrix3::Identity()))
+        make_pair(10, 2 * Matrix3::Identity()))(
-      (make_pair(15, 3*Matrix3::Identity()));
+        make_pair(15, 3 * Matrix3::Identity()));
 
-    // RHS and sigmas
+// RHS and sigmas
-    const Vector b = (Vector(3) << 1., 2., 3.).finished();
+const Vector b = (Vector(3) << 1., 2., 3.).finished();
-    const SharedDiagonal noise = noiseModel::Constrained::All(3);
+const SharedDiagonal noise = noiseModel::Constrained::All(3);
-  }
+}
 }
 
 /* ************************************************************************* */
@@ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors)
   {
     // One term constructor
     LinearEquality expected(
-      boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
+        boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
     LinearEquality actual(terms[0].first, terms[0].second, b, 0);
     EXPECT(assert_equal(expected, actual));
     LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
@@ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors)
   {
     // Two term constructor
     LinearEquality expected(
-      boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
+        boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
     LinearEquality actual(terms[0].first, terms[0].second,
-      terms[1].first, terms[1].second, b, 0);
+        terms[1].first, terms[1].second, b, 0);
     EXPECT(assert_equal(expected, actual));
     LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
     EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
@@ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors)
   {
     // Three term constructor
     LinearEquality expected(
-      boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
+        boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
     LinearEquality actual(terms[0].first, terms[0].second,
-      terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
+        terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
     EXPECT(assert_equal(expected, actual));
     LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
     EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
@@ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors)
 /* ************************************************************************* */
 TEST(LinearEquality, Hessian_conversion) {
   HessianFactor hessian(0, (Matrix(4,4) <<
-        1.57,        2.695,         -1.1,        -2.35,
+          1.57, 2.695, -1.1, -2.35,
-       2.695,      11.3125,        -0.65,      -10.225,
+          2.695, 11.3125, -0.65, -10.225,
-        -1.1,        -0.65,            1,          0.5,
+          -1.1, -0.65, 1, 0.5,
-       -2.35,      -10.225,          0.5,         9.25).finished(),
+          -2.35, -10.225, 0.5, 9.25).finished(),
       (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
       73.1725);
 
@@ -169,8 +169,8 @@ TEST(LinearEquality, matrices)
   augmentedJacobianExpected << jacobianExpected, rhsExpected;
 
   Matrix augmentedHessianExpected =
-    augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
+  augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
-    * simple::noise->R() * augmentedJacobianExpected;
+  * simple::noise->R() * augmentedJacobianExpected;
 
   // Whitened Jacobian
   EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
@@ -210,8 +210,8 @@ TEST(LinearEquality, operators )
   // test gradient at zero
   Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
   VectorValues expectedG;
-  expectedG.insert(1, (Vector(2) <<  0.2, -0.1).finished());
+  expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
-  expectedG.insert(2, (Vector(2) << -0.2,  0.1).finished());
+  expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
   VectorValues actualG = lf.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
 }
@@ -233,5 +233,8 @@ TEST(LinearEquality, empty )
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -42,8 +42,8 @@ QP createTestCase() {
   //TODO:  THIS TEST MIGHT BE WRONG : the last parameter  might be 5 instead of 10 because the form of the equation
   // Should be 0.5x'Gx + gx + f : Nocedal 449
   qp.cost.push_back(
-      HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1,
+      HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1,
-          3.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0));
+          Z_1x1, 10.0));
 
   // Inequality constraints
   qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
@@ -96,8 +96,8 @@ QP createEqualityConstrainedTest() {
   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
   // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
   qp.cost.push_back(
-      HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1,
+      HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1,
-          2.0 * I_1x1, Z_1x1, 0.0));
+          0.0));
 
   // Equality constraints
   // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
@@ -140,9 +140,9 @@ TEST(QPSolver, indentifyActiveConstraints) {
       qp.inequalities, currentSolution);
 
   CHECK(!workingSet.at(0)->active()); // inactive
-  CHECK(workingSet.at(1)->active()); // active
+  CHECK(workingSet.at(1)->active());// active
-  CHECK(workingSet.at(2)->active()); // active
+  CHECK(workingSet.at(2)->active());// active
-  CHECK(!workingSet.at(3)->active()); // inactive
+  CHECK(!workingSet.at(3)->active());// inactive
 
   VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
   VectorValues expectedSolution;
@@ -211,14 +211,15 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
   expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
   CHECK(assert_equal(expectedSolution, solution, 1e-100));
 }
+
 pair<QP, QP> testParser(QPSParser parser) {
   QP exampleqp = parser.Parse();
   QP expectedqp;
   Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
   // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
   expectedqp.cost.push_back(
-      HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne,
+      HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, 10.0 * I_1x1,
-          10.0 * I_1x1, -2.0 * kOne, 4.0));
+          -2.0 * kOne, 4.0));
   // 2x + y >= 2
   // -x + 2y <= 6
   expectedqp.inequalities.push_back(
@@ -267,13 +268,15 @@ QP createTestMatlabQPEx() {
   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
   // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
   qp.cost.push_back(
-      HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
+      HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1,
-          2.0 * I_1x1, 6 * I_1x1, 1000.0));
+          6 * I_1x1, 1000.0));
 
   // Inequality constraints
   qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
-  qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
+  qp.inequalities.push_back(
-  qp.inequalities.push_back(LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
+      LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
+  qp.inequalities.push_back(
+      LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
   qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1      <= 0
   qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); //      -x2 <= 0