[REFACTOR] Replace BOOST_FOREACH with c++ foreach loop

[BUGFIX] Fixed failing unit test due to assert.

gtsam_unstable/linear/ActiveSetSolver.cpp

@@ -45,23 +45,24 @@ namespace gtsam {
  *
  */
 int ActiveSetSolver::identifyLeavingConstraint(
-  const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const {
+    const InequalityFactorGraph& workingSet,
-int worstFactorIx = -1;
+    const VectorValues& lambdas) const {
+  int worstFactorIx = -1;
 // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is
 // either
 // inactive or a good inequality constraint, so we don't care!
-double maxLambda = 0.0;
+  double maxLambda = 0.0;
-for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
+  for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
-  const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
+    const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
-  if (factor->active()) {
+    if (factor->active()) {
-    double lambda = lambdas.at(factor->dualKey())[0];
+      double lambda = lambdas.at(factor->dualKey())[0];
-    if (lambda > maxLambda) {
+      if (lambda > maxLambda) {
-      worstFactorIx = factorIx;
+        worstFactorIx = factorIx;
-      maxLambda = lambda;
+        maxLambda = lambda;
+      }
     }
   }
-}
+  return worstFactorIx;
-return worstFactorIx;
 }
 
 /*  This function will create a dual graph that solves for the
@@ -76,15 +77,16 @@ return worstFactorIx;
  *  if lambda > 0  you are violating the constraint.
  */
 GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph(
-  const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
+    const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
-GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
+  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
+    // 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;
+  }
+  return dualGraph;
 }
 
 /*
@@ -96,35 +98,35 @@ return dualGraph;
  *            in the next iteration.
  */
 boost::tuple<double, int> ActiveSetSolver::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 {
-double minAlpha = startAlpha;
+  double minAlpha = startAlpha;
-int closestFactorIx = -1;
+  int closestFactorIx = -1;
-for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
+  for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
-  const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
+    const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
-  double b = factor->getb()[0];
+    double b = factor->getb()[0];
-  // only check inactive factors
+    // only check inactive factors
-  if (!factor->active()) {
+    if (!factor->active()) {
-    // Compute a'*p
+      // Compute a'*p
-    double aTp = factor->dotProductRow(p);
+      double aTp = factor->dotProductRow(p);
 
-    // Check if  a'*p >0. Don't care if it's not.
+      // Check if  a'*p >0. Don't care if it's not.
-    if (aTp <= 0)
+      if (aTp <= 0)
-      continue;
+        continue;
 
-    // Compute a'*xk
+      // Compute a'*xk
-    double aTx = factor->dotProductRow(xk);
+      double aTx = factor->dotProductRow(xk);
 
-    // alpha = (b - a'*xk) / (a'*p)
+      // alpha = (b - a'*xk) / (a'*p)
-    double alpha = (b - aTx) / aTp;
+      double alpha = (b - aTx) / aTp;
-    // We want the minimum of all those max alphas
+      // We want the minimum of all those max alphas
-    if (alpha < minAlpha) {
+      if (alpha < minAlpha) {
-      closestFactorIx = factorIx;
+        closestFactorIx = factorIx;
-      minAlpha = alpha;
+        minAlpha = alpha;
+      }
     }
   }
-}
+  return boost::make_tuple(minAlpha, closestFactorIx);
-return boost::make_tuple(minAlpha, closestFactorIx);
 }
 
 }

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,10 +43,11 @@ 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;
-      keysDim[key] = factor->getDim(factor->find(key));
+      for (Key key : factor->keys())
+        keysDim[key] = factor->getDim(factor->find(key));
     }
     return keysDim;
   }

gtsam_unstable/linear/QPSolver.cpp

@@ -184,9 +184,9 @@ 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)
+    if (newKey < key)
-  newKey = key;
+      newKey = key;
   newKey++;
   initProblem.cost = LinearCost(newKey, Vector::Ones(1));
   initProblem.equalities = qp_.equalities;

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);