[REFACTOR] Replace BOOST_FOREACH with c++ foreach loop

[BUGFIX] Fixed failing unit test due to assert.

gtsam_unstable/linear/ActiveSetSolver.cpp

@@ -45,7 +45,8 @@ namespace gtsam {
  *
  */
 int ActiveSetSolver::identifyLeavingConstraint(
-  const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const {
+    const InequalityFactorGraph& workingSet,
+    const VectorValues& lambdas) const {
   int worstFactorIx = -1;
 // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is
 // either
@@ -78,11 +79,12 @@ return worstFactorIx;
 GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph(
     const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
   GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
-BOOST_FOREACH (Key key, constrainedKeys_) {
+  for (Key key : constrainedKeys_) {
     // Each constrained key becomes a factor in the dual graph
-  JacobianFactor::shared_ptr dualFactor =
+    JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet,
-  createDualFactor(key, workingSet, delta);
+        delta);
-  if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
+    if (!dualFactor->empty())
+      dualGraph->push_back(dualFactor);
   }
   return dualGraph;
 }

gtsam_unstable/linear/ActiveSetSolver.h

@@ -53,7 +53,7 @@ public:
      */
     TermsContainer Aterms;
     if (variableIndex.find(key) != variableIndex.end()) {
-    BOOST_FOREACH (size_t factorIx, variableIndex[key]) {
+    for(size_t factorIx: variableIndex[key]) {
       typename FACTOR::shared_ptr factor = graph.at(factorIx);
       if (!factor->active()) continue;
       Matrix Ai = factor->getA(factor->find(key)).transpose();

gtsam_unstable/linear/EqualityFactorGraph.h

@@ -36,7 +36,7 @@ public:
    */
   double error(const VectorValues& x) const {
     double total_error = 0.;
-    BOOST_FOREACH(const sharedFactor& factor, *this){
+    for(const sharedFactor& factor: *this){
       if(factor)
         total_error += factor->error(x);
     }

gtsam_unstable/linear/InequalityFactorGraph.h

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

gtsam_unstable/linear/LPSolver.cpp

@@ -161,7 +161,8 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
   InequalityFactorGraph workingSet;
   for (const LinearInequality::shared_ptr &factor : inequalities) {
     LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-
+    GTSAM_PRINT(*workingFactor);
+    GTSAM_PRINT(initialValues);
     double error = workingFactor->error(initialValues);
     // TODO: find a feasible initial point for LPSolver.
     // For now, we just throw an exception

gtsam_unstable/linear/LPSolver.h

@@ -43,9 +43,10 @@ public:
   template<class LinearGraph>
   KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const {
     KeyDimMap keysDim;
-    BOOST_FOREACH(const typename LinearGraph::sharedFactor &factor, linearGraph) {
+    for (const typename LinearGraph::sharedFactor &factor : linearGraph) {
-      if (!factor) continue;
+      if (!factor)
-      BOOST_FOREACH(Key key, factor->keys())
+        continue;
+      for (Key key : factor->keys())
         keysDim[key] = factor->getDim(factor->find(key));
     }
     return keysDim;

gtsam_unstable/linear/QPSolver.cpp

@@ -184,7 +184,7 @@ pair<VectorValues, VectorValues> QPSolver::optimize() const {
   //Make an LP with any linear cost function. It doesn't matter for initialization.
   LP initProblem;
   Key newKey = 0; // make an unrelated key for a random variable cost
-  BOOST_FOREACH(Key key, qp_.cost.getKeyDimMap() | boost::adaptors::map_keys)
+  for (Key key : qp_.cost.getKeyDimMap() | boost::adaptors::map_keys)
     if (newKey < key)
       newKey = key;
   newKey++;

gtsam_unstable/linear/tests/testLPSolver.cpp

@@ -94,7 +94,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
   VectorValues starter;
   starter.insert(X, kZero);
   starter.insert(Y, kZero);
-  starter.insert(Z, Vector::Constant(2, 2.0));
+  starter.insert(Z, Vector::Constant(1, 2.0));
   VectorValues results, duals;
   boost::tie(results, duals) = solver.optimize(starter);
   VectorValues expected;

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -149,7 +149,6 @@ TEST(QPSolver, indentifyActiveConstraints) {
   expectedSolution.insert(X(1), kZero);
   expectedSolution.insert(X(2), kZero);
   CHECK(assert_equal(expectedSolution, solution, 1e-100));
-
 }
 
 /* ************************************************************************* */

gtsam_unstable/nonlinear/LinearConstraintSQP.cpp

@@ -37,7 +37,7 @@ bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state
 
 /* ************************************************************************* */
 bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) {
+  for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearInequalities) {
     ConstrainedFactor::shared_ptr inequality
         = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
 
@@ -66,7 +66,7 @@ bool LinearConstraintSQP::checkConvergence(const LinearConstraintNLPState& state
 /* ************************************************************************* */
 VectorValues LinearConstraintSQP::initializeDuals() const {
   VectorValues duals;
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
+  for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearEqualities){
     ConstrainedFactor::shared_ptr constraint
         = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
     duals.insert(constraint->dualKey(), Vector::Zero(factor->dim()));
@@ -92,7 +92,7 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
   VectorValues delta, duals;
   QPSolver qpSolver(qp);
   VectorValues zeroInitialValues;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values)
+  for(const Values::ConstKeyValuePair& key_value: state.values)
     zeroInitialValues.insert(key_value.key, Vector::Zero(key_value.value.dim()));
 
   boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,

gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp

@@ -27,7 +27,7 @@ EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize(
     const Values& linearizationPoint) const {
   EqualityFactorGraph::shared_ptr linearGraph(
       new EqualityFactorGraph());
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+  for(const NonlinearFactor::shared_ptr& factor: *this){
     JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
         factor->linearize(linearizationPoint));
     ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
@@ -38,7 +38,7 @@ EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize(
 
 /* ************************************************************************* */
 bool LinearEqualityFactorGraph::checkFeasibility(const Values& values, double tol) const {
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+  for(const NonlinearFactor::shared_ptr& factor: *this){
     NoiseModelFactor::shared_ptr noiseModelFactor
         = boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
     Vector error = noiseModelFactor->unwhitenedError(values);

gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp

@@ -26,7 +26,7 @@ namespace gtsam {
 InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize(
     const Values& linearizationPoint) const {
   InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph());
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
+  for(const NonlinearFactor::shared_ptr& factor: *this) {
     JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
         factor->linearize(linearizationPoint));
     ConstrainedFactor::shared_ptr constraint
@@ -40,7 +40,7 @@ InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize(
 bool LinearInequalityFactorGraph::checkFeasibilityAndComplimentary(
     const Values& values, const VectorValues& dualValues, double tol) const {
 
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
+  for(const NonlinearFactor::shared_ptr& factor: *this) {
     NoiseModelFactor::shared_ptr noiseModelFactor
         = boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
     Vector error = noiseModelFactor->unwhitenedError(values);