Merge branch 'develop' into feature/LPSolver

# Conflicts:
#	gtsam_unstable/linear/QPSolver.cpp
#	gtsam_unstable/linear/QPSolver.h

examples/CreateSFMExampleData.cpp

@@ -18,7 +18,6 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/vector.hpp>
 
 using namespace boost::assign;
@@ -41,7 +40,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
   data.cameras.push_back(SfM_Camera(pose1, K));
   data.cameras.push_back(SfM_Camera(pose2, K));
 
-  BOOST_FOREACH(const Point3& p, P) {
+  for(const Point3& p: P) {
 
     // Create the track
     SfM_Track track;

examples/Pose3SLAMExample_changeKeys.cpp

@@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    for(const Values::ConstKeyValuePair& key_value: *initial) {
       Key key;
       if(add)
         key = key_value.key + firstKey;
@@ -66,7 +66,7 @@ int main(const int argc, const char *argv[]) {
       simpleInitial.insert(key, initial->at(key_value.key));
     }
     NonlinearFactorGraph simpleGraph;
-    BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
+    for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
       boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
           boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
       if (pose3Between){

examples/Pose3SLAMExample_g2o.cpp

@@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

examples/RangeISAMExample_plaza2.cpp

@@ -43,7 +43,6 @@
 #include <gtsam/slam/dataset.h>
 
 // Standard headers, added last, so we know headers above work on their own
-#include <boost/foreach.hpp>
 #include <fstream>
 #include <iostream>
 
@@ -151,7 +150,7 @@ int main (int argc, char** argv) {
 
   // Loop over odometry
   gttic_(iSAM);
-  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
+  for(const TimedOdometry& timedOdometry: odometry) {
     //--------------------------------- odometry loop -----------------------------------------
     double t;
     Pose2 odometry;
@@ -196,7 +195,7 @@ int main (int argc, char** argv) {
     }
     i += 1;
     //--------------------------------- odometry loop -----------------------------------------
-  } // BOOST_FOREACH
+  } // end for
   gttoc_(iSAM);
 
   // Print timings

examples/SFMExampleExpressions_bal.cpp

@@ -78,10 +78,10 @@ int main(int argc, char* argv[]) {
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   size_t j = 0;
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+  for(const SfM_Track& track: mydata.tracks) {
     // Leaf expression for j^th point
     Point3_ point_('p', j);
-    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+    for(const SfM_Measurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       // Leaf expression for i^th camera
@@ -98,9 +98,9 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0;
   j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
+  for(const SfM_Camera& camera: mydata.cameras)
     initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
+  for(const SfM_Track& track: mydata.tracks)
     initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */

examples/SFMExample_bal.cpp

@@ -55,8 +55,8 @@ int main (int argc, char* argv[]) {
 
   // Add measurements to the factor graph
   size_t j = 0;
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+  for(const SfM_Track& track: mydata.tracks) {
-    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+    for(const SfM_Measurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
@@ -72,8 +72,8 @@ int main (int argc, char* argv[]) {
   // Create initial estimate
   Values initial;
   size_t i = 0; j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+  for(const SfM_Track& track: mydata.tracks)    initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -60,8 +60,8 @@ int main (int argc, char* argv[]) {
 
   // Add measurements to the factor graph
   size_t j = 0;
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+  for(const SfM_Track& track: mydata.tracks) {
-    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+    for(const SfM_Measurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
@@ -77,8 +77,8 @@ int main (int argc, char* argv[]) {
   // Create initial estimate
   Values initial;
   size_t i = 0; j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+  for(const SfM_Track& track: mydata.tracks)    initial.insert(P(j++), track.p);
 
   /** ---------------  COMPARISON  -----------------------**/
   /** ----------------------------------------------------**/

examples/SolverComparer.cpp

@@ -50,6 +50,7 @@
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 #include <boost/random.hpp>
 #include <boost/serialization/export.hpp>
 
@@ -80,7 +81,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c
   // the factor graph already includes a factor for the prior/equality constraint.
   //  double dof = graph.size() - config.size();
   int graph_dim = 0;
-  BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
+  for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
     graph_dim += (int)nlf->dim();
   }
   double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
@@ -421,9 +422,9 @@ void runIncremental()
   //try {
   //  Marginals marginals(graph, values);
   //  int i=0;
-  //  BOOST_REVERSE_FOREACH(Key key1, values.keys()) {
+  //  for (Key key1: boost::adaptors::reverse(values.keys())) {
   //    int j=0;
-  //    BOOST_REVERSE_FOREACH(Key key2, values.keys()) {
+  //    for (Key key12: boost::adaptors::reverse(values.keys())) {
   //      if(i != j) {
   //        gttic_(jointMarginalInformation);
   //        std::vector<Key> keys(2);
@@ -442,7 +443,7 @@ void runIncremental()
   //      break;
   //  }
   //  tictoc_print_();
-  //  BOOST_FOREACH(Key key, values.keys()) {
+  //  for(Key key: values.keys()) {
   //    gttic_(marginalInformation);
   //    Matrix info = marginals.marginalInformation(key);
   //    gttoc_(marginalInformation);
@@ -535,7 +536,7 @@ void runCompare()
   vector<Key> commonKeys;
   br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
   double maxDiff = 0.0;
-  BOOST_FOREACH(Key j, commonKeys)
+  for(Key j: commonKeys)
     maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
   cout << "  Maximum solution difference (norm of logmap): " << maxDiff << endl;
 }
@@ -549,7 +550,7 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
+  for(const Values::KeyValuePair& key_val: initial)
   {
     Vector noisev(key_val.value.dim());
     for(Vector::Index i = 0; i < noisev.size(); ++i)

examples/TimeTBB.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/base/Matrix.h>
 
 #include <boost/assign/list_of.hpp>
-#include <boost/foreach.hpp>
 #include <map>
 
 using namespace std;
@@ -76,7 +75,7 @@ map<int, double> testWithoutMemoryAllocation()
 
   const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
   map<int, double> timingResults;
-  BOOST_FOREACH(size_t grainSize, grainSizes)
+  for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
     tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
@@ -129,7 +128,7 @@ map<int, double> testWithMemoryAllocation()
 
   const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
   map<int, double> timingResults;
-  BOOST_FOREACH(size_t grainSize, grainSizes)
+  for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
     tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
@@ -150,7 +149,7 @@ int main(int argc, char* argv[])
   const vector<int> numThreads = list_of(1)(4)(8);
   Results results;
 
-  BOOST_FOREACH(size_t n, numThreads)
+  for(size_t n: numThreads)
   {
     cout << "With " << n << " threads:" << endl;
     tbb::task_scheduler_init init((int)n);
@@ -160,19 +159,19 @@ int main(int argc, char* argv[])
   }
 
   cout << "Summary of results:" << endl;
-  BOOST_FOREACH(const Results::value_type& threads_result, results)
+  for(const Results::value_type& threads_result: results)
   {
     const int threads = threads_result.first;
     const ResultWithThreads& result = threads_result.second;
     if(threads != 1)
     {
-      BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation)
+      for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation)
       {
         const int grainsize = grainsize_time.first;
         const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
         cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
       }
-      BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation)
+      for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation)
       {
         const int grainsize = grainsize_time.first;
         const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;

gtsam/base/DSFVector.cpp

@@ -18,7 +18,6 @@
 
 #include <gtsam/base/DSFVector.h>
 #include <boost/make_shared.hpp>
-#include <boost/foreach.hpp>
 #include <algorithm>
 
 using namespace std;
@@ -79,7 +78,7 @@ DSFVector::DSFVector(const boost::shared_ptr<V>& v_in,
 /* ************************************************************************* */
 bool DSFVector::isSingleton(const size_t& label) const {
   bool result = false;
-  BOOST_FOREACH(size_t key,keys_) {
+  for(size_t key: keys_) {
     if (find(key) == label) {
       if (!result) // find the first occurrence
         result = true;
@@ -93,7 +92,7 @@ bool DSFVector::isSingleton(const size_t& label) const {
 /* ************************************************************************* */
 std::set<size_t> DSFVector::set(const size_t& label) const {
   std::set < size_t > set;
-  BOOST_FOREACH(size_t key,keys_)
+  for(size_t key: keys_)
     if (find(key) == label)
       set.insert(key);
   return set;
@@ -102,7 +101,7 @@ std::set<size_t> DSFVector::set(const size_t& label) const {
 /* ************************************************************************* */
 std::map<size_t, std::set<size_t> > DSFVector::sets() const {
   std::map<size_t, std::set<size_t> > sets;
-  BOOST_FOREACH(size_t key,keys_)
+  for(size_t key: keys_)
     sets[find(key)].insert(key);
   return sets;
 }
@@ -110,7 +109,7 @@ std::map<size_t, std::set<size_t> > DSFVector::sets() const {
 /* ************************************************************************* */
 std::map<size_t, std::vector<size_t> > DSFVector::arrays() const {
   std::map<size_t, std::vector<size_t> > arrays;
-  BOOST_FOREACH(size_t key,keys_)
+  for(size_t key: keys_)
     arrays[find(key)].push_back(key);
   return arrays;
 }

gtsam/base/Matrix.cpp

@@ -23,7 +23,6 @@
 #include <Eigen/SVD>
 #include <Eigen/LU>
 
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/tokenizer.hpp>
 
@@ -190,7 +189,7 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
   // Copy coefficients to matrix
   destinationMatrix.resize(height, width);
   int row = 0;
-  BOOST_FOREACH(const vector<double>& rowVec, coeffs) {
+  for(const vector<double>& rowVec: coeffs) {
     destinationMatrix.row(row) = Eigen::Map<const Eigen::RowVectorXd>(&rowVec[0], width);
     ++ row;
   }
@@ -419,7 +418,7 @@ Matrix stack(size_t nrMatrices, ...)
 Matrix stack(const std::vector<Matrix>& blocks) {
   if (blocks.size() == 1) return blocks.at(0);
   DenseIndex nrows = 0, ncols = blocks.at(0).cols();
-  BOOST_FOREACH(const Matrix& mat, blocks) {
+  for(const Matrix& mat: blocks) {
     nrows += mat.rows();
     if (ncols != mat.cols())
       throw invalid_argument("Matrix::stack(): column size mismatch!");
@@ -427,7 +426,7 @@ Matrix stack(const std::vector<Matrix>& blocks) {
   Matrix result(nrows, ncols);
 
   DenseIndex cur_row = 0;
-  BOOST_FOREACH(const Matrix& mat, blocks) {
+  for(const Matrix& mat: blocks) {
     result.middleRows(cur_row, mat.rows()) = mat;
     cur_row += mat.rows();
   }
@@ -441,7 +440,7 @@ Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
   size_t dimA1 = m;
   size_t dimA2 = n*matrices.size();
   if (!m && !n) {
-    BOOST_FOREACH(const Matrix* M, matrices) {
+    for(const Matrix* M: matrices) {
       dimA1 =  M->rows();  // TODO: should check if all the same !
       dimA2 += M->cols();
     }
@@ -450,7 +449,7 @@ Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
   // stl::copy version
   Matrix A(dimA1, dimA2);
   size_t hindex = 0;
-  BOOST_FOREACH(const Matrix* M, matrices) {
+  for(const Matrix* M: matrices) {
     size_t row_len = M->cols();
     A.block(0, hindex, dimA1, row_len) = *M;
     hindex += row_len;
@@ -611,7 +610,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
     boost::tokenizer<boost::char_separator<char> > tok(matrixStr, boost::char_separator<char>("\n"));
 
     DenseIndex row = 0;
-    BOOST_FOREACH(const std::string& line, tok)
+    for(const std::string& line: tok)
     {
       assert(row < effectiveRows);
       if(row > 0)

gtsam/base/TestableAssertions.h

@@ -20,7 +20,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/global_includes.h>
 
-#include <boost/foreach.hpp>
 #include <boost/optional.hpp>
 #include <map>
 #include <iostream>
@@ -91,7 +90,7 @@ bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual,
     match = false;
   if(match) {
     size_t i = 0;
-    BOOST_FOREACH(const V& a, expected) {
+    for(const V& a: expected) {
       if (!assert_equal(a, actual[i++], tol)) {
         match = false;
         break;
@@ -100,9 +99,9 @@ bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual,
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; }
+    for(const V& a: expected) { std::cout << a << " "; }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; }
+    for(const V& a: actual) { std::cout << a << " "; }
     std::cout << std::endl;
     return false;
   }
@@ -133,12 +132,12 @@ bool assert_container_equal(const std::map<V1,V2>& expected, const std::map<V1,V
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const typename Map::value_type& a, expected) {
+    for(const typename Map::value_type& a: expected) {
       a.first.print("key");
       a.second.print("    value");
     }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const typename Map::value_type& a, actual)  {
+    for(const typename Map::value_type& a: actual)  {
       a.first.print("key");
       a.second.print("    value");
     }
@@ -171,12 +170,12 @@ bool assert_container_equal(const std::map<size_t,V2>& expected, const std::map<
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const typename Map::value_type& a, expected) {
+    for(const typename Map::value_type& a: expected) {
       std::cout << "Key: " << a.first << std::endl;
       a.second.print("    value");
     }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const typename Map::value_type& a, actual)  {
+    for(const typename Map::value_type& a: actual)  {
       std::cout << "Key: " << a.first << std::endl;
       a.second.print("    value");
     }
@@ -210,12 +209,12 @@ bool assert_container_equal(const std::vector<std::pair<V1,V2> >& expected,
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const typename VectorPair::value_type& a, expected) {
+    for(const typename VectorPair::value_type& a: expected) {
       a.first.print( "    first ");
       a.second.print("    second");
     }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const typename VectorPair::value_type& a, actual)  {
+    for(const typename VectorPair::value_type& a: actual)  {
       a.first.print( "    first ");
       a.second.print("    second");
     }
@@ -247,9 +246,9 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const typename V::value_type& a, expected) { a.print("  "); }
+    for(const typename V::value_type& a: expected) { a.print("  "); }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const typename V::value_type& a, actual) { a.print("  "); }
+    for(const typename V::value_type& a: actual) { a.print("  "); }
     std::cout << std::endl;
     return false;
   }
@@ -279,12 +278,12 @@ bool assert_container_equality(const std::map<size_t,V2>& expected, const std::m
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const typename Map::value_type& a, expected) {
+    for(const typename Map::value_type& a: expected) {
       std::cout << "Key:   " << a.first << std::endl;
       std::cout << "Value: " << a.second << std::endl;
     }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const typename Map::value_type& a, actual)  {
+    for(const typename Map::value_type& a: actual)  {
       std::cout << "Key:   " << a.first << std::endl;
       std::cout << "Value: " << a.second << std::endl;
     }
@@ -316,9 +315,9 @@ bool assert_container_equality(const V& expected, const V& actual) {
   }
   if(!match) {
     std::cout << "expected: " << std::endl;
-    BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; }
+    for(const typename V::value_type& a: expected) { std::cout << a << " "; }
     std::cout << "\nactual: " << std::endl;
-    BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; }
+    for(const typename V::value_type& a: actual) { std::cout << a << " "; }
     std::cout << std::endl;
     return false;
   }

gtsam/base/Vector.cpp

@@ -17,7 +17,6 @@
  */
 
 #include <gtsam/base/Vector.h>
-#include <boost/foreach.hpp>
 #include <boost/optional.hpp>
 #include <stdexcept>
 #include <cstdarg>
@@ -264,12 +263,12 @@ weightedPseudoinverse(const Vector& a, const Vector& weights) {
 Vector concatVectors(const std::list<Vector>& vs)
 {
   size_t dim = 0;
-  BOOST_FOREACH(Vector v, vs)
+  for(Vector v: vs)
   dim += v.size();
 
   Vector A(dim);
   size_t index = 0;
-  BOOST_FOREACH(Vector v, vs) {
+  for(Vector v: vs) {
     for(int d = 0; d < v.size(); d++)
       A(d+index) = v(d);
     index += v.size();

gtsam/base/tests/testDSFVector.cpp

@@ -20,7 +20,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/std/set.hpp>
@@ -70,7 +69,7 @@ TEST(DSFBase, mergePairwiseMatches) {
 
   // Merge matches
   DSFBase dsf(7); // We allow for keys 0..6
-  BOOST_FOREACH(const Match& m, matches)
+  for(const Match& m: matches)
     dsf.merge(m.first,m.second);
 
   // Each point is now associated with a set, represented by one of its members
@@ -206,7 +205,7 @@ TEST(DSFVector, mergePairwiseMatches) {
 
   // Merge matches
   DSFVector dsf(keys);
-  BOOST_FOREACH(const Match& m, matches)
+  for(const Match& m: matches)
     dsf.merge(m.first,m.second);
 
   // Check that we have two connected components, 1,2,3 and 4,5,6

gtsam/base/tests/testGroup.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/base/Testable.h>
 #include <Eigen/Core>
 #include <iostream>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 

gtsam/base/tests/testMatrix.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/base/testLie.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/tuple/tuple.hpp>
-#include <boost/foreach.hpp>
 #include <iostream>
 #include <sstream>
 
@@ -907,10 +906,6 @@ TEST(Matrix, weighted_elimination )
   Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2).finished();
   Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
 
-  Vector r;
-  double di, sigma;
-  size_t i;
-
   // perform elimination
   Matrix A1 = A;
   Vector b1 = b;
@@ -918,8 +913,11 @@ TEST(Matrix, weighted_elimination )
       weighted_eliminate(A1, b1, sigmas);
 
   // unpack and verify
-  i = 0;
+  size_t i = 0;
-  BOOST_FOREACH(boost::tie(r, di, sigma), solution){
+  for (const auto& tuple : solution) {
+    Vector r;
+    double di, sigma;
+    boost::tie(r, di, sigma) = tuple;
     EXPECT(assert_equal(r, expectedR.row(i))); // verify r
     DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
     DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma

gtsam/base/timing.cpp

@@ -20,7 +20,6 @@
 #include <gtsam/base/timing.h>
 
 #include <boost/algorithm/string/replace.hpp>
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 
 #include <cmath>
@@ -66,7 +65,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) :
 size_t TimingOutline::time() const {
   size_t time = 0;
   bool hasChildren = false;
-  BOOST_FOREACH(const ChildMap::value_type& child, children_) {
+  for(const ChildMap::value_type& child: children_) {
     time += child.second->time();
     hasChildren = true;
   }
@@ -86,11 +85,11 @@ void TimingOutline::print(const std::string& outline) const {
   // Order children
   typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildOrder;
   ChildOrder childOrder;
-  BOOST_FOREACH(const ChildMap::value_type& child, children_) {
+  for(const ChildMap::value_type& child: children_) {
     childOrder[child.second->myOrder_] = child.second;
   }
   // Print children
-  BOOST_FOREACH(const ChildOrder::value_type order_child, childOrder) {
+  for(const ChildOrder::value_type order_child: childOrder) {
     std::string childOutline(outline);
     childOutline += "|   ";
     order_child.second->print(childOutline);
@@ -130,7 +129,7 @@ void TimingOutline::print2(const std::string& outline,
     std::cout << std::endl;
   }
 
-  BOOST_FOREACH(const ChildMap::value_type& child, children_) {
+  for(const ChildMap::value_type& child: children_) {
     std::string childOutline(outline);
     if (n_ == 0) {
       child.second->print2(childOutline, childTotal);
@@ -210,7 +209,7 @@ void TimingOutline::finishedIteration() {
   if (tMin_ == 0 || tIt_ < tMin_)
     tMin_ = tIt_;
   tIt_ = 0;
-  BOOST_FOREACH(ChildMap::value_type& child, children_) {
+  for(ChildMap::value_type& child: children_) {
     child.second->finishedIteration();
   }
 }

gtsam/base/treeTraversal-inst.h

@@ -29,7 +29,6 @@
 #include <string>
 #include <boost/shared_ptr.hpp>
 #include <boost/make_shared.hpp>
-#include <boost/foreach.hpp>
 #include <boost/bind.hpp>
 
 namespace gtsam {
@@ -91,7 +90,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
   // Add roots to stack (insert such that they are visited and processed in order
   {
     typename Stack::iterator insertLocation = stack.begin();
-    BOOST_FOREACH(const sharedNode& root, forest.roots())
+    for(const sharedNode& root: forest.roots())
       stack.insert(insertLocation, TraversalNode(root, rootData));
   }
 
@@ -112,7 +111,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
       node.dataPointer = dataList.insert(dataList.end(),
           visitorPre(node.treeNode, node.parentData));
       typename Stack::iterator insertLocation = stack.begin();
-      BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
+      for(const sharedNode& child: node.treeNode->children)
         stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
       node.expanded = true;
     }

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -20,7 +20,6 @@
 
 #include <boost/shared_ptr.hpp>
 #include <boost/make_shared.hpp>
-#include <boost/foreach.hpp>
 
 #ifdef GTSAM_USE_TBB
 #  include <tbb/tbb.h>
@@ -101,7 +100,7 @@ namespace gtsam {
 
                 tbb::task* firstChild = 0;
                 tbb::task_list childTasks;
-                BOOST_FOREACH(const boost::shared_ptr<NODE>& child, treeNode->children)
+                for(const boost::shared_ptr<NODE>& child: treeNode->children)
                 {
                   // Process child in a subtask.  Important:  Run visitorPre before calling
                   // allocate_child so that if visitorPre throws an exception, we will not have
@@ -143,7 +142,7 @@ namespace gtsam {
 
         void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
         {
-          BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
+          for(const boost::shared_ptr<NODE>& child: node->children)
           {
             DATA childData = visitorPre(child, myData);
             processNodeRecursively(child, childData);
@@ -174,7 +173,7 @@ namespace gtsam {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children
           tbb::task_list tasks;
-          BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots)
+          for(const boost::shared_ptr<NODE>& root: roots)
           {
             boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
             tasks.push_back(*new(allocate_child())

gtsam/base/treeTraversal/statistics.h

@@ -63,7 +63,7 @@ namespace gtsam {
         {
           int largestProblemSize = 0;
           int secondLargestProblemSize = 0;
-          BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
+          for(const boost::shared_ptr<NODE>& child: node->children)
           {
             if (child->problemSize() > largestProblemSize)
             {

gtsam/discrete/Assignment.h

@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include <boost/foreach.hpp>
 #include <iostream>
 #include <vector>
 #include <map>
@@ -36,7 +35,7 @@ namespace gtsam {
   public:
     void print(const std::string& s = "Assignment: ") const {
       std::cout << s << ": ";
-      BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this)
+      for(const typename Assignment::value_type& keyValue: *this)
         std::cout << "(" << keyValue.first << ", " << keyValue.second << ")";
       std::cout << std::endl;
     }
@@ -65,7 +64,7 @@ namespace gtsam {
     std::vector<Assignment<L> > allPossValues;
     Assignment<L> values;
     typedef std::pair<L, size_t> DiscreteKey;
-    BOOST_FOREACH(const DiscreteKey& key, keys)
+    for(const DiscreteKey& key: keys)
       values[key.first] = 0;  //Initialize from 0
     while (1) {
       allPossValues.push_back(values);

gtsam/discrete/DecisionTree-inl.h

@@ -24,7 +24,6 @@
 
 #include <boost/format.hpp>
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/vector.hpp>
 using boost::assign::operator+=;
@@ -310,7 +309,7 @@ namespace gtsam {
       label_(label), allSame_(true) {
 
       branches_.reserve(f.branches_.size()); // reserve space
-      BOOST_FOREACH (const NodePtr& branch, f.branches_)
+      for (const NodePtr& branch: f.branches_)
               push_back(branch->apply(op));
     }
 
@@ -332,7 +331,7 @@ namespace gtsam {
     // If second argument of binary op is Leaf node, recurse on branches
     NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
       boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
-      BOOST_FOREACH(NodePtr branch, branches_)
+      for(NodePtr branch: branches_)
               h->push_back(fL.apply_f_op_g(*branch, op));
       return Unique(h);
     }
@@ -347,7 +346,7 @@ namespace gtsam {
     template<typename OP>
     NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const {
       boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
-      BOOST_FOREACH(const NodePtr& branch, branches_)
+      for(const NodePtr& branch: branches_)
               h->push_back(branch->apply_f_op_g(gL, op));
       return Unique(h);
     }
@@ -359,7 +358,7 @@ namespace gtsam {
     
       // second case, not label of interest, just recurse
       boost::shared_ptr<Choice> r(new Choice(label_, branches_.size()));
-      BOOST_FOREACH(const NodePtr& branch, branches_)
+      for(const NodePtr& branch: branches_)
               r->push_back(branch->choose(label, index));
       return Unique(r);
     }
@@ -593,7 +592,7 @@ namespace gtsam {
 
     // put together via Shannon expansion otherwise not sorted.
     std::vector<LY> functions;
-    BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) {
+    for(const MXNodePtr& branch: choice->branches()) {
       LY converted(convert<M, X>(branch, map, op));
       functions += converted;
     }

gtsam/discrete/DecisionTreeFactor.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/base/FastSet.h>
 
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 
 using namespace std;
@@ -66,11 +65,11 @@ namespace gtsam {
     ADT::Binary op) const {
     map<Key,size_t> cs; // new cardinalities
     // make unique key-cardinality map
-    BOOST_FOREACH(Key j, keys()) cs[j] = cardinality(j);
+    for(Key j: keys()) cs[j] = cardinality(j);
-    BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j);
+    for(Key j: f.keys()) cs[j] = f.cardinality(j);
     // Convert map into keys
     DiscreteKeys keys;
-    BOOST_FOREACH(const DiscreteKey& key, cs)
+    for(const DiscreteKey& key: cs)
       keys.push_back(key);
     // apply operand
     ADT result = ADT::apply(f, op);

gtsam/discrete/DecisionTreeFactor.h

@@ -22,7 +22,6 @@
 #include <gtsam/discrete/Potentials.h>
 #include <gtsam/inference/Ordering.h>
 
-#include <boost/foreach.hpp>
 #include <boost/shared_ptr.hpp>
 
 #include <vector>

gtsam/discrete/DiscreteBayesNet.cpp

@@ -19,7 +19,9 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/inference/FactorGraph-inst.h>
+
 #include <boost/make_shared.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 
 namespace gtsam {
 
@@ -46,7 +48,7 @@ namespace gtsam {
   double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) const {
     // evaluate all conditionals and multiply
     double result = 1.0;
-    BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, *this)
+    for(DiscreteConditional::shared_ptr conditional: *this)
       result *= (*conditional)(values);
     return result;
   }
@@ -55,7 +57,7 @@ namespace gtsam {
   DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const {
     // solve each node in turn in topological sort order (parents first)
     DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
-    BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, *this)
+    for (auto conditional: boost::adaptors::reverse(*this))
       conditional->solveInPlace(*result);
     return result;
   }
@@ -64,7 +66,7 @@ namespace gtsam {
   DiscreteFactor::sharedValues DiscreteBayesNet::sample() const {
     // sample each node in turn in topological sort order (parents first)
     DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
-    BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, *this)
+    for (auto conditional: boost::adaptors::reverse(*this))
       conditional->sampleInPlace(*result);
     return result;
   }

gtsam/discrete/DiscreteConditional.cpp

@@ -87,7 +87,7 @@ bool DiscreteConditional::equals(const DiscreteFactor& other,
 Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
   ADT pFS(*this);
   Key j; size_t value;
-  BOOST_FOREACH(Key key, parents())
+  for(Key key: parents())
   try {
     j = (key);
     value = parentsValues.at(j);
@@ -111,7 +111,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
   double maxP = 0;
 
   DiscreteKeys keys;
-  BOOST_FOREACH(Key idx, frontals()) {
+  for(Key idx: frontals()) {
     DiscreteKey dk(idx, cardinality(idx));
     keys & dk;
   }
@@ -119,7 +119,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
   vector<Values> allPosbValues = cartesianProduct(keys);
 
   // Find the MPE
-  BOOST_FOREACH(Values& frontalVals, allPosbValues) {
+  for(Values& frontalVals: allPosbValues) {
     double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues)
     // Update MPE solution if better
     if (pValueS > maxP) {
@@ -129,7 +129,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
   }
 
   //set values (inPlace) to mpe
-  BOOST_FOREACH(Key j, frontals()) {
+  for(Key j: frontals()) {
     values[j] = mpe[j];
   }
 }

gtsam/discrete/DiscreteFactor.cpp

@@ -18,7 +18,6 @@
  */
 
 #include <gtsam/discrete/DiscreteFactor.h>
-#include <boost/foreach.hpp>
 
 using namespace std;
 

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -41,7 +41,7 @@ namespace gtsam {
   /* ************************************************************************* */
   KeySet DiscreteFactorGraph::keys() const {
     KeySet keys;
-    BOOST_FOREACH(const sharedFactor& factor, *this)
+    for(const sharedFactor& factor: *this)
     if (factor) keys.insert(factor->begin(), factor->end());
     return keys;
   }
@@ -49,7 +49,7 @@ namespace gtsam {
   /* ************************************************************************* */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
     DecisionTreeFactor result;
-    BOOST_FOREACH(const sharedFactor& factor, *this)
+    for(const sharedFactor& factor: *this)
       if (factor) result = (*factor) * result;
     return result;
   }
@@ -58,7 +58,7 @@ namespace gtsam {
   double DiscreteFactorGraph::operator()(
       const DiscreteFactor::Values &values) const {
     double product = 1.0;
-    BOOST_FOREACH( const sharedFactor& factor, factors_ )
+    for( const sharedFactor& factor: factors_ )
       product *= (*factor)(values);
     return product;
   }
@@ -78,7 +78,7 @@ namespace gtsam {
 //  /* ************************************************************************* */
 //  void DiscreteFactorGraph::permuteWithInverse(
 //    const Permutation& inversePermutation) {
-//      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+//      for(const sharedFactor& factor: factors_) {
 //        if(factor)
 //          factor->permuteWithInverse(inversePermutation);
 //      }
@@ -87,7 +87,7 @@ namespace gtsam {
 //  /* ************************************************************************* */
 //  void DiscreteFactorGraph::reduceWithInverse(
 //    const internal::Reduction& inverseReduction) {
-//      BOOST_FOREACH(const sharedFactor& factor, factors_) {
+//      for(const sharedFactor& factor: factors_) {
 //        if(factor)
 //          factor->reduceWithInverse(inverseReduction);
 //      }
@@ -107,7 +107,7 @@ namespace gtsam {
     // PRODUCT: multiply all factors
     gttic(product);
     DecisionTreeFactor product;
-    BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors)
+    for(const DiscreteFactor::shared_ptr& factor: factors)
       product = (*factor) * product;
     gttoc(product);
 

gtsam/discrete/DiscreteKey.cpp

@@ -18,7 +18,6 @@
 
 #include <iostream>
 #include <boost/format.hpp> // for key names
-#include <boost/foreach.hpp> // FOREACH
 #include "DiscreteKey.h"
 
 namespace gtsam {
@@ -34,7 +33,7 @@ namespace gtsam {
 
   vector<Key> DiscreteKeys::indices() const {
     vector < Key > js;
-    BOOST_FOREACH(const DiscreteKey& key, *this)
+    for(const DiscreteKey& key: *this)
       js.push_back(key.first);
     return js;
   }
@@ -42,7 +41,7 @@ namespace gtsam {
   map<Key,size_t> DiscreteKeys::cardinalities() const {
     map<Key,size_t> cs;
     cs.insert(begin(),end());
-//    BOOST_FOREACH(const DiscreteKey& key, *this)
+//    for(const DiscreteKey& key: *this)
 //      cs.insert(key);
     return cs;
   }

gtsam/discrete/Potentials.cpp

@@ -54,7 +54,7 @@ namespace gtsam {
   void Potentials::print(const string& s,
       const KeyFormatter& formatter) const {
     cout << s << "\n  Cardinalities: ";
-    BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
+    for(const DiscreteKey& key: cardinalities_)
       cout << formatter(key.first) << "=" << formatter(key.second) << " ";
     cout << endl;
     ADT::print(" ");
@@ -68,11 +68,11 @@ namespace gtsam {
 //    map<Key, Key> ordering;
 //
 //    // Get the original keys from cardinalities_
-//    BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
+//    for(const DiscreteKey& key: cardinalities_)
 //      keys & key;
 //
 //    // Perform Permutation
-//    BOOST_FOREACH(DiscreteKey& key, keys) {
+//    for(DiscreteKey& key: keys) {
 //      ordering[key.first] = remapping[key.first];
 //      key.first = ordering[key.first];
 //    }

gtsam/discrete/Signature.cpp

@@ -17,7 +17,6 @@
  */
 
 #include <sstream>
-#include <boost/foreach.hpp>
 
 #include "Signature.h"
 
@@ -125,7 +124,7 @@ namespace gtsam {
 
   DiscreteKeys Signature::discreteKeysParentsFirst() const {
     DiscreteKeys keys;
-    BOOST_FOREACH(const DiscreteKey& key, parents_)
+    for(const DiscreteKey& key: parents_)
       keys.push_back(key);
     keys.push_back(key_);
     return keys;
@@ -134,7 +133,7 @@ namespace gtsam {
   vector<Key> Signature::indices() const {
     vector<Key> js;
     js.push_back(key_.first);
-    BOOST_FOREACH(const DiscreteKey& key, parents_)
+    for(const DiscreteKey& key: parents_)
       js.push_back(key.first);
     return js;
   }
@@ -142,8 +141,8 @@ namespace gtsam {
   vector<double> Signature::cpt() const {
     vector<double> cpt;
     if (table_) {
-      BOOST_FOREACH(const Row& row, *table_)
+      for(const Row& row: *table_)
-              BOOST_FOREACH(const double& x, row)
+              for(const double& x: row)
                       cpt.push_back(x);
     }
     return cpt;
@@ -171,7 +170,7 @@ namespace gtsam {
 //        qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar
           parser::parse_table(spec, table);
     if (success) {
-      BOOST_FOREACH(Row& row, table)
+      for(Row& row: table)
         normalize(row);
       table_.reset(table);
     }
@@ -180,7 +179,7 @@ namespace gtsam {
 
   Signature& Signature::operator=(const Table& t) {
     Table table = t;
-    BOOST_FOREACH(Row& row, table)
+    for(Row& row: table)
       normalize(row);
     table_.reset(table);
     return *this;

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -25,7 +25,6 @@
 #define DISABLE_TIMING
 
 #include <boost/timer.hpp>
-#include <boost/foreach.hpp>
 #include <boost/tokenizer.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/assign/std/vector.hpp>
@@ -66,7 +65,7 @@ void dot(const T&f, const string& filename) {
  typename DecisionTree<L, double>::Node::Ptr DecisionTree<L, double>::Choice::apply_fC_op_gL(
  Cache& cache, const Leaf& gL, Mul op) const {
  Ptr h(new Choice(label(), cardinality()));
- BOOST_FOREACH(const NodePtr& branch, branches_)
+ for(const NodePtr& branch: branches_)
  h->push_back(branch->apply_f_op_g(cache, gL, op));
  return Unique(cache, h);
  }
@@ -401,7 +400,7 @@ TEST(ADT, constructor)
   DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2);
   vector<double> table(5 * 4 * 3 * 2);
   double x = 0;
-  BOOST_FOREACH(double& t, table)
+  for(double& t: table)
   t = x++;
   ADT f3(z0 & z1 & z2 & z3, table);
   Assignment<Key> assignment;

gtsam/discrete/tests/testDiscreteBayesTree.cpp

@@ -63,7 +63,7 @@
 ////  double evaluate(const DiscreteConditional::Values & values) {
 ////    double result = (*(this->conditional_))(values);
 ////    // evaluate all children and multiply into result
-////    BOOST_FOREACH(boost::shared_ptr<Clique> c, children_)
+////    for(boost::shared_ptr<Clique> c: children_)
 ////      result *= c->evaluate(values);
 ////    return result;
 ////  }
@@ -213,7 +213,7 @@
 //
 //  // calculate all shortcuts to root
 //  DiscreteBayesTree::Nodes cliques = bayesTree.nodes();
-//  BOOST_FOREACH(Clique::shared_ptr c, cliques) {
+//  for(Clique::shared_ptr c: cliques) {
 //    DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete);
 //    if (debug) {
 //      c->printSignature();

gtsam/discrete/tests/testDiscreteFactorGraph.cpp

@@ -321,7 +321,7 @@ struct Graph2: public std::list<Factor2> {
 
 /** Add a factor graph*/
 //  void operator +=(const Graph2& graph) {
-//    BOOST_FOREACH(const Factor2& f, graph)
+//    for(const Factor2& f: graph)
 //        push_back(f);
 //  }
 friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);
@@ -334,7 +334,7 @@ friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);
 //  return graph;
 //}
 std::ostream& operator <<(std::ostream &os, const Graph2& graph) {
-BOOST_FOREACH(const Factor2& f, graph)
+for(const Factor2& f: graph)
 os << f << endl;
 return os;
 }

gtsam/geometry/PinholeSet.h

@@ -20,7 +20,6 @@
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/triangulation.h>
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 

gtsam/geometry/Point2.cpp

@@ -16,7 +16,6 @@
  */
 
 #include <gtsam/geometry/Point2.h>
-#include <boost/foreach.hpp>
 #include <cmath>
 
 using namespace std;

gtsam/geometry/Pose2.cpp

@@ -19,8 +19,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/concepts.h>
 
-#include <boost/foreach.hpp>
-
 #include <cmath>
 #include <iostream>
 #include <iomanip>
@@ -314,7 +312,7 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
 
   // calculate centroids
   Point2 cp,cq;
-  BOOST_FOREACH(const Point2Pair& pair, pairs) {
+  for(const Point2Pair& pair: pairs) {
     cp += pair.first;
     cq += pair.second;
   }
@@ -323,7 +321,7 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
 
   // calculate cos and sin
   double c=0,s=0;
-  BOOST_FOREACH(const Point2Pair& pair, pairs) {
+  for(const Point2Pair& pair: pairs) {
     Point2 dq = pair.first  - cp;
     Point2 dp = pair.second - cq;
     c +=  dp.x() * dq.x() + dp.y() * dq.y();

gtsam/geometry/Pose3.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/geometry/concepts.h>
 #include <gtsam/base/concepts.h>
 
-#include <boost/foreach.hpp>
 #include <iostream>
 #include <cmath>
 
@@ -424,7 +423,7 @@ boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs)
 
 boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
   vector<Point3Pair> abPointPairs;
-  BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) {
+  for (const Point3Pair& baPair: baPointPairs) {
     abPointPairs.push_back(make_pair(baPair.second, baPair.first));
   }
   return Pose3::Align(abPointPairs);

gtsam/geometry/tests/testPose2.cpp

@@ -22,7 +22,6 @@
 #include <gtsam/base/lieProxies.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/foreach.hpp>
 #include <boost/optional.hpp>
 #include <boost/assign/std/vector.hpp> // for operator +=
 #include <cmath>

gtsam/geometry/tests/testUnit3.cpp

@@ -33,7 +33,6 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/bind.hpp>
-#include <boost/foreach.hpp>
 #include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
 #include <cmath>
@@ -56,7 +55,7 @@ TEST(Unit3, point3) {
   ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
       / sqrt(2.0);
   Matrix actualH, expectedH;
-  BOOST_FOREACH(Point3 p,ps) {
+  for(Point3 p: ps) {
     Unit3 s(p);
     expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
     EXPECT(assert_equal(p, s.point3(actualH), 1e-8));

gtsam/geometry/triangulation.h

@@ -237,7 +237,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // construct projection matrices from poses & calibration
   std::vector<Matrix34> projection_matrices;
   CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
-  BOOST_FOREACH(const Pose3& pose, poses)
+  for(const Pose3& pose: poses)
     projection_matrices.push_back(createP(pose));
 
   // Triangulate linearly
@@ -250,7 +250,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
-  BOOST_FOREACH(const Pose3& pose, poses) {
+  for(const Pose3& pose: poses) {
     const Point3& p_local = pose.transform_to(point);
     if (p_local.z() <= 0)
       throw(TriangulationCheiralityException());
@@ -286,7 +286,7 @@ Point3 triangulatePoint3(
 
   // construct projection matrices from poses & calibration
   std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const CAMERA& camera, cameras)
+  for(const CAMERA& camera: cameras)
     projection_matrices.push_back(
         CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
             camera.pose()));
@@ -298,7 +298,7 @@ Point3 triangulatePoint3(
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
-  BOOST_FOREACH(const CAMERA& camera, cameras) {
+  for(const CAMERA& camera: cameras) {
     const Point3& p_local = camera.pose().transform_to(point);
     if (p_local.z() <= 0)
       throw(TriangulationCheiralityException());
@@ -454,7 +454,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
       // Check landmark distance and re-projection errors to avoid outliers
       size_t i = 0;
       double totalReprojError = 0.0;
-      BOOST_FOREACH(const CAMERA& camera, cameras) {
+      for(const CAMERA& camera: cameras) {
         const Pose3& pose = camera.pose();
         if (params.landmarkDistanceThreshold > 0
             && distance(pose.translation(), point)

gtsam/inference/BayesNet-inst.h

@@ -21,7 +21,7 @@
 #include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam/inference/BayesNet.h>
 
-#include <boost/foreach.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 #include <fstream>
 
 namespace gtsam {
@@ -40,11 +40,11 @@ namespace gtsam {
     std::ofstream of(s.c_str());
     of << "digraph G{\n";
 
-    BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
+    for (auto conditional: boost::adaptors::reverse(*this)) {
       typename CONDITIONAL::Frontals frontals = conditional->frontals();
       Key me = frontals.front();
       typename CONDITIONAL::Parents parents = conditional->parents();
-      BOOST_FOREACH(Key p, parents)
+      for(Key p: parents)
         of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
     }
 

gtsam/inference/BayesTree-inst.h

@@ -26,7 +26,6 @@
 #include <gtsam/base/timing.h>
 
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 #include <boost/assign/list_of.hpp>
 #include <fstream>
 
@@ -38,7 +37,7 @@ namespace gtsam {
   template<class CLIQUE>
   BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
     BayesTreeCliqueData data;
-    BOOST_FOREACH(const sharedClique& root, roots_)
+    for(const sharedClique& root: roots_)
       getCliqueData(data, root);
     return data;
   }
@@ -48,7 +47,7 @@ namespace gtsam {
   void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
     data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
     data.separatorSizes.push_back(clique->conditional()->nrParents());
-    BOOST_FOREACH(sharedClique c, clique->children) {
+    for(sharedClique c: clique->children) {
       getCliqueData(data, c);
     }
   }
@@ -57,7 +56,7 @@ namespace gtsam {
   template<class CLIQUE>
   size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
     size_t count = 0;
-    BOOST_FOREACH(const sharedClique& root, roots_)
+    for(const sharedClique& root: roots_)
       count += root->numCachedSeparatorMarginals();
     return count;
   }
@@ -68,7 +67,7 @@ namespace gtsam {
     if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
     std::ofstream of(s.c_str());
     of<< "digraph G{\n";
-    BOOST_FOREACH(const sharedClique& root, roots_)
+    for(const sharedClique& root: roots_)
       saveGraph(of, root, keyFormatter);
     of<<"}";
     of.close();
@@ -84,7 +83,7 @@ namespace gtsam {
     std::string parent = out.str();
     parent += "[label=\"";
 
-    BOOST_FOREACH(Key index, clique->conditional_->frontals()) {
+    for(Key index: clique->conditional_->frontals()) {
       if(!first) parent += ","; first = false;
       parent += indexFormatter(index);
     }
@@ -95,7 +94,7 @@ namespace gtsam {
     }
 
     first = true;
-    BOOST_FOREACH(Key sep, clique->conditional_->parents()) {
+    for(Key sep: clique->conditional_->parents()) {
       if(!first) parent += ","; first = false;
       parent += indexFormatter(sep);
     }
@@ -103,7 +102,7 @@ namespace gtsam {
     s << parent;
     parentnum = num;
 
-    BOOST_FOREACH(sharedClique c, clique->children) {
+    for(sharedClique c: clique->children) {
       num++;
       saveGraph(s, c, indexFormatter, parentnum);
     }
@@ -113,7 +112,7 @@ namespace gtsam {
   template<class CLIQUE>
   size_t BayesTree<CLIQUE>::size() const {
     size_t size = 0;
-    BOOST_FOREACH(const sharedClique& clique, roots_)
+    for(const sharedClique& clique: roots_)
       size += clique->treeSize();
     return size;
   }
@@ -121,7 +120,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CLIQUE>
   void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
-    BOOST_FOREACH(Key j, clique->conditional()->frontals())
+    for(Key j: clique->conditional()->frontals())
       nodes_[j] = clique;
     if (parent_clique != NULL) {
       clique->parent_ = parent_clique;
@@ -189,7 +188,7 @@ namespace gtsam {
     this->clear();
     boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
     treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
-    BOOST_FOREACH(const sharedClique& root, rootContainer->children) {
+    for(const sharedClique& root: rootContainer->children) {
       root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique
       insertRoot(root);
     }
@@ -234,13 +233,13 @@ namespace gtsam {
   template<class CLIQUE>
   void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
     // Add each frontal variable of this root node
-    BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) {
+    for(const Key& j: subtree->conditional()->frontals()) {
       bool inserted = nodes_.insert(std::make_pair(j, subtree)).second;
       assert(inserted); (void)inserted;
     }
     // Fill index for each child
     typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
-    BOOST_FOREACH(const sharedClique& child, subtree->children) {
+    for(const sharedClique& child: subtree->children) {
       fillNodesIndex(child); }
   }
 
@@ -345,7 +344,7 @@ namespace gtsam {
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
         FastVector<Key> C1_minus_B; {
           KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
-          BOOST_FOREACH(const Key j, *B->conditional()) {
+          for(const Key j: *B->conditional()) {
             C1_minus_B_set.erase(j); }
           C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
         }
@@ -357,7 +356,7 @@ namespace gtsam {
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
         FastVector<Key> C2_minus_B; {
           KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
-          BOOST_FOREACH(const Key j, *B->conditional()) {
+          for(const Key j: *B->conditional()) {
             C2_minus_B_set.erase(j); }
           C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
         }
@@ -403,7 +402,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CLIQUE>
   void BayesTree<CLIQUE>::deleteCachedShortcuts() {
-    BOOST_FOREACH(const sharedClique& root, roots_) {
+    for(const sharedClique& root: roots_) {
       root->deleteCachedShortcuts();
     }
   }
@@ -424,10 +423,10 @@ namespace gtsam {
     }
 
     // orphan my children
-    BOOST_FOREACH(sharedClique child, clique->children)
+    for(sharedClique child: clique->children)
       child->parent_ = typename Clique::weak_ptr();
 
-    BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
+    for(Key j: clique->conditional()->frontals()) {
       nodes_.unsafe_erase(j);
     }
   }
@@ -462,7 +461,7 @@ namespace gtsam {
   void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans)
   {
     // process each key of the new factor
-    BOOST_FOREACH(const Key& j, keys)
+    for(const Key& j: keys)
     {
       // get the clique
       // TODO: Nodes will be searched again in removeClique
@@ -475,7 +474,7 @@ namespace gtsam {
 
     // Delete cachedShortcuts for each orphan subtree
     //TODO: Consider Improving
-    BOOST_FOREACH(sharedClique& orphan, orphans)
+    for(sharedClique& orphan: orphans)
       orphan->deleteCachedShortcuts();
   }
 
@@ -499,14 +498,14 @@ namespace gtsam {
     for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
     {
       // Add children
-      BOOST_FOREACH(const sharedClique& child, (*clique)->children) {
+      for(const sharedClique& child: (*clique)->children) {
         cliques.push_back(child); }
 
       // Delete cached shortcuts
       (*clique)->deleteCachedShortcutsNonRecursive();
 
       // Remove this node from the nodes index
-      BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) {
+      for(Key j: (*clique)->conditional()->frontals()) {
         nodes_.unsafe_erase(j); }
 
       // Erase the parent and children pointers

gtsam/inference/BayesTree.cpp

@@ -20,7 +20,6 @@
 
 #include <gtsam/inference/BayesTree.h>
 
-#include <boost/foreach.hpp>
 #include <iostream>
 
 namespace gtsam {
@@ -41,7 +40,7 @@ BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
 
   double sum = 0.0;
   size_t max = 0;
-  BOOST_FOREACH(size_t s, conditionalSizes) {
+  for(size_t s: conditionalSizes) {
     sum += (double)s;
     if(s > max) max = s;
   }
@@ -50,7 +49,7 @@ BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
 
   sum = 0.0;
   max = 1;
-  BOOST_FOREACH(size_t s, separatorSizes) {
+  for(size_t s: separatorSizes) {
     sum += (double)s;
     if(s > max) max = s;
   }

gtsam/inference/BayesTreeCliqueBase-inst.h

@@ -18,7 +18,6 @@
 
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 #include <gtsam/base/timing.h>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -83,7 +82,7 @@ namespace gtsam {
   template<class DERIVED, class FACTORGRAPH>
   size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
     size_t size = 1;
-    BOOST_FOREACH(const derived_ptr& child, children)
+    for(const derived_ptr& child: children)
       size += child->treeSize();
     return size;
   }
@@ -96,7 +95,7 @@ namespace gtsam {
       return 0;
 
     size_t subtree_count = 1;
-    BOOST_FOREACH(const derived_ptr& child, children)
+    for(const derived_ptr& child: children)
       subtree_count += child->numCachedSeparatorMarginals();
 
     return subtree_count;
@@ -204,7 +203,7 @@ namespace gtsam {
     // root are also generated. So, if this clique's cached shortcut is set,
     // recursively call over all child cliques. Otherwise, it is unnecessary.
     if (cachedSeparatorMarginal_) {
-      BOOST_FOREACH(derived_ptr& child, children) {
+      for(derived_ptr& child: children) {
         child->deleteCachedShortcuts();
       }
 

gtsam/inference/ClusterTree-inst.h

@@ -13,7 +13,6 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
 
-#include <boost/foreach.hpp>
 #include <boost/bind.hpp>
 
 namespace gtsam {
@@ -87,7 +86,7 @@ struct EliminationData {
       gatheredFactors += myData.childFactors;
 
       // Check for Bayes tree orphan subtrees, and add them to our children
-      BOOST_FOREACH(const sharedFactor& f, node->factors) {
+      for(const sharedFactor& f: node->factors) {
         if (const BayesTreeOrphanWrapper<BTNode>* asSubtree =
             dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) {
           myData.bayesTreeNode->children.push_back(asSubtree->clique);
@@ -107,7 +106,7 @@ struct EliminationData {
       // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
       // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
       // object they're added to.
-      BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
+      for(const Key& j: myData.bayesTreeNode->conditional()->frontals())
         nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
 
       // Store remaining factor in parent's gathered factors
@@ -138,7 +137,7 @@ void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
   size_t nrNewChildren = 0;
   // Loop over children
   size_t i = 0;
-  BOOST_FOREACH(const sharedNode& child, children) {
+  for(const sharedNode& child: children) {
     if (merge[i]) {
       nrKeys += child->orderedFrontalKeys.size();
       nrFactors += child->factors.size();
@@ -155,7 +154,7 @@ void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
   typename Node::Children newChildren;
   newChildren.reserve(nrNewChildren);
   i = 0;
-  BOOST_FOREACH(const sharedNode& child, children) {
+  for(const sharedNode& child: children) {
     if (merge[i]) {
       // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
       orderedFrontalKeys.insert(orderedFrontalKeys.end(),
@@ -228,7 +227,7 @@ std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > ClusterTree<
   remaining->reserve(
       remainingFactors_.size() + rootsContainer.childFactors.size());
   remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
-  BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) {
+  for(const sharedFactor& factor: rootsContainer.childFactors) {
     if (factor)
       remaining->push_back(factor);
   }

gtsam/inference/Conditional-inst.h

@@ -18,7 +18,6 @@
 // \callgraph
 #pragma once
 
-#include <boost/foreach.hpp>
 #include <iostream>
 
 #include <gtsam/inference/Conditional.h>
@@ -29,11 +28,11 @@ namespace gtsam {
   template<class FACTOR, class DERIVEDFACTOR>
   void Conditional<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
     std::cout << s << " P(";
-    BOOST_FOREACH(Key key, frontals())
+    for(Key key: frontals())
       std::cout << " " << formatter(key);
     if (nrParents() > 0)
       std::cout << " |";
-    BOOST_FOREACH(Key parent, parents())
+    for(Key parent: parents())
       std::cout << " " << formatter(parent);
     std::cout << ")" << std::endl;
   }

gtsam/inference/EliminationTree-inst.h

@@ -17,7 +17,6 @@
 */
 #pragma once
 
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/bind.hpp>
 #include <stack>
@@ -66,7 +65,7 @@ namespace gtsam {
     const std::string& str, const KeyFormatter& keyFormatter) const
   {
     std::cout << str << "(" << keyFormatter(key) << ")\n";
-    BOOST_FOREACH(const sharedFactor& factor, factors) {
+    for(const sharedFactor& factor: factors) {
       if(factor)
         factor->print(str);
       else
@@ -107,7 +106,7 @@ namespace gtsam {
         // for row i \in Struct[A*j] do
         node->children.reserve(factors.size());
         node->factors.reserve(factors.size());
-        BOOST_FOREACH(const size_t i, factors) {
+        for(const size_t i: factors) {
           // If we already hit a variable in this factor, make the subtree containing the previous
           // variable in this factor a child of the current node.  This means that the variables
           // eliminated earlier in the factor depend on the later variables in the factor.  If we
@@ -222,15 +221,15 @@ namespace gtsam {
     // Add roots in sorted order
     {
       FastMap<Key,sharedNode> keys;
-      BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); }
+      for(const sharedNode& root: this->roots_) { keys.insert(std::make_pair(root->key, root)); }
       typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
-      BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
+      for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
     }
     {
       FastMap<Key,sharedNode> keys;
-      BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
+      for(const sharedNode& root: expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
       typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
-      BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
+      for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
     }
 
     // Traverse, adding children in sorted order
@@ -262,15 +261,15 @@ namespace gtsam {
       // Add children in sorted order
       {
         FastMap<Key,sharedNode> keys;
-        BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); }
+        for(const sharedNode& node: node1->children) { keys.insert(std::make_pair(node->key, node)); }
         typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
-        BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
+        for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
       }
       {
         FastMap<Key,sharedNode> keys;
-        BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); }
+        for(const sharedNode& node: node2->children) { keys.insert(std::make_pair(node->key, node)); }
         typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
-        BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
+        for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
       }
     }
 

gtsam/inference/Factor.cpp

@@ -19,7 +19,6 @@
 
 // \callgraph
 
-#include <boost/foreach.hpp>
 #include <iostream>
 
 #include <gtsam/inference/Factor.h>
@@ -35,7 +34,7 @@ namespace gtsam {
   /* ************************************************************************* */
   void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const {
     std::cout << s << " ";
-    BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key);
+    for(Key key: keys_) std::cout << " " << formatter(key);
     std::cout << std::endl;
   }
 
@@ -44,4 +43,4 @@ namespace gtsam {
     return keys_ == other.keys_;
   }
 
-}
+}

gtsam/inference/FactorGraph-inst.h

@@ -23,7 +23,6 @@
 
 #include <gtsam/inference/FactorGraph.h>
 
-#include <boost/foreach.hpp>
 #include <boost/bind.hpp>
 
 #include <stdio.h>
@@ -66,7 +65,7 @@ namespace gtsam {
   template<class FACTOR>
   size_t FactorGraph<FACTOR>::nrFactors() const {
     size_t size_ = 0;
-    BOOST_FOREACH(const sharedFactor& factor, factors_)
+    for(const sharedFactor& factor: factors_)
       if (factor) size_++;
     return size_;
   }
@@ -75,7 +74,7 @@ namespace gtsam {
   template<class FACTOR>
   KeySet FactorGraph<FACTOR>::keys() const {
     KeySet keys;
-    BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
+    for(const sharedFactor& factor: this->factors_) {
       if(factor)
         keys.insert(factor->begin(), factor->end());
     }
@@ -87,7 +86,7 @@ namespace gtsam {
   KeyVector FactorGraph<FACTOR>::keyVector() const {
     KeyVector keys;
     keys.reserve(2 * size());  // guess at size
-    BOOST_FOREACH (const sharedFactor& factor, factors_)
+    for (const sharedFactor& factor: factors_)
       if (factor)
         keys.insert(keys.end(), factor->begin(), factor->end());
     std::sort(keys.begin(), keys.end());

gtsam/inference/ISAM-inst.h

@@ -39,7 +39,7 @@ namespace gtsam {
     factors += newFactors;
 
     // Add the orphaned subtrees
-    BOOST_FOREACH(const sharedClique& orphan, orphans)
+    for(const sharedClique& orphan: orphans)
       factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
 
     // eliminate into a Bayes net

gtsam/inference/JunctionTree-inst.h

@@ -105,7 +105,7 @@ struct ConstructorTraversalData {
     // decide which children to merge, as index into children
     std::vector<bool> merge(nrChildren, false);
     size_t myNrFrontals = 1, i = 0;
-    BOOST_FOREACH(const sharedNode& child, node->children) {
+    for(const sharedNode& child: node->children) {
       // Check if we should merge the i^th child
       if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
         // Increment number of frontal variables

gtsam/inference/Key.cpp

@@ -20,7 +20,6 @@
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/LabeledSymbol.h>
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 #include <iostream>
 
@@ -63,7 +62,7 @@ static void Print(const CONTAINER& keys, const string& s,
   if (keys.empty())
     cout << "(none)" << endl;
   else {
-    BOOST_FOREACH(const Key& key, keys)
+    for(const Key& key: keys)
       cout << keyFormatter(key) << " ";
     cout << endl;
   }

gtsam/inference/MetisIndex-inl.h

@@ -17,7 +17,6 @@
 
 #pragma once
 
-#include <boost/foreach.hpp>
 #include <map>
 #include <vector>
 
@@ -42,7 +41,7 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
   // key to integer mapping. This is referenced during the adjaceny step
   for (size_t i = 0; i < factors.size(); i++) {
     if (factors[i]) {
-      BOOST_FOREACH(const Key& key, *factors[i]) {
+      for(const Key& key: *factors[i]) {
         keySet.insert(keySet.end(), key); // Keep a track of all unique keys
         if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) {
           intKeyBMap_.insert(bm_type::value_type(key, keyCounter));
@@ -55,8 +54,8 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
   // Create an adjacency mapping that stores the set of all adjacent keys for every key
   for (size_t i = 0; i < factors.size(); i++) {
     if (factors[i]) {
-      BOOST_FOREACH(const Key& k1, *factors[i])
+      for(const Key& k1: *factors[i])
-        BOOST_FOREACH(const Key& k2, *factors[i])
+        for(const Key& k2: *factors[i])
           if (k1 != k2) {
             // Store both in Key and int32_t format
             int i = intKeyBMap_.left.at(k1);

gtsam/inference/VariableIndex-inl.h

@@ -19,7 +19,6 @@
 
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/base/timing.h>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -34,7 +33,7 @@ void VariableIndex::augment(const FG& factors,
     if (factors[i]) {
       const size_t globalI =
           newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
-      BOOST_FOREACH(const Key key, *factors[i]) {
+      for(const Key key: *factors[i]) {
         index_[key].push_back(globalI);
         ++nEntries_;
       }
@@ -67,7 +66,7 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
       throw std::invalid_argument(
           "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
     if (factors[i]) {
-      BOOST_FOREACH(Key j, *factors[i]) {
+      for(Key j: *factors[i]) {
         Factors& factorEntries = internalAt(j);
         Factors::iterator entry = std::find(factorEntries.begin(),
             factorEntries.end(), *factorIndex);

gtsam/inference/VariableIndex.cpp

@@ -33,9 +33,9 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
 void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const {
   cout << str;
   cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
-  BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
+  for(KeyMap::value_type key_factors: index_) {
     cout << "var " << keyFormatter(key_factors.first) << ":";
-    BOOST_FOREACH(const size_t factor, key_factors.second)
+    for(const size_t factor: key_factors.second)
       cout << " " << factor;
     cout << "\n";
   }
@@ -46,9 +46,9 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
 void VariableIndex::outputMetisFormat(ostream& os) const {
   os << size() << " " << nFactors() << "\n";
   // run over variables, which will be hyper-edges.
-  BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
+  for(KeyMap::value_type key_factors: index_) {
     // every variable is a hyper-edge covering its factors
-    BOOST_FOREACH(const size_t factor, key_factors.second)
+    for(const size_t factor: key_factors.second)
       os << (factor+1) << " "; // base 1
     os << "\n";
   }

gtsam/inference/VariableSlots.cpp

@@ -18,7 +18,6 @@
 #include <gtsam/inference/VariableSlots.h>
 
 #include <iostream>
-#include <boost/foreach.hpp>
 
 using namespace std;
 
@@ -33,12 +32,12 @@ void VariableSlots::print(const std::string& str) const {
   else {
     cout << str << "\n";
     cout << "Var:\t";
-    BOOST_FOREACH(const value_type& slot, *this) { cout << slot.first << "\t"; }
+    for(const value_type& slot: *this) { cout << slot.first << "\t"; }
     cout << "\n";
 
     for(size_t i=0; i<this->begin()->second.size(); ++i) {
       cout << "    \t";
-      BOOST_FOREACH(const value_type& slot, *this) {
+      for(const value_type& slot: *this) {
         if(slot.second[i] == Empty)
           cout << "x" << "\t";
         else

gtsam/inference/VariableSlots.h

@@ -24,7 +24,6 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/Testable.h>
 
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 
 #include <iostream>
@@ -99,10 +98,10 @@ VariableSlots::VariableSlots(const FG& factorGraph)
   // removed factors.  The slot number is the max integer value if the
   // factor does not involve that variable.
   size_t jointFactorPos = 0;
-  BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
+  for(const typename FG::sharedFactor& factor: factorGraph) {
     assert(factor);
     size_t factorVarSlot = 0;
-    BOOST_FOREACH(const Key involvedVariable, *factor) {
+    for(const Key involvedVariable: *factor) {
       // Set the slot in this factor for this variable.  If the
       // variable was not already discovered, create an array for it
       // that we'll fill with the slot indices for each factor that

gtsam/inference/graph-inl.h

@@ -18,7 +18,6 @@
 #pragma once
 
 #include <stdexcept>
-#include <boost/foreach.hpp>
 #ifdef __GNUC__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-variable"
@@ -32,8 +31,6 @@
 
 #include <gtsam/inference/graph.h>
 
-#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
-
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -123,9 +120,10 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
   G g;
   std::map<KEY, V> key2vertex;
   V v1, v2, root;
-  KEY child, parent;
   bool foundRoot = false;
-  FOREACH_PAIR(child, parent, p_map) {
+  for(auto child_parent: p_map) {
+    KEY child, parent;
+    std::tie(child,parent) = child_parent;
     if (key2vertex.find(child) == key2vertex.end()) {
        v1 = add_vertex(child, g);
        key2vertex.insert(std::make_pair(child, v1));
@@ -193,7 +191,7 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
   // attach the relative poses to the edges
   PoseEdge edge12, edge21;
   bool found1, found2;
-  BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) {
+  for(typename G::sharedFactor nl_factor: graph) {
 
     if (nl_factor->keys().size() > 2)
       throw std::invalid_argument("composePoses: only support factors with at most two keys");
@@ -243,7 +241,7 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
   // convert edge to string pairs
   PredecessorMap<KEY> tree;
   typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
-  BOOST_FOREACH(const typename SDGraph<KEY>::Vertex& vi, p_map){
+  for(const typename SDGraph<KEY>::Vertex& vi: p_map){
     KEY key = boost::get(boost::vertex_name, g, *itVertex);
     KEY parent = boost::get(boost::vertex_name, g, vi);
     tree.insert(key, parent);
@@ -258,7 +256,7 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
 
   typedef typename G::sharedFactor F ;
 
-  BOOST_FOREACH(const F& factor, g)
+  for(const F& factor: g)
   {
     if (factor->keys().size() > 2)
       throw(std::invalid_argument("split: only support factors with at most two keys"));

gtsam/linear/Errors.cpp

@@ -17,7 +17,6 @@
  * @author  Christian Potthast
  */
 
-#include <boost/foreach.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <gtsam/linear/Errors.h>
 #include <gtsam/linear/VectorValues.h>
@@ -31,7 +30,7 @@ Errors::Errors(){}
 
 /* ************************************************************************* */
 Errors::Errors(const VectorValues& V) {
-  BOOST_FOREACH(const Vector& e, V | boost::adaptors::map_values) {
+  for(const Vector& e: V | boost::adaptors::map_values) {
     push_back(e);
   }
 }
@@ -39,7 +38,7 @@ Errors::Errors(const VectorValues& V) {
 /* ************************************************************************* */
 void Errors::print(const std::string& s) const {
   cout << s << endl;
-  BOOST_FOREACH(const Vector& v, *this)
+  for(const Vector& v: *this)
     gtsam::print(v);
 }
 
@@ -66,7 +65,7 @@ Errors Errors::operator+(const Errors& b) const {
 #endif
   Errors result;
   Errors::const_iterator it = b.begin();
-    BOOST_FOREACH(const Vector& ai, *this)
+    for(const Vector& ai: *this)
     result.push_back(ai + *(it++));
   return result;
 }
@@ -81,7 +80,7 @@ Errors Errors::operator-(const Errors& b) const {
 #endif
   Errors result;
   Errors::const_iterator it = b.begin();
-  BOOST_FOREACH(const Vector& ai, *this)
+  for(const Vector& ai: *this)
     result.push_back(ai - *(it++));
   return result;
 }
@@ -89,7 +88,7 @@ Errors Errors::operator-(const Errors& b) const {
 /* ************************************************************************* */
 Errors Errors::operator-() const {
   Errors result;
-  BOOST_FOREACH(const Vector& ai, *this)
+  for(const Vector& ai: *this)
     result.push_back(-ai);
   return result;
 }
@@ -105,7 +104,7 @@ double dot(const Errors& a, const Errors& b) {
 #endif
   double result = 0.0;
   Errors::const_iterator it = b.begin();
-  BOOST_FOREACH(const Vector& ai, a)
+  for(const Vector& ai: a)
     result += gtsam::dot(ai, *(it++));
   return result;
 }
@@ -114,7 +113,7 @@ double dot(const Errors& a, const Errors& b) {
 template<>
 void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) {
   Errors::const_iterator it = x.begin();
-  BOOST_FOREACH(Vector& yi, y)
+  for(Vector& yi: y)
     axpy(alpha,*(it++),yi);
 }
 

gtsam/linear/GaussianBayesNet.cpp

@@ -20,14 +20,11 @@
 #include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/foreach.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 
 using namespace std;
 using namespace gtsam;
 
-#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) 
-#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
-
 namespace gtsam {
 
   // Instantiate base class
@@ -52,7 +49,7 @@ namespace gtsam {
     VectorValues soln(solutionForMissing); // possibly empty
     // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
     /** solve each node in turn in topological sort order (parents first)*/
-    BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
+    for (auto cg: boost::adaptors::reverse(*this)) {
       // i^th part of R*x=y, x=inv(R)*y
       // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
       soln.insert(cg->solve(soln));
@@ -88,7 +85,7 @@ namespace gtsam {
     VectorValues result;
     // TODO this looks pretty sketchy. result is passed as the parents argument
     //  as it's filled up by solving the gaussian conditionals.
-    BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
+    for (auto cg: boost::adaptors::reverse(*this)) {
       result.insert(cg->solveOtherRHS(result, rhs));
     }
     return result;
@@ -107,7 +104,7 @@ namespace gtsam {
 
     // we loop from first-eliminated to last-eliminated
     // i^th part of L*gy=gx is done block-column by block-column of L
-    BOOST_FOREACH(const sharedConditional& cg, *this)
+    for(const sharedConditional& cg: *this)
       cg->solveTransposeInPlace(gy);
 
     return gy;
@@ -146,15 +143,15 @@ namespace gtsam {
     KeySet keys = factorGraph.keys();
     // add frontal keys in order
     Ordering ordering;
-    BOOST_FOREACH (const sharedConditional& cg, *this)
+    for (const sharedConditional& cg: *this)
       if (cg) {
-        BOOST_FOREACH (Key key, cg->frontals()) {
+        for (Key key: cg->frontals()) {
           ordering.push_back(key);
           keys.erase(key);
         }
       }
     // add remaining keys in case Bayes net is incomplete
-    BOOST_FOREACH (Key key, keys)
+    for (Key key: keys)
       ordering.push_back(key);
     // return matrix and RHS
     return factorGraph.jacobian(ordering);
@@ -182,7 +179,7 @@ namespace gtsam {
   double GaussianBayesNet::logDeterminant() const
   {
     double logDet = 0.0;
-    BOOST_FOREACH(const sharedConditional& cg, *this) {
+    for(const sharedConditional& cg: *this) {
       if(cg->get_model()) {
         Vector diag = cg->get_R().diagonal();
         cg->get_model()->whitenInPlace(diag);

gtsam/linear/GaussianBayesTree-inl.h

@@ -19,8 +19,6 @@
 
 #pragma once
 
-#include <boost/foreach.hpp>
-
 #include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
 
 #include <stdarg.h>
@@ -35,7 +33,7 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue
   clique->conditional()->solveInPlace(result);
 
   // starting from the root, call optimize on each conditional
-  BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
+  for(const typename BAYESTREE::sharedClique& child: clique->children_)
     optimizeInPlace<BAYESTREE>(child, result);
 }
 
@@ -48,7 +46,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
   result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
 
   // sum of children
-  BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
+  for(const typename BAYESTREE::sharedClique& child: clique->children_)
     result += logDeterminant<BAYESTREE>(child);
 
   return result;

gtsam/linear/GaussianFactorGraph.cpp

@@ -30,8 +30,6 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/cholesky.h>
 
-#include <boost/foreach.hpp>
-
 using namespace std;
 using namespace gtsam;
 
@@ -50,7 +48,7 @@ namespace gtsam {
   /* ************************************************************************* */
   GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
     KeySet keys;
-    BOOST_FOREACH(const sharedFactor& factor, *this)
+    for(const sharedFactor& factor: *this)
     if (factor)
       keys.insert(factor->begin(), factor->end());
     return keys;
@@ -59,7 +57,7 @@ namespace gtsam {
   /* ************************************************************************* */
   std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const {
     map<Key, size_t> spec;
-    BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) {
+    for ( const GaussianFactor::shared_ptr &gf: *this ) {
       for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) {
         map<Key,size_t>::iterator it2 = spec.find(*it);
         if ( it2 == spec.end() ) {
@@ -80,7 +78,7 @@ namespace gtsam {
   /* ************************************************************************* */
   GaussianFactorGraph GaussianFactorGraph::clone() const {
     GaussianFactorGraph result;
-    BOOST_FOREACH(const sharedFactor& f, *this) {
+    for(const sharedFactor& f: *this) {
       if (f)
         result.push_back(f->clone());
       else
@@ -92,7 +90,7 @@ namespace gtsam {
   /* ************************************************************************* */
   GaussianFactorGraph GaussianFactorGraph::negate() const {
     GaussianFactorGraph result;
-    BOOST_FOREACH(const sharedFactor& f, *this) {
+    for(const sharedFactor& f: *this) {
       if (f)
         result.push_back(f->negate());
       else
@@ -106,7 +104,7 @@ namespace gtsam {
     // First find dimensions of each variable
     typedef std::map<Key, size_t> KeySizeMap;
     KeySizeMap dims;
-    BOOST_FOREACH(const sharedFactor& factor, *this) {
+    for(const sharedFactor& factor: *this) {
       if (!static_cast<bool>(factor)) continue;
 
       for (GaussianFactor::const_iterator key = factor->begin();
@@ -118,7 +116,7 @@ namespace gtsam {
     // Compute first scalar column of each variable
     size_t currentColIndex = 0;
     KeySizeMap columnIndices = dims;
-    BOOST_FOREACH(const KeySizeMap::value_type& col, dims) {
+    for(const KeySizeMap::value_type& col: dims) {
       columnIndices[col.first] = currentColIndex;
       currentColIndex += dims[col.first];
     }
@@ -127,7 +125,7 @@ namespace gtsam {
     typedef boost::tuple<size_t, size_t, double> triplet;
     vector<triplet> entries;
     size_t row = 0;
-    BOOST_FOREACH(const sharedFactor& factor, *this) {
+    for(const sharedFactor& factor: *this) {
       if (!static_cast<bool>(factor)) continue;
 
       // Convert to JacobianFactor if necessary
@@ -229,7 +227,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues GaussianFactorGraph::hessianDiagonal() const {
     VectorValues d;
-    BOOST_FOREACH(const sharedFactor& factor, *this) {
+    for(const sharedFactor& factor: *this) {
       if(factor){
         VectorValues di = factor->hessianDiagonal();
         d.addInPlace_(di);
@@ -241,7 +239,7 @@ namespace gtsam {
   /* ************************************************************************* */
   map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal() const {
     map<Key,Matrix> blocks;
-    BOOST_FOREACH(const sharedFactor& factor, *this) {
+    for(const sharedFactor& factor: *this) {
       if (!factor) continue;
       map<Key,Matrix> BD = factor->hessianBlockDiagonal();
       map<Key,Matrix>::const_iterator it = BD.begin();
@@ -279,7 +277,7 @@ namespace gtsam {
   VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const
   {
     VectorValues g = VectorValues::Zero(x0);
-    BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
+    for(const sharedFactor& Ai_G: *this) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       Vector e = Ai->error_vector(x0);
       Ai->transposeMultiplyAdd(1.0, e, g);
@@ -291,7 +289,7 @@ namespace gtsam {
   VectorValues GaussianFactorGraph::gradientAtZero() const {
     // Zero-out the gradient
     VectorValues g;
-    BOOST_FOREACH(const sharedFactor& factor, *this) {
+    for(const sharedFactor& factor: *this) {
       if (!factor) continue;
       VectorValues gi = factor->gradientAtZero();
       g.addInPlace_(gi);
@@ -331,7 +329,7 @@ namespace gtsam {
   /* ************************************************************************* */
   Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
     Errors e;
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) {
+    for(const GaussianFactor::shared_ptr& Ai_G: *this) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       e.push_back((*Ai) * x);
     }
@@ -341,7 +339,7 @@ namespace gtsam {
   /* ************************************************************************* */
   void GaussianFactorGraph::multiplyHessianAdd(double alpha,
       const VectorValues& x, VectorValues& y) const {
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
+    for(const GaussianFactor::shared_ptr& f: *this)
      f->multiplyHessianAdd(alpha, x, y);
   }
 
@@ -353,7 +351,7 @@ namespace gtsam {
   /* ************************************************************************* */
   void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const {
     Errors::iterator ei = e;
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) {
+    for(const GaussianFactor::shared_ptr& Ai_G: *this) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       *ei = (*Ai)*x;
       ei++;
@@ -363,7 +361,7 @@ namespace gtsam {
   /* ************************************************************************* */
   bool hasConstraints(const GaussianFactorGraph& factors) {
     typedef JacobianFactor J;
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
+    for(const GaussianFactor::shared_ptr& factor: factors) {
       J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
       if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
         return true;
@@ -378,7 +376,7 @@ namespace gtsam {
                                                  VectorValues& x) const {
     // For each factor add the gradient contribution
     Errors::const_iterator ei = e.begin();
-    BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
+    for(const sharedFactor& Ai_G: *this) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       Ai->transposeMultiplyAdd(alpha, *(ei++), x);
     }
@@ -387,7 +385,7 @@ namespace gtsam {
   ///* ************************************************************************* */
   //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
   //  Key i = 0 ;
-  //  BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+  //  for(const GaussianFactor::shared_ptr& Ai_G: fg) {
   //    JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
   //    r[i] = Ai->getb();
   //    i++;
@@ -401,7 +399,7 @@ namespace gtsam {
   //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
   //  r.setZero();
   //  Key i = 0;
-  //  BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+  //  for(const GaussianFactor::shared_ptr& Ai_G: fg) {
   //    JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
   //    Vector &y = r[i];
   //    for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
@@ -416,7 +414,7 @@ namespace gtsam {
   {
     VectorValues x;
     Errors::const_iterator ei = e.begin();
-    BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
+    for(const sharedFactor& Ai_G: *this) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
         // Create the value as a zero vector if it does not exist.
@@ -434,7 +432,7 @@ namespace gtsam {
   Errors GaussianFactorGraph::gaussianErrors(const VectorValues& x) const
   {
     Errors e;
-    BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
+    for(const sharedFactor& Ai_G: *this) {
       JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       e.push_back(Ai->error_vector(x));
     }

gtsam/linear/GaussianFactorGraph.h

@@ -144,7 +144,7 @@ namespace gtsam {
     /** unnormalized error */
     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/linear/HessianFactor.cpp

@@ -29,7 +29,6 @@
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -241,7 +240,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
   keys_.resize(n);
   FastVector<DenseIndex> dims(n + 1);
   DenseIndex slot = 0;
-  BOOST_FOREACH(const SlotEntry& slotentry, *scatter) {
+  for(const SlotEntry& slotentry: *scatter) {
     keys_[slot] = slotentry.key;
     dims[slot] = slotentry.dimension;
     ++slot;
@@ -253,7 +252,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
 
   // Form A' * A
   gttic(update);
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
+  for(const GaussianFactor::shared_ptr& factor: factors)
     if (factor)
       factor->updateHessian(keys_, &info_);
   gttoc(update);

gtsam/linear/IterativeSolver.cpp

@@ -20,7 +20,6 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/algorithm/string.hpp>
-#include <boost/foreach.hpp>
 #include <iostream>
 
 using namespace std;
@@ -127,7 +126,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
 /****************************************************************************/
 vector<size_t> KeyInfo::colSpec() const {
   std::vector<size_t> result(size(), 0);
-  BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
+  for ( const KeyInfo::value_type &item: *this ) {
     result[item.second.index()] = item.second.dim();
   }
   return result;
@@ -136,7 +135,7 @@ vector<size_t> KeyInfo::colSpec() const {
 /****************************************************************************/
 VectorValues KeyInfo::x0() const {
   VectorValues result;
-  BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
+  for ( const KeyInfo::value_type &item: *this ) {
     result.insert(item.first, Vector::Zero(item.second.dim()));
   }
   return result;

gtsam/linear/JacobianFactor.cpp

@@ -32,7 +32,6 @@
 #include <gtsam/base/cholesky.h>
 
 #include <boost/assign/list_of.hpp>
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/array.hpp>
@@ -185,12 +184,12 @@ boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
           "Unable to determine dimensionality for all variables");
   }
 
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
+  for(const JacobianFactor::shared_ptr& factor: factors) {
     m += factor->rows();
   }
 
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
-  BOOST_FOREACH(DenseIndex d, varDims) {
+  for(DenseIndex d: varDims) {
     assert(d != numeric_limits<DenseIndex>::max());
   }
 #endif
@@ -204,7 +203,7 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
   gttic(Convert_to_Jacobians);
   FastVector<JacobianFactor::shared_ptr> jacobians;
   jacobians.reserve(factors.size());
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
+  for(const GaussianFactor::shared_ptr& factor: factors) {
     if (factor) {
       if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
           JacobianFactor>(factor))
@@ -262,7 +261,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
           "The ordering provided to the JacobianFactor combine constructor\n"
               "contained extra variables that did not appear in the factors to combine.");
     // Add the remaining slots
-    BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) {
+    for(VariableSlots::const_iterator item: unorderedSlots) {
       orderedSlots.push_back(item);
     }
   } else {
@@ -292,7 +291,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
   // Loop over slots in combined factor and copy blocks from source factors
   gttic(copy_blocks);
   size_t combinedSlot = 0;
-  BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) {
+  for(VariableSlots::const_iterator varslot: orderedSlots) {
     JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot));
     // Loop over source jacobians
     DenseIndex nextRow = 0;

gtsam/linear/NoiseModel.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/random/linear_congruential.hpp>
@@ -211,7 +210,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
 }
 
 void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
-  BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
+  for(Matrix& Aj: A) { WhitenInPlace(Aj); }
   whitenInPlace(b);
 }
 
@@ -542,7 +541,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
   size_t i = 0;  // start with first row
   bool mixed = false;
   Ab.setZero();  // make sure we don't look below
-  BOOST_FOREACH (const Triple& t, Rd) {
+  for (const Triple& t: Rd) {
     const size_t& j = t.get<0>();
     const Matrix& rd = t.get<1>();
     precisions(i) = t.get<2>();
@@ -647,14 +646,14 @@ void Base::reweight(Vector& error) const {
 void Base::reweight(vector<Matrix> &A, Vector &error) const {
   if ( reweight_ == Block ) {
     const double w = sqrtWeight(error.norm());
-    BOOST_FOREACH(Matrix& Aj, A) {
+    for(Matrix& Aj: A) {
       Aj *= w;
     }
     error *= w;
   }
   else {
     const Vector W = sqrtWeight(error);
-    BOOST_FOREACH(Matrix& Aj, A) {
+    for(Matrix& Aj: A) {
       vector_scale_inplace(W,Aj);
     }
     error = W.cwiseProduct(error);

gtsam/linear/PCGSolver.cpp

@@ -22,7 +22,6 @@
 #include <gtsam/linear/Preconditioner.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/foreach.hpp>
 #include <boost/algorithm/string.hpp>
 
 #include <algorithm>
@@ -150,7 +149,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
 /**********************************************************************************/
 VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
   VectorValues result;
-  BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
+  for ( const KeyInfo::value_type &item: keyInfo ) {
     result.insert(item.first,
         v.segment(item.second.colstart(), item.second.dim()));
   }

gtsam/linear/Preconditioner.cpp

@@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build(
 
   /* getting the block diagonals over the factors */
   std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
-  BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
+  for ( const Matrix hessian: hessianMap | boost::adaptors::map_values)
     blocks.push_back(hessian);
 
   /* if necessary, allocating the memory for cacheing the factorization results */

gtsam/linear/RegularHessianFactor.h

@@ -20,7 +20,6 @@
 
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/RegularJacobianFactor.h>
-#include <boost/foreach.hpp>
 #include <vector>
 
 namespace gtsam {
@@ -114,7 +113,7 @@ public:
       double* yvalues) const {
     // Create a vector of temporary y_ values, corresponding to rows i
     y_.resize(size());
-    BOOST_FOREACH(VectorD & yi, y_)
+    for(VectorD & yi: y_)
       yi.setZero();
 
     // Accessing the VectorValues one by one is expensive
@@ -147,7 +146,7 @@ public:
 
     // Create a vector of temporary y_ values, corresponding to rows i
     y_.resize(size());
-    BOOST_FOREACH(VectorD & yi, y_)
+    for(VectorD & yi: y_)
       yi.setZero();
 
     // Accessing the VectorValues one by one is expensive

gtsam/linear/RegularJacobianFactor.h

@@ -20,7 +20,6 @@
 
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 

gtsam/linear/Scatter.cpp

@@ -20,7 +20,6 @@
 #include <gtsam/linear/Scatter.h>
 #include <gtsam/inference/Ordering.h>
 
-#include <boost/foreach.hpp>
 #include <algorithm>
 
 using namespace std;
@@ -41,13 +40,13 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
 
   // If we have an ordering, pre-fill the ordered variables first
   if (ordering) {
-    BOOST_FOREACH (Key key, *ordering) {
+    for (Key key: *ordering) {
       push_back(SlotEntry(key, 0));
     }
   }
 
   // Now, find dimensions of variables and/or extend
-  BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) {
+  for (const GaussianFactor::shared_ptr& factor: gfg) {
     if (!factor) continue;
 
     // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers

gtsam/linear/SubgraphPreconditioner.cpp

@@ -32,7 +32,7 @@
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/vector.hpp>
-#include <boost/foreach.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 #include <boost/shared_ptr.hpp>
 
 #include <algorithm>
@@ -59,7 +59,7 @@ namespace gtsam {
 /* ************************************************************************* */
 static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
   GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
-  BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
+  for(const GaussianFactor::shared_ptr &gf: gfg) {
     JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
     if( !jf ) {
       jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
@@ -122,7 +122,7 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
 
     /* sampling and cache results */
     vector<size_t> samples = iidSampler(localWeights, n-count);
-    BOOST_FOREACH ( const size_t &id, samples ) {
+    for ( const size_t &id: samples ) {
       if ( touched[id] == false ) {
         touched[id] = true ;
         result.push_back(id);
@@ -136,7 +136,7 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
 /****************************************************************************/
 Subgraph::Subgraph(const std::vector<size_t> &indices) {
   edges_.reserve(indices.size());
-  BOOST_FOREACH ( const size_t &idx, indices ) {
+  for ( const size_t &idx: indices ) {
     edges_.push_back(SubgraphEdge(idx, 1.0));
   }
 }
@@ -144,7 +144,7 @@ Subgraph::Subgraph(const std::vector<size_t> &indices) {
 /****************************************************************************/
 std::vector<size_t> Subgraph::edgeIndices() const {
   std::vector<size_t> eid; eid.reserve(size());
-  BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) {
+  for ( const SubgraphEdge &edge: edges_ ) {
     eid.push_back(edge.index_);
   }
   return eid;
@@ -180,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
 /****************************************************************************/
 std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
   os << "Subgraph" << endl;
-  BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) {
+  for ( const SubgraphEdge &e: subgraph.edges() ) {
     os << e << ", " ;
   }
   return os;
@@ -286,7 +286,7 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
 std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
   std::vector<size_t> result ;
   size_t idx = 0;
-  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+  for ( const GaussianFactor::shared_ptr &gf: gfg ) {
     if ( gf->size() == 1 ) {
       result.push_back(idx);
     }
@@ -299,7 +299,7 @@ std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const
 std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
   std::vector<size_t> result ;
   size_t idx = 0;
-  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+  for ( const GaussianFactor::shared_ptr &gf: gfg ) {
     if ( gf->size() == 2 ) {
       const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
       if ( (k1-k0) == 1 || (k0-k1) == 1 )
@@ -332,9 +332,9 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
   /* traversal */
   while ( !q.empty() ) {
     const size_t head = q.front(); q.pop();
-    BOOST_FOREACH ( const size_t id, variableIndex[head] ) {
+    for ( const size_t id: variableIndex[head] ) {
       const GaussianFactor &gf = *gfg[id];
-      BOOST_FOREACH ( const size_t key, gf.keys() ) {
+      for ( const size_t key: gf.keys() ) {
         if ( flags.count(key) == 0 ) {
           q.push(key);
           flags.insert(key);
@@ -360,7 +360,7 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
   DSFVector D(n) ;
 
   size_t count = 0 ; double sum = 0.0 ;
-  BOOST_FOREACH (const size_t id, idx) {
+  for (const size_t id: idx) {
     const GaussianFactor &gf = *gfg[id];
     if ( gf.keys().size() != 2 ) continue;
     const size_t u = ordering.find(gf.keys()[0])->second,
@@ -399,7 +399,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
   }
 
   /* down weight the tree edges to zero */
-  BOOST_FOREACH ( const size_t id, tree ) {
+  for ( const size_t id: tree ) {
     w[id] = 0.0;
   }
 
@@ -419,7 +419,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
   const size_t m = gfg.size() ;
   Weights weight; weight.reserve(m);
 
-  BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) {
+  for(const GaussianFactor::shared_ptr &gf: gfg ) {
     switch ( parameters_.skeletonWeight_ ) {
     case SubgraphBuilderParameters::EQUAL:
       weight.push_back(1.0);
@@ -569,7 +569,7 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
   std::copy(y.data(), y.data() + y.rows(), x.data());
 
   /* in place back substitute */
-  BOOST_REVERSE_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
+  for (auto cg: boost::adaptors::reverse(*Rc1_)) {
     /* collect a subvector of x that consists of the parents of cg (S) */
     const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
     const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
@@ -589,7 +589,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
   std::copy(y.data(), y.data() + y.rows(), x.data());
 
   /* in place back substitute */
-  BOOST_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
+  for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
     const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
 //    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
     const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
@@ -634,7 +634,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
 
   /* figure out dimension by traversing the keys */
   size_t d = 0;
-  BOOST_FOREACH ( const Key &key, keys ) {
+  for ( const Key &key: keys ) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
     cache.push_back(make_pair(entry.colstart(), entry.dim()));
     d += entry.dim();
@@ -643,7 +643,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
   /* use the cache to fill the result */
   Vector result = Vector::Zero(d, 1);
   size_t idx = 0;
-  BOOST_FOREACH ( const Cache::value_type &p, cache ) {
+  for ( const Cache::value_type &p: cache ) {
     result.segment(idx, p.second) = src.segment(p.first, p.second) ;
     idx += p.second;
   }
@@ -655,7 +655,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
 void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
   /* use the cache  */
   size_t idx = 0;
-  BOOST_FOREACH ( const Key &key, keys ) {
+  for ( const Key &key: keys ) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
     dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
     idx += entry.dim();
@@ -668,7 +668,7 @@ buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, co
 
   GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
   result->reserve(subgraph.size());
-  BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) {
+  for ( const SubgraphEdge &e: subgraph ) {
     const size_t idx = e.index();
     if ( clone ) result->push_back(gfg[idx]->clone());
     else result->push_back(gfg[idx]);

gtsam/linear/SubgraphSolver.cpp

@@ -131,7 +131,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
   GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
 
   size_t t = 0;
-  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
+  for ( const GaussianFactor::shared_ptr &gf: jfg ) {
 
     if (gf->keys().size() > 2) {
       throw runtime_error(

gtsam/linear/VectorValues.cpp

@@ -18,7 +18,6 @@
 
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/foreach.hpp>
 #include <boost/bind.hpp>
 #include <boost/range/combine.hpp>
 #include <boost/range/numeric.hpp>
@@ -48,7 +47,7 @@ namespace gtsam {
   VectorValues::VectorValues(const Vector& x, const Dims& dims) {
     typedef pair<Key, size_t> Pair;
     size_t j = 0;
-    BOOST_FOREACH(const Pair& v, dims) {
+    for(const Pair& v: dims) {
       Key key;
       size_t n;
       boost::tie(key, n) = v;
@@ -61,7 +60,7 @@ namespace gtsam {
   VectorValues VectorValues::Zero(const VectorValues& other)
   {
     VectorValues result;
-    BOOST_FOREACH(const KeyValuePair& v, other)
+    for(const KeyValuePair& v: other)
       result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size())));
     return result;
   }
@@ -82,7 +81,7 @@ namespace gtsam {
   void VectorValues::update(const VectorValues& values)
   {
     iterator hint = begin();
-    BOOST_FOREACH(const KeyValuePair& key_value, values)
+    for(const KeyValuePair& key_value: values)
     {
       // Use this trick to find the value using a hint, since we are inserting from another sorted map
       size_t oldSize = values_.size();
@@ -108,14 +107,14 @@ namespace gtsam {
   /* ************************************************************************* */
   void VectorValues::setZero()
   {
-    BOOST_FOREACH(Vector& v, values_ | map_values)
+    for(Vector& v: values_ | map_values)
       v.setZero();
   }
 
   /* ************************************************************************* */
   void VectorValues::print(const string& str, const KeyFormatter& formatter) const {
     cout << str << ": " << size() << " elements\n";
-    BOOST_FOREACH(const value_type& key_value, *this)
+    for(const value_type& key_value: *this)
       cout << "  " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n";
     cout.flush();
   }
@@ -125,7 +124,7 @@ namespace gtsam {
     if(this->size() != x.size())
       return false;
     typedef boost::tuple<value_type, value_type> ValuePair;
-    BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) {
+    for(const ValuePair& values: boost::combine(*this, x)) {
       if(values.get<0>().first != values.get<1>().first ||
         !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
         return false;
@@ -138,13 +137,13 @@ namespace gtsam {
   {
     // Count dimensions
     DenseIndex totalDim = 0;
-    BOOST_FOREACH(const Vector& v, *this | map_values)
+    for(const Vector& v: *this | map_values)
       totalDim += v.size();
 
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
-    BOOST_FOREACH(const Vector& v, *this | map_values) {
+    for(const Vector& v: *this | map_values) {
       result.segment(pos, v.size()) = v;
       pos += v.size();
     }
@@ -166,7 +165,7 @@ namespace gtsam {
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
-    BOOST_FOREACH(const Vector *v, items) {
+    for(const Vector *v: items) {
       result.segment(pos, v->size()) = *v;
       pos += v->size();
     }
@@ -179,11 +178,11 @@ namespace gtsam {
   {
     // Count dimensions
     DenseIndex totalDim = 0;
-    BOOST_FOREACH(size_t dim, keys | map_values)
+    for(size_t dim: keys | map_values)
       totalDim += dim;
     Vector result(totalDim);
     size_t j = 0;
-    BOOST_FOREACH(const Dims::value_type& it, keys) {
+    for(const Dims::value_type& it: keys) {
       result.segment(j,it.second) = at(it.first);
       j += it.second;
     }
@@ -221,7 +220,7 @@ namespace gtsam {
     double result = 0.0;
     typedef boost::tuple<value_type, value_type> ValuePair;
     using boost::adaptors::map_values;
-    BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) {
+    for(const ValuePair& values: boost::combine(*this, v)) {
       assert_throw(values.get<0>().first == values.get<1>().first,
         invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
       assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
@@ -240,7 +239,7 @@ namespace gtsam {
   double VectorValues::squaredNorm() const {
     double sumSquares = 0.0;
     using boost::adaptors::map_values;
-    BOOST_FOREACH(const Vector& v, *this | map_values)
+    for(const Vector& v: *this | map_values)
       sumSquares += v.squaredNorm();
     return sumSquares;
   }
@@ -329,7 +328,7 @@ namespace gtsam {
   VectorValues operator*(const double a, const VectorValues &v)
   {
     VectorValues result;
-    BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v)
+    for(const VectorValues::KeyValuePair& key_v: v)
       result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second));
     return result;
   }
@@ -343,7 +342,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues& VectorValues::operator*=(double alpha)
   {
-    BOOST_FOREACH(Vector& v, *this | map_values)
+    for(Vector& v: *this | map_values)
       v *= alpha;
     return *this;
   }

gtsam/linear/linearAlgorithms-inst.h

@@ -55,7 +55,7 @@ namespace gtsam
           OptimizeData myData;
           myData.parentData = parentData;
           // Take any ancestor results we'll need
-          BOOST_FOREACH(Key parent, clique->conditional_->parents())
+          for(Key parent: clique->conditional_->parents())
             myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent)));
           // Solve and store in our results
           //collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/));
@@ -68,7 +68,7 @@ namespace gtsam
               DenseIndex dim = 0;
               FastVector<VectorValues::const_iterator> parentPointers;
               parentPointers.reserve(clique->conditional()->nrParents());
-              BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
+              for(Key parent: clique->conditional()->parents()) {
                 parentPointers.push_back(myData.cliqueResults.at(parent));
                 dim += parentPointers.back()->second.size();
               }
@@ -76,7 +76,7 @@ namespace gtsam
               // Fill parent vector
               xS.resize(dim);
               DenseIndex vectorPos = 0;
-              BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) {
+              for(const VectorValues::const_iterator& parentPointer: parentPointers) {
                 const Vector& parentVector = parentPointer->second;
                 xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
                 vectorPos += parentVector.size();
@@ -108,7 +108,7 @@ namespace gtsam
       //  OptimizeData myData;
       //  myData.parentData = parentData;
       //  // Take any ancestor results we'll need
-      //  BOOST_FOREACH(Key parent, clique->conditional_->parents())
+      //  for(Key parent: clique->conditional_->parents())
       //    myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
       //  // Solve and store in our results
       //  myData.results.insert(clique->conditional()->solve(myData.ancestorResults));

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -30,7 +30,6 @@ using namespace boost::assign;
 #include <sstream>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/tuple/tuple.hpp>
-#include <boost/foreach.hpp>
 
 using namespace std;
 using namespace gtsam;

gtsam/linear/tests/testNoiseModel.cpp

@@ -22,7 +22,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/vector.hpp>
 
 #include <iostream>
@@ -58,32 +57,32 @@ TEST(NoiseModel, constructors)
   m.push_back(Isotropic::Precision(3, prc,false));
 
   // test kSigmas
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(kSigmas,mi->sigmas()));
 
   // test whiten
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(whitened,mi->whiten(unwhitened)));
 
   // test unwhiten
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
 
   // test Mahalanobis distance
   double distance = 5*5+10*10+15*15;
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
 
   // test R matrix
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(R,mi->R()));
 
   // test covariance
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(kCovariance,mi->covariance()));
 
   // test covariance
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(kCovariance.inverse(),mi->information()));
 
   // test Whiten operator
@@ -92,7 +91,7 @@ TEST(NoiseModel, constructors)
       0.0, 1.0, 0.0, 1.0,
       1.0, 0.0, 0.0, 1.0).finished());
   Matrix expected = kInverseSigma * H;
-  BOOST_FOREACH(Gaussian::shared_ptr mi, m)
+  for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(expected,mi->Whiten(H)));
 
   // can only test inplace version once :-)

gtsam/nonlinear/ISAM2-impl.cpp

@@ -93,7 +93,7 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISA
                                   KeySet& fixedVariables)
 {
   variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
-  BOOST_FOREACH(Key key, unusedKeys) {
+  for(Key key: unusedKeys) {
     delta.erase(key);
     deltaNewton.erase(key);
     RgProd.erase(key);
@@ -112,7 +112,7 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
 
   if(const double* threshold = boost::get<double>(&relinearizeThreshold))
   {
-    BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) {
+    for(const VectorValues::KeyValuePair& key_delta: delta) {
       double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
       if(maxDelta >= *threshold)
         relinKeys.insert(key_delta.first);
@@ -120,7 +120,7 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
   }
   else if(const FastMap<char,Vector>* thresholds = boost::get<FastMap<char,Vector> >(&relinearizeThreshold))
   {
-    BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) {
+    for(const VectorValues::KeyValuePair& key_delta: delta) {
       const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second;
       if(threshold.rows() != key_delta.second.rows())
         throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality.");
@@ -138,7 +138,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
 {
   // Check the current clique for relinearization
   bool relinearize = false;
-  BOOST_FOREACH(Key var, *clique->conditional()) {
+  for(Key var: *clique->conditional()) {
     double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
     if(maxDelta >= threshold) {
       relinKeys.insert(var);
@@ -148,7 +148,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
 
   // If this node was relinearized, also check its children
   if(relinearize) {
-    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+    for(const ISAM2Clique::shared_ptr& child: clique->children) {
       CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child);
     }
   }
@@ -161,7 +161,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vect
 {
   // Check the current clique for relinearization
   bool relinearize = false;
-  BOOST_FOREACH(Key var, *clique->conditional()) {
+  for(Key var: *clique->conditional()) {
     // Find the threshold for this variable type
     const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
 
@@ -180,7 +180,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vect
 
   // If this node was relinearized, also check its children
   if(relinearize) {
-    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+    for(const ISAM2Clique::shared_ptr& child: clique->children) {
       CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child);
     }
   }
@@ -192,7 +192,7 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedCl
                                                         const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
 {
   KeySet relinKeys;
-  BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
+  for(const ISAM2::sharedClique& root: roots) {
     if(relinearizeThreshold.type() == typeid(double))
       CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
     else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>))
@@ -207,7 +207,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke
   static const bool debug = false;
   // does the separator contain any of the variables?
   bool found = false;
-  BOOST_FOREACH(Key key, clique->conditional()->parents()) {
+  for(Key key: clique->conditional()->parents()) {
     if (markedMask.exists(key)) {
       found = true;
       break;
@@ -219,7 +219,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke
     if(debug) clique->print("Key(s) marked in clique ");
     if(debug) cout << "so marking key " << clique->conditional()->front() << endl;
   }
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+  for(const ISAM2Clique::shared_ptr& child: clique->children) {
     FindAll(child, keys, markedMask);
   }
 }
@@ -267,7 +267,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
   result.update(clique->conditional()->solve(result));
 
   // starting from the root, call optimize on each conditional
-  BOOST_FOREACH(const boost::shared_ptr<ISAM2Clique>& child, clique->children)
+  for(const boost::shared_ptr<ISAM2Clique>& child: clique->children)
     optimizeInPlace(child, result);
 }
 }
@@ -280,14 +280,14 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
 
   if (wildfireThreshold <= 0.0) {
     // Threshold is zero or less, so do a full recalculation
-    BOOST_FOREACH(const ISAM2::sharedClique& root, roots)
+    for(const ISAM2::sharedClique& root: roots)
       internal::optimizeInPlace(root, delta);
     lastBacksubVariableCount = delta.size();
 
   } else {
     // Optimize with wildfire
     lastBacksubVariableCount = 0;
-    BOOST_FOREACH(const ISAM2::sharedClique& root, roots)
+    for(const ISAM2::sharedClique& root: roots)
       lastBacksubVariableCount += optimizeWildfireNonRecursive(
       root, wildfireThreshold, replacedKeys, delta); // modifies delta
 
@@ -309,7 +309,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& re
   // update deltas and recurse to children, but if not, we do not need to
   // recurse further because of the running separator property.
   bool anyReplaced = false;
-  BOOST_FOREACH(Key j, *clique->conditional()) {
+  for(Key j: *clique->conditional()) {
     if(replacedKeys.exists(j)) {
       anyReplaced = true;
       break;
@@ -327,7 +327,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& re
 
     // Write into RgProd vector
     DenseIndex vectorPosition = 0;
-    BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+    for(Key frontal: clique->conditional()->frontals()) {
       Vector& RgProdValue = RgProd[frontal];
       RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
       vectorPosition += RgProdValue.size();
@@ -339,7 +339,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& re
     varsUpdated += clique->conditional()->nrFrontals();
 
     // Recurse to children
-    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+    for(const ISAM2Clique::shared_ptr& child: clique->children) {
       updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); }
   }
 }
@@ -351,7 +351,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replac
 
   // Update variables
   size_t varsUpdated = 0;
-  BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
+  for(const ISAM2::sharedClique& root: roots) {
     internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated);
   }
 

gtsam/nonlinear/ISAM2-inl.h

@@ -45,7 +45,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
   // Are any clique variables part of the tree that has been redone?
   bool cliqueReplaced = replaced.exists((*clique)->frontals().front());
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
-  BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+  for(Key frontal: clique->conditional()->frontals()) {
     assert(cliqueReplaced == replaced.exists(frontal));
   }
 #endif
@@ -53,7 +53,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
   // If not redone, then has one of the separator variables changed significantly?
   bool recalculate = cliqueReplaced;
   if(!recalculate) {
-    BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
+    for(Key parent: clique->conditional()->parents()) {
       if(changed.exists(parent)) {
         recalculate = true;
         break;
@@ -94,7 +94,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
     // If the values were above the threshold or this clique was replaced
     if(valuesChanged) {
       // Set changed flag for each frontal variable and leave the new values
-      BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+      for(Key frontal: clique->conditional()->frontals()) {
         changed.insert(frontal);
       }
     } else {
@@ -105,7 +105,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
     }
 
     // Recurse to children
-    BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) {
+    for(const typename CLIQUE::shared_ptr& child: clique->children) {
       optimizeWildfire(child, threshold, changed, replaced, delta, count);
     }
   }
@@ -122,7 +122,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
   // Are any clique variables part of the tree that has been redone?
   bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front());
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
-  BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+  for(Key frontal: clique->conditional()->frontals()) {
     assert(cliqueReplaced == replaced.exists(frontal));
   }
 #endif
@@ -130,7 +130,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
   // If not redone, then has one of the separator variables changed significantly?
   bool recalculate = cliqueReplaced;
   if(!recalculate) {
-    BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
+    for(Key parent: clique->conditional()->parents()) {
       if(changed.exists(parent)) {
         recalculate = true;
         break;
@@ -156,9 +156,9 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
       boost::shared_ptr<CLIQUE> parent = clique->parent_.lock();
       if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty()))
       {
-        BOOST_FOREACH(Key key, clique->conditional()->frontals())
+        for(Key key: clique->conditional()->frontals())
           clique->solnPointers_.insert(std::make_pair(key, delta.find(key)));
-        BOOST_FOREACH(Key key, clique->conditional()->parents())
+        for(Key key: clique->conditional()->parents())
           clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key)));
       }
 
@@ -174,7 +174,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
           DenseIndex dim = 0;
           FastVector<VectorValues::const_iterator> parentPointers;
           parentPointers.reserve(clique->conditional()->nrParents());
-          BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
+          for(Key parent: clique->conditional()->parents()) {
             parentPointers.push_back(clique->solnPointers_.at(parent));
             dim += parentPointers.back()->second.size();
           }
@@ -182,7 +182,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
           // Fill parent vector
           xS.resize(dim);
           DenseIndex vectorPos = 0;
-          BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) {
+          for(const VectorValues::const_iterator& parentPointer: parentPointers) {
             const Vector& parentVector = parentPointer->second;
             xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
             vectorPos += parentVector.size();
@@ -227,7 +227,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
     // If the values were above the threshold or this clique was replaced
     if(valuesChanged) {
       // Set changed flag for each frontal variable and leave the new values
-      BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+      for(Key frontal: clique->conditional()->frontals()) {
         changed.insert(frontal);
       }
     } else {
@@ -270,7 +270,7 @@ size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, doubl
       travStack.pop();
       bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count);
       if (recalculate) {
-        BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children) {
+        for(const typename CLIQUE::shared_ptr& child: currentNode->children) {
           travStack.push(child);
         }
       }
@@ -287,7 +287,7 @@ void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
   int dimSep = (int)clique->conditional()->get_S().cols();
   result += ((dimR+1)*dimR)/2 + dimSep*dimR;
   // traverse the children
-  BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) {
+  for(const typename CLIQUE::shared_ptr& child: clique->children) {
     nnz_internal(child, result);
   }
 }

gtsam/nonlinear/ISAM2.cpp

@@ -15,7 +15,6 @@
  * @author  Michael Kaess, Richard Roberts
  */
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 #include <boost/range/adaptors.hpp>
@@ -174,17 +173,17 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
 KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
   static const bool debug = false;
   if(debug) cout << "Getting affected factors for ";
-  if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } }
+  if(debug) { for(const Key key: keys) { cout << key << " "; } }
   if(debug) cout << endl;
 
   NonlinearFactorGraph allAffected;
   KeySet indices;
-  BOOST_FOREACH(const Key key, keys) {
+  for(const Key key: keys) {
     const VariableIndex::Factors& factors(variableIndex_[key]);
     indices.insert(factors.begin(), factors.end());
   }
   if(debug) cout << "Affected factors are: ";
-  if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } }
+  if(debug) { for(const size_t index: indices) { cout << index << " "; } }
   if(debug) cout << endl;
   return indices;
 }
@@ -210,10 +209,10 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
 
   gttic(check_candidates_and_linearize);
   GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>();
-  BOOST_FOREACH(Key idx, candidates) {
+  for(Key idx: candidates) {
     bool inside = true;
     bool useCachedLinear = params_.cacheLinearizedFactors;
-    BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
+    for(Key key: nonlinearFactors_[idx]->keys()) {
       if(affectedKeysSet.find(key) == affectedKeysSet.end()) {
         inside = false;
         break;
@@ -251,7 +250,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
 GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
   GaussianFactorGraph cachedBoundary;
 
-  BOOST_FOREACH(sharedClique orphan, orphans) {
+  for(sharedClique orphan: orphans) {
     // retrieve the cached factor and add to boundary
     cachedBoundary.push_back(orphan->cachedFactor());
   }
@@ -292,10 +291,10 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
 
   if(debug) {
     cout << "markedKeys: ";
-    BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; }
+    for(const Key key: markedKeys) { cout << key << " "; }
     cout << endl;
     cout << "observedKeys: ";
-    BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; }
+    for(const Key key: observedKeys) { cout << key << " "; }
     cout << endl;
   }
 
@@ -322,7 +321,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
   // ordering provides all keys in conditionals, there cannot be others because path to root included
   gttic(affectedKeys);
   FastList<Key> affectedKeys;
-  BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet)
+  for(const ConditionalType::shared_ptr& conditional: affectedBayesNet)
     affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
   gttoc(affectedKeys);
 
@@ -349,7 +348,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
       {
         // Only if some variables are unconstrained
         FastMap<Key, int> constraintGroups;
-        BOOST_FOREACH(Key var, observedKeys)
+        for(Key var: observedKeys)
           constraintGroups[var] = 1;
         order = Ordering::ColamdConstrained(variableIndex_, constraintGroups);
       }
@@ -386,7 +385,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
 
     // Reeliminated keys for detailed results
     if(params_.enableDetailedResults) {
-      BOOST_FOREACH(Key key, theta_.keys()) {
+      for(Key key: theta_.keys()) {
         result.detail->variableStatus[key].isReeliminated = true;
       }
     }
@@ -406,11 +405,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
     if(debug) factors.print("Relinearized factors: ");
     gttoc(relinearizeAffected);
 
-    if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Key key, affectedKeys) { cout << key << " "; } cout << endl; }
+    if(debug) { cout << "Affected keys: "; for(const Key key: affectedKeys) { cout << key << " "; } cout << endl; }
 
     // Reeliminated keys for detailed results
     if(params_.enableDetailedResults) {
-      BOOST_FOREACH(Key key, affectedAndNewKeys) {
+      for(Key key: affectedAndNewKeys) {
         result.detail->variableStatus[key].isReeliminated = true;
       }
     }
@@ -437,7 +436,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
 
     gttic(orphans);
     // Add the orphaned subtrees
-    BOOST_FOREACH(const sharedClique& orphan, orphans)
+    for(const sharedClique& orphan: orphans)
       factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
     gttoc(orphans);
 
@@ -465,7 +464,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
     } else {
       constraintGroups = FastMap<Key,int>();
       const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0;
-      BOOST_FOREACH (Key var, observedKeys)
+      for (Key var: observedKeys)
         constraintGroups.insert(make_pair(var, group));
     }
 
@@ -500,8 +499,8 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
 
   // Root clique variables for detailed results
   if(params_.enableDetailedResults) {
-    BOOST_FOREACH(const sharedNode& root, this->roots())
+    for(const sharedNode& root: this->roots())
-      BOOST_FOREACH(Key var, *root->conditional())
+      for(Key var: *root->conditional())
         result.detail->variableStatus[var].inRootClique = true;
   }
 
@@ -554,7 +553,7 @@ ISAM2Result ISAM2::update(
 
   // Remove the removed factors
   NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size());
-  BOOST_FOREACH(size_t index, removeFactorIndices) {
+  for(size_t index: removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if(params_.cacheLinearizedFactors)
@@ -571,7 +570,7 @@ ISAM2Result ISAM2::update(
     // Get keys from removed factors and new factors, and compute unused keys,
     // i.e., keys that are empty now and do not appear in the new factors.
     KeySet removedAndEmpty;
-    BOOST_FOREACH(Key key, removeFactors.keys()) {
+    for(Key key: removeFactors.keys()) {
       if(variableIndex_[key].empty())
         removedAndEmpty.insert(removedAndEmpty.end(), key);
     }
@@ -580,7 +579,7 @@ ISAM2Result ISAM2::update(
       newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
 
     // Get indices for unused keys
-    BOOST_FOREACH(Key key, unusedKeys) {
+    for(Key key: unusedKeys) {
       unusedIndices.insert(unusedIndices.end(), key);
     }
   }
@@ -591,7 +590,7 @@ ISAM2Result ISAM2::update(
   Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_);
   // New keys for detailed results
   if(params_.enableDetailedResults) {
-    BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
+    for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
   gttoc(add_new_variables);
 
   gttic(evaluate_error_before);
@@ -609,14 +608,14 @@ ISAM2Result ISAM2::update(
   }
   // Also mark any provided extra re-eliminate keys
   if(extraReelimKeys) {
-    BOOST_FOREACH(Key key, *extraReelimKeys) {
+    for(Key key: *extraReelimKeys) {
       markedKeys.insert(key);
     }
   }
 
   // Observed keys for detailed results
   if(params_.enableDetailedResults) {
-    BOOST_FOREACH(Key key, markedKeys) {
+    for(Key key: markedKeys) {
       result.detail->variableStatus[key].isObserved = true;
     }
   }
@@ -624,7 +623,7 @@ ISAM2Result ISAM2::update(
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Key value) instead of the iterator constructor.
   FastVector<Key> observedKeys;  observedKeys.reserve(markedKeys.size());
-  BOOST_FOREACH(Key index, markedKeys) {
+  for(Key index: markedKeys) {
     if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused
       observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them
   }
@@ -642,24 +641,24 @@ ISAM2Result ISAM2::update(
     if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging
 
     // Remove from relinKeys any keys whose linearization points are fixed
-    BOOST_FOREACH(Key key, fixedVariables_) {
+    for(Key key: fixedVariables_) {
       relinKeys.erase(key);
     }
     if(noRelinKeys) {
-      BOOST_FOREACH(Key key, *noRelinKeys) {
+      for(Key key: *noRelinKeys) {
         relinKeys.erase(key);
       }
     }
 
     // Above relin threshold keys for detailed results
     if(params_.enableDetailedResults) {
-      BOOST_FOREACH(Key key, relinKeys) {
+      for(Key key: relinKeys) {
         result.detail->variableStatus[key].isAboveRelinThreshold = true;
         result.detail->variableStatus[key].isRelinearized = true; } }
 
     // Add the variables being relinearized to the marked keys
     KeySet markedRelinMask;
-    BOOST_FOREACH(const Key key, relinKeys)
+    for(const Key key: relinKeys)
       markedRelinMask.insert(key);
     markedKeys.insert(relinKeys.begin(), relinKeys.end());
     gttoc(gather_relinearize_keys);
@@ -667,16 +666,16 @@ ISAM2Result ISAM2::update(
     gttic(fluid_find_all);
     // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
     if (!relinKeys.empty()) {
-      BOOST_FOREACH(const sharedClique& root, roots_)
+      for(const sharedClique& root: roots_)
         // add other cliques that have the marked ones in the separator
         Impl::FindAll(root, markedKeys, markedRelinMask);
 
       // Relin involved keys for detailed results
       if(params_.enableDetailedResults) {
         KeySet involvedRelinKeys;
-        BOOST_FOREACH(const sharedClique& root, roots_)
+        for(const sharedClique& root: roots_)
           Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
-        BOOST_FOREACH(Key key, involvedRelinKeys) {
+        for(Key key: involvedRelinKeys) {
           if(!result.detail->variableStatus[key].isAboveRelinThreshold) {
             result.detail->variableStatus[key].isRelinearizeInvolved = true;
             result.detail->variableStatus[key].isRelinearized = true; } }
@@ -771,7 +770,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
   KeySet leafKeysRemoved;
 
   // Remove each variable and its subtrees
-  BOOST_FOREACH(Key j, leafKeys) {
+  for(Key j: leafKeys) {
     if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree
 
       // Traverse up the tree to find the root of the marginalized subtree
@@ -789,7 +788,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
 
       // See if we should remove the whole clique
       bool marginalizeEntireClique = true;
-      BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+      for(Key frontal: clique->conditional()->frontals()) {
         if(!leafKeys.exists(frontal)) {
           marginalizeEntireClique = false;
           break; } }
@@ -806,10 +805,10 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
         // Now remove this clique and its subtree - all of its marginal
         // information has been stored in marginalFactors.
         const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques
-        BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
+        for(const sharedClique& removedClique: removedCliques) {
           marginalFactors.erase(removedClique->conditional()->front());
           leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals());
-          BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals())
+          for(Key frontal: removedClique->conditional()->frontals())
           {
             // Add to factors to remove
             const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
@@ -832,9 +831,9 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
         GaussianFactorGraph graph;
         KeySet factorsInSubtreeRoot;
         Cliques subtreesToRemove;
-        BOOST_FOREACH(const sharedClique& child, clique->children) {
+        for(const sharedClique& child: clique->children) {
           // Remove subtree if child depends on any marginalized keys
-          BOOST_FOREACH(Key parent, child->conditional()->parents()) {
+          for(Key parent: child->conditional()->parents()) {
             if(leafKeys.exists(parent)) {
               subtreesToRemove.push_back(child);
               graph.push_back(child->cachedFactor()); // Add child marginal
@@ -843,13 +842,13 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
           }
         }
         Cliques childrenRemoved;
-        BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) {
+        for(const sharedClique& childToRemove: subtreesToRemove) {
           const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques
           childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end());
-          BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
+          for(const sharedClique& removedClique: removedCliques) {
             marginalFactors.erase(removedClique->conditional()->front());
             leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals());
-            BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals())
+            for(Key frontal: removedClique->conditional()->frontals())
             {
               // Add to factors to remove
               const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
@@ -867,16 +866,16 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
         // but do not involve frontal variables of any of its children.
         // TODO: reuse cached linear factors
         KeySet factorsFromMarginalizedInClique_step1;
-        BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
+        for(Key frontal: clique->conditional()->frontals()) {
           if(leafKeys.exists(frontal))
             factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
         // Remove any factors in subtrees that we're removing at this step
-        BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) {
+        for(const sharedClique& removedChild: childrenRemoved) {
-          BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) {
+          for(Key indexInClique: removedChild->conditional()->frontals()) {
-            BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) {
+            for(Key factorInvolving: variableIndex_[indexInClique]) {
               factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } }
         // Create factor graph from factor indices
-        BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) {
+        for(size_t i: factorsFromMarginalizedInClique_step1) {
           graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
 
         // Reeliminate the linear graph to get the marginal and discard the conditional
@@ -908,7 +907,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
         clique->conditional()->nrFrontals() -= nToRemove;
 
         // Add to factors to remove factors involved in frontals of current clique
-        BOOST_FOREACH(Key frontal, cliqueFrontalsToEliminate)
+        for(Key frontal: cliqueFrontalsToEliminate)
         {
           const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
           factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end());
@@ -925,8 +924,8 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
   // Gather factors to add - the new marginal factors
   GaussianFactorGraph factorsToAdd;
   typedef pair<Key, vector<GaussianFactor::shared_ptr> > Key_Factors;
-  BOOST_FOREACH(const Key_Factors& key_factors, marginalFactors) {
+  for(const Key_Factors& key_factors: marginalFactors) {
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, key_factors.second) {
+    for(const GaussianFactor::shared_ptr& factor: key_factors.second) {
       if(factor) {
         factorsToAdd.push_back(factor);
         if(marginalFactorsIndices)
@@ -935,7 +934,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
           factor));
         if(params_.cacheLinearizedFactors)
           linearFactors_.push_back(factor);
-        BOOST_FOREACH(Key factorKey, *factor) {
+        for(Key factorKey: *factor) {
           fixedVariables_.insert(factorKey); }
       }
     }
@@ -944,7 +943,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
 
   // Remove the factors to remove that have been summarized in the newly-added marginal factors
   NonlinearFactorGraph removedFactors;
-  BOOST_FOREACH(size_t i, factorIndicesToRemove) {
+  for(size_t i: factorIndicesToRemove) {
     removedFactors.push_back(nonlinearFactors_[i]);
     nonlinearFactors_.remove(i);
     if(params_.cacheLinearizedFactors)
@@ -1065,7 +1064,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
 
   // Recursively add contributions from children
   typedef boost::shared_ptr<ISAM2Clique> sharedClique;
-  BOOST_FOREACH(const sharedClique& child, root->children) {
+  for(const sharedClique& child: root->children) {
     gradientAtZeroTreeAdder(child, g);
   }
 }
@@ -1077,7 +1076,7 @@ VectorValues ISAM2::gradientAtZero() const
   VectorValues g;
 
   // Sum up contributions for each clique
-  BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots())
+  for(const ISAM2::sharedClique& root: this->roots())
   gradientAtZeroTreeAdder(root, g);
 
   return g;

gtsam/nonlinear/ISAM2.h

@@ -200,7 +200,7 @@ struct GTSAM_EXPORT ISAM2Params {
     else
     {
       std::cout << "relinearizeThreshold:              " << "{mapped}" << "\n";
-      BOOST_FOREACH(const ISAM2ThresholdMapValue& value, boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
+      for(const ISAM2ThresholdMapValue& value: boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
         std::cout << "                                   '" << value.first << "' -> [" << value.second.transpose() << " ]\n";
       }
     }

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -157,7 +157,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
   // Only retrieve diagonal vector when reuse_diagonal = false
   if (params_.diagonalDamping && state_.reuseDiagonal == false) {
     state_.hessianDiagonal = linear.hessianDiagonal();
-    BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) {
+    for(Vector& v: state_.hessianDiagonal | map_values) {
       for (int aa = 0; aa < v.size(); aa++) {
         v(aa) = std::min(std::max(v(aa), params_.minDiagonal),
             params_.maxDiagonal);
@@ -172,7 +172,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
   GaussianFactorGraph &damped = (*dampedPtr);
   damped.reserve(damped.size() + state_.values.size());
   if (params_.diagonalDamping) {
-    BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
+    for(const VectorValues::KeyValuePair& key_vector: state_.hessianDiagonal) {
       // Fill in the diagonal of A with diag(hessian)
       try {
         Matrix A = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(
@@ -192,7 +192,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
 
     // initialize noise model cache to a reasonable default size
     NoiseCacheVector noises(6);
-    BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
+    for(const Values::KeyValuePair& key_value: state_.values) {
       size_t dim = key_value.value.dim();
 
       if (dim > noises.size())

gtsam/nonlinear/LinearContainerFactor.cpp

@@ -11,7 +11,6 @@
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -19,7 +18,7 @@ namespace gtsam {
 void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) {
   if (!linearizationPoint.empty()) {
     linearizationPoint_ = Values();
-    BOOST_FOREACH(const gtsam::Key& key, this->keys()) {
+    for(const gtsam::Key& key: this->keys()) {
       linearizationPoint_->insert(key, linearizationPoint.at(key));
     }
   } else {
@@ -82,7 +81,7 @@ double LinearContainerFactor::error(const Values& c) const {
 
   // Extract subset of values for comparison
   Values csub;
-  BOOST_FOREACH(const gtsam::Key& key, keys())
+  for(const gtsam::Key& key: keys())
     csub.insert(key, c.at(key));
 
   // create dummy ordering for evaluation
@@ -111,7 +110,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
 
   // Extract subset of values
   Values subsetC;
-  BOOST_FOREACH(const gtsam::Key& key, this->keys())
+  for(const gtsam::Key& key: this->keys())
     subsetC.insert(key, c.at(key));
 
   // Determine delta between linearization points using new ordering
@@ -170,7 +169,7 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
     const GaussianFactorGraph& linear_graph, const Values& linearizationPoint)
 {
   NonlinearFactorGraph result;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph)
+  for(const GaussianFactor::shared_ptr& f: linear_graph)
     if (f)
       result.push_back(NonlinearFactorGraph::sharedFactor(
           new LinearContainerFactor(f, linearizationPoint)));

gtsam/nonlinear/Marginals.cpp

@@ -127,7 +127,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
     // Get dimensions from factor graph
     std::vector<size_t> dims;
     dims.reserve(variablesSorted.size());
-    BOOST_FOREACH(Key key, variablesSorted) {
+    for(Key key: variablesSorted) {
       dims.push_back(values_.at(key).dim());
     }
 
@@ -144,7 +144,7 @@ VectorValues Marginals::optimize() const {
 void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
   cout << s << "Joint marginal on keys ";
   bool first = true;
-  BOOST_FOREACH(Key key, keys_) {
+  for(Key key: keys_) {
     if(!first)
       cout << ", ";
     else

gtsam/nonlinear/NonlinearFactor.cpp

@@ -26,7 +26,7 @@ namespace gtsam {
 void NonlinearFactor::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
   std::cout << s << "  keys = { ";
-  BOOST_FOREACH(Key key, keys()) {
+  for(Key key: keys()) {
     std::cout << keyFormatter(key) << " ";
   }
   std::cout << "}" << std::endl;

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -31,7 +31,6 @@
 #  include <tbb/parallel_for.h>
 #endif
 
-#include <boost/foreach.hpp>
 #include <cmath>
 #include <limits>
 
@@ -133,7 +132,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   // Find bounds
   double minX = numeric_limits<double>::infinity(), maxX = -numeric_limits<double>::infinity();
   double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity();
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     if(values.exists(key)) {
       boost::optional<Point2> xy = getXY(values.at(key), formatting);
       if(xy) {
@@ -150,7 +149,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   }
 
   // Create nodes for each variable in the graph
-  BOOST_FOREACH(Key key, keys){
+  for(Key key: keys){
     // Label the node with the label from the KeyFormatter
     stm << "  var" << key << "[label=\"" << keyFormatter(key) << "\"";
     if(values.exists(key)) {
@@ -165,7 +164,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
   if (formatting.mergeSimilarFactors) {
     // Remove duplicate factors
     std::set<vector<Key> > structure;
-    BOOST_FOREACH(const sharedFactor& factor, *this){
+    for(const sharedFactor& factor: *this){
       if(factor) {
         vector<Key> factorKeys = factor->keys();
         std::sort(factorKeys.begin(), factorKeys.end());
@@ -175,7 +174,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
     // Create factors and variable connections
     size_t i = 0;
-    BOOST_FOREACH(const vector<Key>& factorKeys, structure){
+    for(const vector<Key>& factorKeys: structure){
       // Make each factor a dot
       stm << "  factor" << i << "[label=\"\", shape=point";
       {
@@ -187,7 +186,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
       stm << "];\n";
 
       // Make factor-variable connections
-      BOOST_FOREACH(Key key, factorKeys) {
+      for(Key key: factorKeys) {
         stm << "  var" << key << "--" << "factor" << i << ";\n";
       }
 
@@ -214,7 +213,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
           // Make factor-variable connections
           if(formatting.connectKeysToFactor && factor) {
-            BOOST_FOREACH(Key key, *factor) {
+            for(Key key: *factor) {
               stm << "  var" << key << "--" << "factor" << i << ";\n";
             }
           }
@@ -224,7 +223,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
         if(factor) {
           Key k;
           bool firstTime = true;
-          BOOST_FOREACH(Key key, *this->at(i)) {
+          for(Key key: *this->at(i)) {
             if(firstTime) {
               k = key;
               firstTime = false;
@@ -246,7 +245,7 @@ double NonlinearFactorGraph::error(const Values& values) const {
   gttic(NonlinearFactorGraph_error);
   double total_error = 0.;
   // iterate over all the factors_ to accumulate the log probabilities
-  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
+  for(const sharedFactor& factor: this->factors_) {
     if(factor)
       total_error += factor->error(values);
   }
@@ -272,7 +271,7 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const
   SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared<SymbolicFactorGraph>();
   symbolic->reserve(this->size());
 
-  BOOST_FOREACH(const sharedFactor& factor, *this) {
+  for(const sharedFactor& factor: *this) {
     if(factor)
       *symbolic += SymbolicFactor(*factor);
     else
@@ -330,7 +329,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
   linearFG->reserve(this->size());
 
   // linearize all factors
-  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
+  for(const sharedFactor& factor: this->factors_) {
     if(factor) {
       (*linearFG) += factor->linearize(linearizationPoint);
     } else
@@ -345,7 +344,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
 /* ************************************************************************* */
 NonlinearFactorGraph NonlinearFactorGraph::clone() const {
   NonlinearFactorGraph result;
-  BOOST_FOREACH(const sharedFactor& f, *this) {
+  for(const sharedFactor& f: *this) {
     if (f)
       result.push_back(f->clone());
     else
@@ -357,7 +356,7 @@ NonlinearFactorGraph NonlinearFactorGraph::clone() const {
 /* ************************************************************************* */
 NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
   NonlinearFactorGraph result;
-  BOOST_FOREACH(const sharedFactor& f, *this) {
+  for(const sharedFactor& f: *this) {
     if (f)
       result.push_back(f->rekey(rekey_mapping));
     else

gtsam/nonlinear/NonlinearISAM.cpp

@@ -16,12 +16,9 @@
  */
 
 #include <gtsam/nonlinear/NonlinearISAM.h>
-
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/inference/Ordering.h>
 
-#include <boost/foreach.hpp>
-
 #include <iostream>
 
 using namespace std;

gtsam/nonlinear/Values-inl.h

@@ -26,8 +26,6 @@
 
 #include <utility>
 
-#include <boost/foreach.hpp>
-
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
 
@@ -220,7 +218,7 @@ namespace gtsam {
   /** Constructor from a Filtered view copies out all values */
   template<class ValueType>
   Values::Values(const Values::Filtered<ValueType>& view) {
-    BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) {
+    for(const typename Filtered<ValueType>::KeyValuePair& key_value: view) {
       Key key = key_value.key;
       insert(key, static_cast<const ValueType&>(key_value.value));
     }
@@ -229,7 +227,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   Values::Values(const Values::ConstFiltered<ValueType>& view) {
-    BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
+    for(const typename ConstFiltered<ValueType>::KeyValuePair& key_value: view) {
       Key key = key_value.key;
       insert(key, static_cast<const ValueType&>(key_value.value));
     }

gtsam/nonlinear/Values.cpp

@@ -25,7 +25,6 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/foreach.hpp>
 #ifdef __GNUC__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-variable"
@@ -194,7 +193,7 @@ namespace gtsam {
   /* ************************************************************************* */
   size_t Values::dim() const {
     size_t result = 0;
-    BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
+    for(const ConstKeyValuePair& key_value: *this) {
       result += key_value.value.dim();
     }
     return result;
@@ -203,7 +202,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues Values::zeroVectors() const {
     VectorValues result;
-    BOOST_FOREACH(const ConstKeyValuePair& key_value, *this)
+    for(const ConstKeyValuePair& key_value: *this)
       result.insert(key_value.key, Vector::Zero(key_value.value.dim()));
     return result;
   }

gtsam/nonlinear/factorTesting.h

@@ -22,8 +22,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/foreach.hpp>
-
 namespace gtsam {
 
 /**
@@ -49,7 +47,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
   // Loop over all variables
   const double one_over_2delta = 1.0 / (2.0 * delta);
   VectorValues dX = values.zeroVectors();
-  BOOST_FOREACH(Key key, factor) {
+  for(Key key: factor) {
     // Compute central differences using the values struct.
     const size_t cols = dX.dim(key);
     Matrix J = Matrix::Zero(rows, cols);

gtsam/nonlinear/tests/testValues.cpp

@@ -366,7 +366,7 @@ TEST(Values, filter) {
   int i = 0;
   Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
   EXPECT_LONGS_EQUAL(2, (long)filtered.size());
-  BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) {
+  for(const Values::Filtered<>::KeyValuePair& key_value: filtered) {
     if(i == 0) {
       LONGS_EQUAL(2, (long)key_value.key);
       try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
@@ -401,7 +401,7 @@ TEST(Values, filter) {
   i = 0;
   Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();
   EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size());
-  BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, pose_filtered) {
+  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: pose_filtered) {
     if(i == 0) {
       EXPECT_LONGS_EQUAL(1, (long)key_value.key);
       EXPECT(assert_equal(pose1, key_value.value));
@@ -437,7 +437,7 @@ TEST(Values, Symbol_filter) {
   values.insert(Symbol('y', 3), pose3);
 
   int i = 0;
-  BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) {
+  for(const Values::Filtered<Value>::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) {
     if(i == 0) {
       LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
       EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));

gtsam/slam/InitializePose3.cpp

@@ -43,7 +43,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
   GaussianFactorGraph linearGraph;
   noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
 
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
     Matrix3 Rij;
 
     boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
@@ -75,7 +75,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
   ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
 
   Values validRot3;
-  BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
+  for(const VectorValues::value_type& it: relaxedRot3) {
     Key key = it.first;
     if (key != keyAnchor) {
       const Vector& rotVector = it.second;
@@ -108,7 +108,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
   gttic(InitializePose3_buildPose3graph);
   NonlinearFactorGraph pose3Graph;
 
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
 
     // recast to a between on Pose3
     boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
@@ -150,7 +150,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   // this works on the inverse rotations, according to Tron&Vidal,2011
   Values inverseRot;
   inverseRot.insert(keyAnchor, Rot3());
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) {
+  for(const Values::ConstKeyValuePair& key_value: givenGuess) {
     Key key = key_value.key;
     const Pose3& pose = givenGuess.at<Pose3>(key);
     inverseRot.insert(key, pose.rotation().inverse());
@@ -165,7 +165,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   // calculate max node degree & allocate gradient
   size_t maxNodeDeg = 0;
   VectorValues grad;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
+  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
     Key key = key_value.key;
     grad.insert(key,Vector3::Zero());
     size_t currNodeDeg = (adjEdgesMap.at(key)).size();
@@ -191,12 +191,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
     //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
     //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
     maxGrad = 0;
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
+    for(const Values::ConstKeyValuePair& key_value: inverseRot) {
       Key key = key_value.key;
       //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
       Vector gradKey = Vector3::Zero();
       // collect the gradient for each edge incident on key
-      BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){
+      for(const size_t& factorId: adjEdgesMap.at(key)){
         Rot3 Rij = factorId2RotMap.at(factorId);
         Rot3 Ri = inverseRot.at<Rot3>(key);
         if( key == (pose3Graph.at(factorId))->keys()[0] ){
@@ -236,7 +236,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   // Return correct rotations
   const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
   Values estimateRot;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
+  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
     Key key = key_value.key;
     if (key != keyAnchor) {
       const Rot3& R = inverseRot.at<Rot3>(key);
@@ -252,7 +252,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
 /* ************************************************************************* */
 void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
   size_t factorId = 0;
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose3Graph) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
     boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
         boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between){
@@ -321,7 +321,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // put into Values structure
   Values initialPose;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
+  for(const Values::ConstKeyValuePair& key_value: initialRot){
     Key key = key_value.key;
     const Rot3& rot = initialRot.at<Rot3>(key);
     Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
@@ -346,7 +346,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // put into Values structure
   Values estimate;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) {
+  for(const Values::ConstKeyValuePair& key_value: GNresult) {
     Key key = key_value.key;
     if (key != keyAnchor) {
       const Pose3& pose = GNresult.at<Pose3>(key);
@@ -391,7 +391,7 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
   // Compute the full poses (1 GN iteration on full poses)
   return computePoses(pose3Graph, orientations);
 
-  //  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
+  //  for(const Values::ConstKeyValuePair& key_value: orientations) {
   //    Key key = key_value.key;
   //    if (key != keyAnchor) {
   //      const Point3& pos = givenGuess.at<Pose3>(key).translation();

gtsam/slam/JacobianFactorQ.h

@@ -44,7 +44,7 @@ public:
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
     QF.reserve(keys.size());
-    BOOST_FOREACH(const Key& key, keys)
+    for(const Key& key: keys)
       QF.push_back(KeyMatrix(key, zeroMatrix));
     JacobianFactor::fillTerms(QF, zeroVector, model);
   }

gtsam/slam/JacobianFactorSVD.h

@@ -45,7 +45,7 @@ public:
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
     QF.reserve(keys.size());
-    BOOST_FOREACH(const Key& key, keys)
+    for(const Key& key: keys)
       QF.push_back(KeyMatrix(key, zeroMatrix));
     JacobianFactor::fillTerms(QF, zeroVector, model);
   }
@@ -67,7 +67,7 @@ public:
     size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
     // PLAIN NULL SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
-    // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
+    // for(const KeyMatrixZD& it: Fblocks)
     //   QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
     // JacobianFactor factor(QF, Q * b);
     std::vector<KeyMatrix> QF;

gtsam/slam/RegularImplicitSchurFactor.h

@@ -10,7 +10,6 @@
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
-#include <boost/foreach.hpp>
 #include <iosfwd>
 
 namespace gtsam {

gtsam/slam/SmartFactorBase.h

@@ -161,7 +161,7 @@ public:
   /// Collect all cameras: important that in key order
   virtual Cameras cameras(const Values& values) const {
     Cameras cameras;
-    BOOST_FOREACH(const Key& k, this->keys_)
+    for(const Key& k: this->keys_)
       cameras.push_back(values.at<CAMERA>(k));
     return cameras;
   }