diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp
index f3f770750..9cab73822 100644
--- a/examples/Pose3SLAMExample_changeKeys.cpp
+++ b/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){
diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp
index c0399c139..5066222e5 100644
--- a/examples/Pose3SLAMExample_g2o.cpp
+++ b/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));
diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp
index 645b24cfc..e90d014c0 100644
--- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp
+++ b/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));
diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp
index 99342099a..10960cf31 100644
--- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp
+++ b/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));
diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp
index 01ce8b08b..032d61a4a 100644
--- a/examples/RangeISAMExample_plaza2.cpp
+++ b/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
diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index 2000b348b..deaf3b781 100644
--- a/examples/SolverComparer.cpp
+++ b/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)
diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp
index 602a00593..aa2195078 100644
--- a/examples/TimeTBB.cpp
+++ b/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;
diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp
index 454dea45b..bdf0543ff 100644
--- a/gtsam/base/DSFVector.cpp
+++ b/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;
 }
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index a9d521a7f..d74e1c122 100644
--- a/gtsam/base/Matrix.cpp
+++ b/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)
diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h
index f976be0e7..8792be0e6 100644
--- a/gtsam/base/TestableAssertions.h
+++ b/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;
   }
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index 40d819de9..8f17e4c6e 100644
--- a/gtsam/base/Vector.cpp
+++ b/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();
diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp
index d900e1779..cf32f1c3b 100644
--- a/gtsam/base/tests/testDSFVector.cpp
+++ b/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
diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp
index bce9a6036..f405bdaf1 100644
--- a/gtsam/base/tests/testGroup.cpp
+++ b/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 {
 
diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp
index 093f22961..88a6fa825 100644
--- a/gtsam/base/tests/testMatrix.cpp
+++ b/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;
-  BOOST_FOREACH(boost::tie(r, di, sigma), solution){
+  size_t i = 0;
+  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
diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp
index b76746ba0..ac58fcca7 100644
--- a/gtsam/base/timing.cpp
+++ b/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();
   }
 }
diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h
index 12a29e45b..0d670ba2e 100644
--- a/gtsam/base/treeTraversal-inst.h
+++ b/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;
     }
diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h
index c1df47a01..ccdd269c3 100644
--- a/gtsam/base/treeTraversal/parallelTraversalTasks.h
+++ b/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())
diff --git a/gtsam/base/treeTraversal/statistics.h b/gtsam/base/treeTraversal/statistics.h
index 805c69758..7a5c7d256 100644
--- a/gtsam/base/treeTraversal/statistics.h
+++ b/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)
             {
diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h
index 709c7350e..3665d6dfa 100644
--- a/gtsam/discrete/Assignment.h
+++ b/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);
diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index c1648888e..ecc8533c1 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/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;
     }
diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index 9ed88bc3d..f83349436 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/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);
-    BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j);
+    for(Key j: keys()) cs[j] = 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);
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index f7253ec24..54cce56be 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/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>
diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp
index 3b3939674..84a80c565 100644
--- a/gtsam/discrete/DiscreteBayesNet.cpp
+++ b/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;
   }
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index f21a705ff..4a918ef02 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/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];
   }
 }
diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp
index e9f57c30d..c101653d2 100644
--- a/gtsam/discrete/DiscreteFactor.cpp
+++ b/gtsam/discrete/DiscreteFactor.cpp
@@ -18,7 +18,6 @@
  */
 
 #include <gtsam/discrete/DiscreteFactor.h>
-#include <boost/foreach.hpp>
 
 using namespace std;
 
diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index c2128c776..af11d4b14 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/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);
 
diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp
index 828a93eec..b4fd2e5a1 100644
--- a/gtsam/discrete/DiscreteKey.cpp
+++ b/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;
   }
diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp
index 11dddee66..c4cdbe0ef 100644
--- a/gtsam/discrete/Potentials.cpp
+++ b/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];
 //    }
diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp
index 9e63b957d..0ee5c63b8 100644
--- a/gtsam/discrete/Signature.cpp
+++ b/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_)
-              BOOST_FOREACH(const double& x, row)
+      for(const Row& row: *table_)
+              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;
diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
index d6902bbef..9c3f4bd63 100644
--- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
+++ b/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;
diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index c1acaf83d..93126f642 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/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();
diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 052180f7a..0fbf44097 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/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;
 }
diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h
index 5101e9fc8..65047df41 100644
--- a/gtsam/geometry/PinholeSet.h
+++ b/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 {
 
diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp
index 7066e527b..bc6132812 100644
--- a/gtsam/geometry/Point2.cpp
+++ b/gtsam/geometry/Point2.cpp
@@ -16,7 +16,6 @@
  */
 
 #include <gtsam/geometry/Point2.h>
-#include <boost/foreach.hpp>
 #include <cmath>
 
 using namespace std;
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 8a0e8fbf5..34b146bee 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/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();
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index d170282fe..3138dfd64 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/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);
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index d1f68fe28..bccfdc1c7 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/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>
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 0e99268ee..912b26262 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/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));
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 4e8ae2f17..369c54bea 100644
--- a/gtsam/geometry/triangulation.h
+++ b/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)
diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h
index f5e69fcce..a3bd87887 100644
--- a/gtsam/inference/BayesNet-inst.h
+++ b/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;
     }
 
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 3a3e1317c..e04af1339 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/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
diff --git a/gtsam/inference/BayesTree.cpp b/gtsam/inference/BayesTree.cpp
index 6d6a4a4d3..2fb0227b6 100644
--- a/gtsam/inference/BayesTree.cpp
+++ b/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;
   }
diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h
index 256ff983d..31a8c55b6 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inst.h
+++ b/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();
       }
 
diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h
index 6b28f2dbf..bd886273a 100644
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/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);
   }
diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h
index a22f290d9..39b60079c 100644
--- a/gtsam/inference/Conditional-inst.h
+++ b/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;
   }
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index 68e623b68..90540aaa6 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/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); }
       }
     }
 
diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp
index 707193a52..49f34fb2b 100644
--- a/gtsam/inference/Factor.cpp
+++ b/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_;
   }
 
-}
\ No newline at end of file
+}
diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
index 52ba23155..0aa4af2e1 100644
--- a/gtsam/inference/FactorGraph-inst.h
+++ b/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());
diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h
index 99f668591..bf2cacc84 100644
--- a/gtsam/inference/ISAM-inst.h
+++ b/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
diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h
index 352a8dded..8fcdd1eac 100644
--- a/gtsam/inference/JunctionTree-inst.h
+++ b/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
diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp
index a2a6906d1..f25727441 100644
--- a/gtsam/inference/Key.cpp
+++ b/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;
   }
diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h
index 06e5ddeec..eb9670254 100644
--- a/gtsam/inference/MetisIndex-inl.h
+++ b/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])
-        BOOST_FOREACH(const Key& k2, *factors[i])
+      for(const Key& k1: *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);
diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h
index c00a3633e..352ea166f 100644
--- a/gtsam/inference/VariableIndex-inl.h
+++ b/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);
diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp
index e67c0e248..72c56be5b 100644
--- a/gtsam/inference/VariableIndex.cpp
+++ b/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";
   }
diff --git a/gtsam/inference/VariableSlots.cpp b/gtsam/inference/VariableSlots.cpp
index 783071ae2..d89ad2ced 100644
--- a/gtsam/inference/VariableSlots.cpp
+++ b/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
diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h
index fad789436..8ebd5c13c 100644
--- a/gtsam/inference/VariableSlots.h
+++ b/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
diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h
index 90c7b32c9..de614c2b4 100644
--- a/gtsam/inference/graph-inl.h
+++ b/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"));
diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp
index d40250266..41e2d8c84 100644
--- a/gtsam/linear/Errors.cpp
+++ b/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);
 }
 
diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index 948daf2ad..c9ef922be 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/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);
diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h
index 1f6c8e9f5..ab311fdf2 100644
--- a/gtsam/linear/GaussianBayesTree-inl.h
+++ b/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;
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 19d6bd607..ff67b9cb1 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/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));
     }
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index ae552adcd..d969f77ad 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/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);
       }
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 1c55d293b..2d635d3c6 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/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);
diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp
index 79ade1c8a..411feb7a9 100644
--- a/gtsam/linear/IterativeSolver.cpp
+++ b/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;
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 7fecefe3c..999d7a325 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/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;
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 5bcf3d635..02dd37ea9 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/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);
diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp
index caf7d0095..f9ef756f1 100644
--- a/gtsam/linear/PCGSolver.cpp
+++ b/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()));
   }
diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp
index 538d886b4..d83385d32 100644
--- a/gtsam/linear/Preconditioner.cpp
+++ b/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 */
diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h
index e5e545c36..cf714e71d 100644
--- a/gtsam/linear/RegularHessianFactor.h
+++ b/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
diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h
index bbc34d482..2b5cf85ff 100644
--- a/gtsam/linear/RegularJacobianFactor.h
+++ b/gtsam/linear/RegularJacobianFactor.h
@@ -20,7 +20,6 @@
 
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp
index ed2af529f..719b3d16c 100644
--- a/gtsam/linear/Scatter.cpp
+++ b/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
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index ebe5bcc9b..d51f365be 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/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]);
diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp
index 3c7256c29..70212b5b1 100644
--- a/gtsam/linear/SubgraphSolver.cpp
+++ b/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(
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 1439d4b61..bce339dca 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/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;
   }
diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h
index 4722a9b6d..0583d1803 100644
--- a/gtsam/linear/linearAlgorithms-inst.h
+++ b/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));
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 26f975296..2a1dc088f 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/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;
diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 43ce271f3..d4403d3e3 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/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 :-)
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 0211df735..a0fc117d9 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/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);
   }
 
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 538c66068..ccaf898d3 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/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);
   }
 }
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index d6f1a636a..a0244ce14 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/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())
-      BOOST_FOREACH(Key var, *root->conditional())
+    for(const sharedNode& root: this->roots())
+      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) {
-          BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) {
-            BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) {
+        for(const sharedClique& removedChild: childrenRemoved) {
+          for(Key indexInClique: removedChild->conditional()->frontals()) {
+            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) {
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, key_factors.second) {
+  for(const Key_Factors& key_factors: marginalFactors) {
+    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;
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 27f21be12..114c04018 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/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";
       }
     }
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 9e42afa33..fdd86d36b 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/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())
diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp
index f26001b16..0b5622f28 100644
--- a/gtsam/nonlinear/LinearContainerFactor.cpp
+++ b/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)));
diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp
index 40d943656..ac8fa4e89 100644
--- a/gtsam/nonlinear/Marginals.cpp
+++ b/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
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 7c8337fc8..5ff022023 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/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;
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 77809b51e..ae95fa72b 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/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
diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp
index 2405ca222..512069a8a 100644
--- a/gtsam/nonlinear/NonlinearISAM.cpp
+++ b/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;
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 4f333f05d..40979b86c 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/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));
     }
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index ba7a040cd..e21661fee 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/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;
   }
diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h
index a256f8fe2..c0eccbd08 100644
--- a/gtsam/nonlinear/factorTesting.h
+++ b/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);
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 9911da450..b7c4efeca 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/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>()));
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 180e5cd24..f049e9d62 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/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();
diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h
index f9f258055..d09627b77 100644
--- a/gtsam/slam/JacobianFactorQ.h
+++ b/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);
   }
diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h
index dabb7600d..8afe0bcbf 100644
--- a/gtsam/slam/JacobianFactorSVD.h
+++ b/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;
diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index c713eff72..973798a64 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/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 {
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 07bd20d4d..3128390c7 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/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;
   }
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index 01f89e526..af7ca64c6 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -290,9 +290,9 @@ public:
 
     if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
       // failed: return"empty" Hessian
-      BOOST_FOREACH(Matrix& m, Gs)
+      for(Matrix& m: Gs)
         m = Matrix::Zero(Base::Dim, Base::Dim);
-      BOOST_FOREACH(Vector& v, gs)
+      for(Vector& v: gs)
         v = Vector::Zero(Base::Dim);
       return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
           Gs, gs, 0.0);
diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h
index 455e0de87..bb261a9c4 100644
--- a/gtsam/slam/SmartProjectionPoseFactor.h
+++ b/gtsam/slam/SmartProjectionPoseFactor.h
@@ -123,7 +123,7 @@ public:
    */
   typename Base::Cameras cameras(const Values& values) const {
     typename Base::Cameras cameras;
-    BOOST_FOREACH(const Key& k, this->keys_) {
+    for(const Key& k: this->keys_) {
       Pose3 pose = values.at<Pose3>(k);
       if (Base::body_P_sensor_)
         pose = pose.compose(*(Base::body_P_sensor_));
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index b2a90cb84..dc25026d2 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -37,7 +37,6 @@
 #include <boost/assign/list_inserter.hpp>
 #include <boost/filesystem/operations.hpp>
 #include <boost/filesystem/path.hpp>
-#include <boost/foreach.hpp>
 
 #include <cmath>
 #include <fstream>
@@ -68,8 +67,8 @@ string findExampleDataFile(const string& name) {
   namesToSearch.push_back(name + ".out");
 
   // Find first name that exists
-  BOOST_FOREACH(const fs::path& root, rootsToSearch) {
-    BOOST_FOREACH(const fs::path& name, namesToSearch) {
+  for(const fs::path& root: rootsToSearch) {
+    for(const fs::path& name: namesToSearch) {
       if (fs::is_regular_file(root / name))
         return (root / name).string();
     }
@@ -366,7 +365,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
 
   // save poses
 
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
+  for(const Values::ConstKeyValuePair& key_value: config) {
     const Pose2& pose = key_value.value.cast<Pose2>();
     stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
         << " " << pose.theta() << endl;
@@ -375,7 +374,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
   // save edges
   Matrix R = model->R();
   Matrix RR = trans(R) * R; //prod(trans(R),R);
-  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
+  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
     boost::shared_ptr<BetweenFactor<Pose2> > factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
     if (!factor)
@@ -413,13 +412,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
 
   // save 2D & 3D poses
   Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
-  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, viewPose2) {
+  for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
     stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
         << key_value.value.y() << " " << key_value.value.theta() << endl;
   }
 
   Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
-  BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, viewPose3) {
+  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
       Point3 p = key_value.value.translation();
       Rot3 R = key_value.value.rotation();
       stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
@@ -428,7 +427,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   }
 
   // save edges (2D or 3D)
-  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
+  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
     boost::shared_ptr<BetweenFactor<Pose2> > factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
     if (factor){
@@ -898,7 +897,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
 Values initialCamerasEstimate(const SfM_data& db) {
   Values initial;
   size_t i = 0; // NO POINTS:  j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, db.cameras)
+  for(const SfM_Camera& camera: db.cameras)
     initial.insert(i++, camera);
   return initial;
 }
@@ -906,9 +905,9 @@ Values initialCamerasEstimate(const SfM_data& db) {
 Values initialCamerasAndPointsEstimate(const SfM_data& db) {
   Values initial;
   size_t i = 0, j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, db.cameras)
+  for(const SfM_Camera& camera: db.cameras)
     initial.insert((i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, db.tracks)
+  for(const SfM_Track& track: db.tracks)
     initial.insert(P(j++), track.p);
   return initial;
 }
diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp
index fa6fce37a..e1cf9cea2 100644
--- a/gtsam/slam/lago.cpp
+++ b/gtsam/slam/lago.cpp
@@ -82,7 +82,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
   thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
 
   // for all nodes in the tree
-  BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) {
+  for(const key2doubleMap::value_type& it: deltaThetaMap) {
     // compute the orientation wrt root
     Key nodeKey = it.first;
     double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
@@ -101,7 +101,7 @@ void getSymbolicGraph(
   // Get keys for which you want the orientation
   size_t id = 0;
   // Loop over the factors
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
     if (factor->keys().size() == 2) {
       Key key1 = factor->keys()[0];
       Key key2 = factor->keys()[1];
@@ -167,14 +167,14 @@ GaussianFactorGraph buildLinearOrientationGraph(
   noiseModel::Diagonal::shared_ptr model_deltaTheta;
 
   // put original measurements in the spanning tree
-  BOOST_FOREACH(const size_t& factorId, spanningTreeIds) {
+  for(const size_t& factorId: spanningTreeIds) {
     const FastVector<Key>& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
   }
   // put regularized measurements in the chordsIds
-  BOOST_FOREACH(const size_t& factorId, chordsIds) {
+  for(const size_t& factorId: chordsIds) {
     const FastVector<Key>& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
@@ -199,7 +199,7 @@ static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
   gttic(lago_buildPose2graph);
   NonlinearFactorGraph pose2Graph;
 
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
 
     // recast to a between on Pose2
     boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
@@ -226,7 +226,7 @@ static PredecessorMap<Key> findOdometricPath(
   Key minKey = keyAnchor; // this initialization does not matter
   bool minUnassigned = true;
 
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
 
     Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
     Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
@@ -299,7 +299,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
   GaussianFactorGraph linearPose2graph;
 
   // We include the linear version of each between factor
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) {
+  for(const boost::shared_ptr<NonlinearFactor>& factor: pose2graph) {
 
     boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
@@ -346,7 +346,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
 
   // put into Values structure
   Values initialGuessLago;
-  BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
+  for(const VectorValues::value_type& it: posesLago) {
     Key key = it.first;
     if (key != keyAnchor) {
       const Vector& poseVector = it.second;
@@ -383,7 +383,7 @@ Values initialize(const NonlinearFactorGraph& graph,
   VectorValues orientations = initializeOrientations(graph);
 
   // for all nodes in the tree
-  BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
+  for(const VectorValues::value_type& it: orientations) {
     Key key = it.first;
     if (key != keyAnchor) {
       const Pose2& pose = initialGuess.at<Pose2>(key);
diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index 48e2e8b2f..b99cb5d9c 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -465,7 +465,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) {
   }
 
   // Now loop over all these noise models
-  BOOST_FOREACH(SharedNoiseModel model, models) {
+  for(SharedNoiseModel model: models) {
     Projection factor(measurement, model, X(1), L(1));
 
     // Test linearize
diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp
index 7db71d8ce..f8157c116 100644
--- a/gtsam/slam/tests/testLago.cpp
+++ b/gtsam/slam/tests/testLago.cpp
@@ -285,7 +285,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
   Values::shared_ptr expected;
   boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
+  for(const Values::KeyValuePair& key_val: *expected){
     Key k = key_val.key;
     EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
   }
@@ -311,7 +311,7 @@ TEST( Lago, largeGraphNoisy ) {
   Values::shared_ptr expected;
   boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
+  for(const Values::KeyValuePair& key_val: *expected){
     Key k = key_val.key;
     EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
   }
diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
index f1e830d03..ea42a1ecd 100644
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@@ -24,7 +24,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
-#include <boost/foreach.hpp>
 #include <boost/assign/std/vector.hpp>
 
 using namespace boost::assign;
diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp
index c5a04eb0d..038ccecbf 100644
--- a/gtsam/symbolic/SymbolicBayesNet.cpp
+++ b/gtsam/symbolic/SymbolicBayesNet.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/symbolic/SymbolicConditional.h>
 #include <gtsam/inference/FactorGraph-inst.h>
 
+#include <boost/range/adaptor/reversed.hpp>
 #include <fstream>
 
 namespace gtsam {
@@ -39,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)) {
       SymbolicConditional::Frontals frontals = conditional->frontals();
       Key me = frontals.front();
       SymbolicConditional::Parents parents = conditional->parents();
-      BOOST_FOREACH(Key p, parents)
+      for(Key p: parents)
         of << p << "->" << me << std::endl;
     }
 
diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp
index 8797ba363..aab626837 100644
--- a/gtsam/symbolic/SymbolicBayesTree.cpp
+++ b/gtsam/symbolic/SymbolicBayesTree.cpp
@@ -16,8 +16,6 @@
  * @author Richard Roberts
  */
 
-#include <boost/foreach.hpp>
-
 #include <gtsam/symbolic/SymbolicBayesTree.h>
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/symbolic/SymbolicBayesNet.h>
diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h
index f1e2b48c2..47bb06515 100644
--- a/gtsam/symbolic/SymbolicFactor-inst.h
+++ b/gtsam/symbolic/SymbolicFactor-inst.h
@@ -24,7 +24,6 @@
 #include <gtsam/base/timing.h>
 
 #include <boost/shared_ptr.hpp>
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 
 #include <utility>
@@ -43,12 +42,12 @@ namespace gtsam
 
       // Gather all keys
       KeySet allKeys;
-      BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) {
+      for(const boost::shared_ptr<FACTOR>& factor: factors) {
         allKeys.insert(factor->begin(), factor->end());
       }
 
       // Check keys
-      BOOST_FOREACH(Key key, keys) {
+      for(Key key: keys) {
         if(allKeys.find(key) == allKeys.end())
           throw std::runtime_error("Requested to eliminate a key that is not in the factors");
       }
diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp
index 13849895d..1b84e291f 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp
@@ -213,7 +213,7 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayes
   if (subtree) {
     cliques.push_back(subtree);
     // Recursive call over all child cliques
-    BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children) {
+    for(SymbolicBayesTree::sharedClique& childClique: subtree->children) {
       getAllCliques(childClique,cliques);
     }
   }
@@ -241,7 +241,7 @@ TEST( BayesTree, shortcutCheck )
   SymbolicBayesTree::Cliques allCliques;
   getAllCliques(rootClique,allCliques);
 
-  BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
+  for(SymbolicBayesTree::sharedClique& clique: allCliques) {
     //clique->print("Clique#");
     SymbolicBayesNet bn = clique->shortcut(rootClique);
     //bn.print("Shortcut:\n");
@@ -250,13 +250,13 @@ TEST( BayesTree, shortcutCheck )
 
   // Check if all the cached shortcuts are cleared
   rootClique->deleteCachedShortcuts();
-  BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
+  for(SymbolicBayesTree::sharedClique& clique: allCliques) {
     bool notCleared = clique->cachedSeparatorMarginal().is_initialized();
     CHECK( notCleared == false);
   }
   EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());
 
-//  BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
+//  for(SymbolicBayesTree::sharedClique& clique: allCliques) {
 //    clique->print("Clique#");
 //    if(clique->cachedShortcut()){
 //      bn = clique->cachedShortcut().get();
diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp
index d192a6064..994e9b107 100644
--- a/gtsam/symbolic/tests/testSymbolicISAM.cpp
+++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp
@@ -15,14 +15,13 @@
  * @author  Michael Kaess
  */
 
-#include <boost/foreach.hpp>
-#include <boost/assign/std/list.hpp> // for operator +=
-using namespace boost::assign;
+#include <gtsam/symbolic/SymbolicISAM.h>
+#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/symbolic/SymbolicISAM.h>
-#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h
index baa6540e7..f544366b2 100644
--- a/gtsam_unstable/base/BTree.h
+++ b/gtsam_unstable/base/BTree.h
@@ -395,7 +395,7 @@ namespace gtsam {
 
     }; // const_iterator
 
-    // to make BTree work with BOOST_FOREACH
+    // to make BTree work with range-based for
     // We do *not* want a non-const iterator
     typedef const_iterator iterator;
 
diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h
index c51d6003c..ba545de35 100644
--- a/gtsam_unstable/base/DSF.h
+++ b/gtsam_unstable/base/DSF.h
@@ -19,7 +19,6 @@
 #pragma once
 
 #include <gtsam_unstable/base/BTree.h>
-#include <boost/foreach.hpp>
 #include <iostream>
 #include <list>
 #include <set>
@@ -58,14 +57,14 @@ public:
   // constructor with a list of unconnected keys
   DSF(const std::list<KEY>& keys) :
       Tree() {
-    BOOST_FOREACH(const KEY& key, keys)
+    for(const KEY& key: keys)
       *this = this->add(key, key);
   }
 
   // constructor with a set of unconnected keys
   DSF(const std::set<KEY>& keys) :
       Tree() {
-    BOOST_FOREACH(const KEY& key, keys)
+    for(const KEY& key: keys)
       *this = this->add(key, key);
   }
 
@@ -109,7 +108,7 @@ public:
   // create a new singleton with a list of fully connected keys
   Self makeList(const std::list<KEY>& keys) const {
     Self t = *this;
-    BOOST_FOREACH(const KEY& key, keys)
+    for(const KEY& key: keys)
       t = t.makePair(key, keys.front());
     return t;
   }
@@ -117,7 +116,7 @@ public:
   // return a dsf in which all find_set operations will be O(1) due to path compression.
   DSF flatten() const {
     DSF t = *this;
-    BOOST_FOREACH(const KeyLabel& pair, (Tree)t)
+    for(const KeyLabel& pair: (Tree)t)
       t.findSet_(pair.first);
     return t;
   }
@@ -125,7 +124,7 @@ public:
   // maps f over all keys, must be invertible
   DSF map(boost::function<KEY(const KEY&)> func) const {
     DSF t;
-    BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
+    for(const KeyLabel& pair: (Tree)*this)
       t = t.add(func(pair.first), func(pair.second));
     return t;
   }
@@ -133,7 +132,7 @@ public:
   // return the number of sets
   size_t numSets() const {
     size_t num = 0;
-    BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
+    for(const KeyLabel& pair: (Tree)*this)
       if (pair.first == pair.second)
         num++;
     return num;
@@ -147,7 +146,7 @@ public:
   // return all sets, i.e. a partition of all elements
   std::map<KEY, Set> sets() const {
     std::map<KEY, Set> sets;
-    BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
+    for(const KeyLabel& pair: (Tree)*this)
       sets[findSet(pair.second)].insert(pair.first);
     return sets;
   }
@@ -155,7 +154,7 @@ public:
   // return a partition of the given elements {keys}
   std::map<KEY, Set> partition(const std::list<KEY>& keys) const {
     std::map<KEY, Set> partitions;
-    BOOST_FOREACH(const KEY& key, keys)
+    for(const KEY& key: keys)
       partitions[findSet(key)].insert(key);
     return partitions;
   }
@@ -163,7 +162,7 @@ public:
   // get the nodes in the tree with the given label
   Set set(const KEY& label) const {
     Set set;
-    BOOST_FOREACH(const KeyLabel& pair, (Tree)*this) {
+    for(const KeyLabel& pair: (Tree)*this) {
       if (pair.second == label || findSet(pair.second) == label)
         set.insert(pair.first);
     }
@@ -183,7 +182,7 @@ public:
   // print the object
   void print(const std::string& name = "DSF") const {
     std::cout << name << std::endl;
-    BOOST_FOREACH(const KeyLabel& pair, (Tree)*this)
+    for(const KeyLabel& pair: (Tree)*this)
       std::cout << (std::string) pair.first << " " << (std::string) pair.second
           << std::endl;
   }
diff --git a/gtsam_unstable/base/tests/testBTree.cpp b/gtsam_unstable/base/tests/testBTree.cpp
index 4fb35dca8..bbc22c8e5 100644
--- a/gtsam_unstable/base/tests/testBTree.cpp
+++ b/gtsam_unstable/base/tests/testBTree.cpp
@@ -17,7 +17,6 @@
  */
 
 #include <boost/shared_ptr.hpp>
-#include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for +=
 using namespace boost::assign;
 
@@ -146,9 +145,9 @@ TEST( BTree, iterating )
   CHECK(*(++it) == p5)
   CHECK((++it)==tree.end())
 
-  // acid iterator test: BOOST_FOREACH
+  // acid iterator test
   int sum = 0;
-  BOOST_FOREACH(const KeyInt& p, tree)
+  for(const KeyInt& p: tree)
 sum  += p.second;
   LONGS_EQUAL(15,sum)
 
diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp
index c8828746b..298d439bc 100644
--- a/gtsam_unstable/base/tests/testDSF.cpp
+++ b/gtsam_unstable/base/tests/testDSF.cpp
@@ -295,7 +295,7 @@ TEST(DSF, mergePairwiseMatches) {
 
   // Merge matches
   DSF<Measurement> dsf(measurements);
-  BOOST_FOREACH(const Match& m, matches)
+  for(const Match& m: matches)
     dsf.makeUnionInPlace(m.first,m.second);
 
   // Check that sets are merged correctly
diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam_unstable/base/tests/testDSFMap.cpp
index 69723d99b..9c9143a15 100644
--- a/gtsam_unstable/base/tests/testDSFMap.cpp
+++ b/gtsam_unstable/base/tests/testDSFMap.cpp
@@ -18,7 +18,6 @@
 
 #include <gtsam_unstable/base/DSFMap.h>
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/std/set.hpp>
 using namespace boost::assign;
@@ -72,7 +71,7 @@ TEST(DSFMap, mergePairwiseMatches) {
 
   // Merge matches
   DSFMap<size_t> dsf;
-  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
@@ -102,7 +101,7 @@ TEST(DSFMap, mergePairwiseMatches2) {
 
   // Merge matches
   DSFMap<Measurement> dsf;
-  BOOST_FOREACH(const Match& m, matches)
+  for(const Match& m: matches)
     dsf.merge(m.first,m.second);
 
   // Check that sets are merged correctly
@@ -122,7 +121,7 @@ TEST(DSFMap, sets){
 
   // Merge matches
   DSFMap<size_t> dsf;
-  BOOST_FOREACH(const Match& m, matches)
+  for(const Match& m: matches)
     dsf.merge(m.first,m.second);
 
   map<size_t, set<size_t> > sets = dsf.sets();
@@ -130,9 +129,9 @@ TEST(DSFMap, sets){
   s1 += 1,2,3;
   s2 += 4,5,6;
   
-  /*BOOST_FOREACH(key_pair st, sets){
+  /*for(key_pair st: sets){
     cout << "Set " << st.first << " :{";
-    BOOST_FOREACH(const size_t s, st.second)
+    for(const size_t s: st.second)
       cout << s << ", ";
     cout << "}" << endl;
   }*/
diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp
index b230aa570..9e124954f 100644
--- a/gtsam_unstable/discrete/AllDiff.cpp
+++ b/gtsam_unstable/discrete/AllDiff.cpp
@@ -15,7 +15,7 @@ namespace gtsam {
   /* ************************************************************************* */
   AllDiff::AllDiff(const DiscreteKeys& dkeys) :
     Constraint(dkeys.indices()) {
-    BOOST_FOREACH(const DiscreteKey& dkey, dkeys)
+    for(const DiscreteKey& dkey: dkeys)
         cardinalities_.insert(dkey);
   }
 
@@ -23,7 +23,7 @@ namespace gtsam {
   void AllDiff::print(const std::string& s,
       const KeyFormatter& formatter) const {
     std::cout << s << "AllDiff on ";
-    BOOST_FOREACH (Key dkey, keys_)
+    for (Key dkey: keys_)
       std::cout << formatter(dkey) << " ";
     std::cout << std::endl;
   }
@@ -31,7 +31,7 @@ namespace gtsam {
   /* ************************************************************************* */
   double AllDiff::operator()(const Values& values) const {
     std::set < size_t > taken; // record values taken by keys
-    BOOST_FOREACH(Key dkey, keys_) {
+    for(Key dkey: keys_) {
       size_t value = values.at(dkey); // get the value for that key
       if (taken.count(value)) return 0.0;// check if value alreday taken
       taken.insert(value);// if not, record it as taken and keep checking
@@ -70,7 +70,7 @@ namespace gtsam {
     // Check all other domains for singletons and erase corresponding values
     // This is the same as arc-consistency on the equivalent binary constraints
     bool changed = false;
-    BOOST_FOREACH(Key k, keys_)
+    for(Key k: keys_)
       if (k != j) {
         const Domain& Dk = domains[k];
         if (Dk.isSingleton()) { // check if singleton
@@ -88,7 +88,7 @@ namespace gtsam {
   Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const {
     DiscreteKeys newKeys;
     // loop over keys and add them only if they do not appear in values
-    BOOST_FOREACH(Key k, keys_)
+    for(Key k: keys_)
       if (values.find(k) == values.end()) {
         newKeys.push_back(DiscreteKey(k,cardinalities_.at(k)));
       }
@@ -99,7 +99,7 @@ namespace gtsam {
   Constraint::shared_ptr AllDiff::partiallyApply(
       const std::vector<Domain>& domains) const {
     DiscreteFactor::Values known;
-    BOOST_FOREACH(Key k, keys_) {
+    for(Key k: keys_) {
         const Domain& Dk = domains[k];
         if (Dk.isSingleton())
           known[k] = Dk.firstValue();
diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp
index ae716f246..cf5abdcb1 100644
--- a/gtsam_unstable/discrete/CSP.cpp
+++ b/gtsam_unstable/discrete/CSP.cpp
@@ -8,7 +8,6 @@
 #include <gtsam_unstable/discrete/Domain.h>
 #include <gtsam_unstable/discrete/CSP.h>
 #include <gtsam/base/Testable.h>
-#include <boost/foreach.hpp>
 
 using namespace std;
 
@@ -45,7 +44,7 @@ namespace gtsam {
         changed[v] = false;
         // loop over all factors/constraints for variable v
         const VariableIndex::Factors& factors = index[v];
-        BOOST_FOREACH(size_t f,factors) {
+        for(size_t f: factors) {
           // if not already a singleton
           if (!domains[v].isSingleton()) {
             // get the constraint and call its ensureArcConsistency method
@@ -85,7 +84,7 @@ namespace gtsam {
     // TODO: create a new ordering as we go, to ensure a connected graph
     // KeyOrdering ordering;
     // vector<Index> dkeys;
-    BOOST_FOREACH(const DiscreteFactor::shared_ptr& f, factors_) {
+    for(const DiscreteFactor::shared_ptr& f: factors_) {
       Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>(f);
       if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor");
       Constraint::shared_ptr reduced = constraint->partiallyApply(domains);
diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h
index 923365c40..93f5c5d7d 100644
--- a/gtsam_unstable/discrete/CSP.h
+++ b/gtsam_unstable/discrete/CSP.h
@@ -54,7 +54,7 @@ namespace gtsam {
 //    /** return product of all factors as a single factor */
 //    DecisionTreeFactor product() const {
 //      DecisionTreeFactor result;
-//      BOOST_FOREACH(const sharedFactor& factor, *this)
+//      for(const sharedFactor& factor: *this)
 //        if (factor) result = (*factor) * result;
 //      return result;
 //    }
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index 94c977cb0..72d424baf 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -19,9 +19,9 @@ namespace gtsam {
       const KeyFormatter& formatter) const {
 //    cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" <<
 //    formatter(keys_[0]) << ") with values";
-//    BOOST_FOREACH (size_t v,values_) cout << " " << v;
+//    for (size_t v: values_) cout << " " << v;
 //    cout << endl;
-    BOOST_FOREACH (size_t v,values_) cout << v;
+    for (size_t v: values_) cout << v;
   }
 
   /* ************************************************************************* */
@@ -50,7 +50,7 @@ namespace gtsam {
   bool Domain::ensureArcConsistency(size_t j, vector<Domain>& domains) const {
     if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain");
     Domain& D = domains[j];
-    BOOST_FOREACH(size_t value, values_)
+    for(size_t value: values_)
       if (!D.contains(value)) throw runtime_error("Unsatisfiable");
     D = *this;
     return true;
@@ -60,9 +60,9 @@ namespace gtsam {
   bool Domain::checkAllDiff(const vector<Key> keys, vector<Domain>& domains) {
     Key j = keys_[0];
     // for all values in this domain
-    BOOST_FOREACH(size_t value, values_) {
+    for(size_t value: values_) {
       // for all connected domains
-      BOOST_FOREACH(Key k, keys)
+      for(Key k: keys)
         // if any domain contains the value we cannot make this domain singleton
         if (k!=j && domains[k].contains(value))
           goto found;
diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp
index 1f22fa0e6..a81048291 100644
--- a/gtsam_unstable/discrete/Scheduler.cpp
+++ b/gtsam_unstable/discrete/Scheduler.cpp
@@ -11,7 +11,6 @@
 #include <gtsam/base/timing.h>
 
 #include <boost/tokenizer.hpp>
-#include <boost/foreach.hpp>
 
 #include <fstream>
 #include <iomanip>
@@ -163,7 +162,7 @@ namespace gtsam {
 
     if (!mutexBound) {
       DiscreteKeys dkeys;
-      BOOST_FOREACH(const Student& s, students_)
+      for(const Student& s: students_)
         dkeys.push_back(s.key_);
       addAllDiff(dkeys);
     } else {
@@ -182,30 +181,30 @@ namespace gtsam {
   void Scheduler::print(const string& s) const {
 
     cout << s << " Faculty:" << endl;
-    BOOST_FOREACH(const string& name, facultyName_)
+    for(const string& name: facultyName_)
             cout << name << '\n';
     cout << endl;
 
     cout << s << " Slots:\n";
     size_t i = 0;
-    BOOST_FOREACH(const string& name, slotName_)
+    for(const string& name: slotName_)
             cout << i++ << " " << name << endl;
     cout << endl;
 
     cout << "Availability:\n" << available_ << '\n';
 
     cout << s << " Area constraints:\n";
-    BOOST_FOREACH(const FacultyInArea::value_type& it, facultyInArea_)
+    for(const FacultyInArea::value_type& it: facultyInArea_)
           {
             cout << setw(12) << it.first << ": ";
-            BOOST_FOREACH(double v, it.second)
+            for(double v: it.second)
                     cout << v << " ";
             cout << '\n';
           }
     cout << endl;
 
     cout << s << " Students:\n";
-    BOOST_FOREACH (const Student& student, students_)
+    for (const Student& student: students_)
             student.print();
     cout << endl;
 
diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp
index 0679dcdfa..e9f63b2d8 100644
--- a/gtsam_unstable/discrete/examples/schedulingExample.cpp
+++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp
@@ -15,7 +15,6 @@
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 
 #include <algorithm>
@@ -303,7 +302,7 @@ void accomodateStudent() {
   vector<size_t> slots;
   slots += 16, 17, 11, 2, 0, 5, 9, 14;
   vector<double> slotsAvailable(scheduler.nrTimeSlots(), 1.0);
-  BOOST_FOREACH(size_t s, slots)
+  for(size_t s: slots)
   slotsAvailable[s] = 0;
   scheduler.setSlotsAvailable(slotsAvailable);
 
diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp
index 3a7ea4d2a..1fc4a1459 100644
--- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp
+++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp
@@ -15,7 +15,6 @@
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 
 #include <algorithm>
diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp
index 0b5238783..95b64f289 100644
--- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp
+++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp
@@ -15,7 +15,6 @@
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 #include <boost/format.hpp>
 
 #include <algorithm>
diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
index 37a4fe5a4..d2efc3a2d 100644
--- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
+++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
@@ -45,7 +45,7 @@ class LoopyBelief {
     void print(const std::string& s = "") const {
       cout << s << ":" << endl;
       star->print("Star graph: ");
-      BOOST_FOREACH(Key key, correctedBeliefIndices | boost::adaptors::map_keys) {
+      for(Key key: correctedBeliefIndices | boost::adaptors::map_keys) {
         cout << "Belief factor index for " << key << ": "
             << correctedBeliefIndices.at(key) << endl;
       }
@@ -70,7 +70,7 @@ public:
   /// print
   void print(const std::string& s = "") const {
     cout << s << ":" << endl;
-    BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) {
+    for(Key key: starGraphs_ | boost::adaptors::map_keys) {
       starGraphs_.at(key).print((boost::format("Node %d:") % key).str());
     }
   }
@@ -83,7 +83,7 @@ public:
     DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph());
     std::map<Key, std::map<Key, DiscreteFactor::shared_ptr> > allMessages;
     // Eliminate each star graph
-    BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) {
+    for(Key key: starGraphs_ | boost::adaptors::map_keys) {
 //      cout << "***** Node " << key << "*****" << endl;
       // initialize belief to the unary factor from the original graph
       DecisionTreeFactor::shared_ptr beliefAtKey;
@@ -92,9 +92,9 @@ public:
       std::map<Key, DiscreteFactor::shared_ptr> messages;
 
       // eliminate each neighbor in this star graph one by one
-      BOOST_FOREACH(Key neighbor, starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) {
+      for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) {
         DiscreteFactorGraph subGraph;
-        BOOST_FOREACH(size_t factor, starGraphs_.at(key).varIndex_[neighbor]) {
+        for(size_t factor: starGraphs_.at(key).varIndex_[neighbor]) {
           subGraph.push_back(starGraphs_.at(key).star->at(factor));
         }
         if (debug) subGraph.print("------- Subgraph:");
@@ -141,9 +141,9 @@ public:
 
     // Update corrected beliefs
     VariableIndex beliefFactors(*beliefs);
-    BOOST_FOREACH(Key key, starGraphs_ | boost::adaptors::map_keys) {
+    for(Key key: starGraphs_ | boost::adaptors::map_keys) {
       std::map<Key, DiscreteFactor::shared_ptr> messages = allMessages[key];
-      BOOST_FOREACH(Key neighbor, starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) {
+      for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) {
         DecisionTreeFactor correctedBelief = (*boost::dynamic_pointer_cast<
             DecisionTreeFactor>(beliefs->at(beliefFactors[key].front())))
             / (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
@@ -169,13 +169,13 @@ private:
       const std::map<Key, DiscreteKey>& allDiscreteKeys) const {
     StarGraphs starGraphs;
     VariableIndex varIndex(graph); ///< access to all factors of each node
-    BOOST_FOREACH(Key key, varIndex | boost::adaptors::map_keys) {
+    for(Key key: varIndex | boost::adaptors::map_keys) {
       // initialize to multiply with other unary factors later
       DecisionTreeFactor::shared_ptr prodOfUnaries;
 
       // collect all factors involving this key in the original graph
       DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
-      BOOST_FOREACH(size_t factorIdx, varIndex[key]) {
+      for(size_t factorIdx: varIndex[key]) {
         star->push_back(graph.at(factorIdx));
 
         // accumulate unary factors
@@ -196,7 +196,7 @@ private:
       KeySet neighbors = star->keys();
       neighbors.erase(key);
       CorrectedBeliefIndices correctedBeliefIndices;
-      BOOST_FOREACH(Key neighbor, neighbors) {
+      for(Key neighbor: neighbors) {
         // TODO: default table for keys with more than 2 values?
         string initialBelief;
         for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) {
diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp
index fa68f04ea..0be4e4b1f 100644
--- a/gtsam_unstable/discrete/tests/testScheduler.cpp
+++ b/gtsam_unstable/discrete/tests/testScheduler.cpp
@@ -14,7 +14,6 @@
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/map.hpp>
 #include <boost/optional.hpp>
-#include <boost/foreach.hpp>
 
 using namespace boost::assign;
 using namespace std;
diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
index f2fa1b31b..8df720cd1 100644
--- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
+++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
@@ -309,19 +309,19 @@ int main(int argc, char** argv) {
   // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
   cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
   cout << "  Concurrent Filter Keys: " << endl;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentFilter.getLinearizationPoint()) {
+  for(const Values::ConstKeyValuePair& key_value: concurrentFilter.getLinearizationPoint()) {
     cout << setprecision(5) << "    Key: " << key_value.key << endl;
   }
   cout << "  Concurrent Smoother Keys: " << endl;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, concurrentSmoother.getLinearizationPoint()) {
+  for(const Values::ConstKeyValuePair& key_value: concurrentSmoother.getLinearizationPoint()) {
     cout << setprecision(5) << "    Key: " << key_value.key << endl;
   }
   cout << "  Fixed-Lag Smoother Keys: " << endl;
-  BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, fixedlagSmoother.timestamps()) {
+  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: fixedlagSmoother.timestamps()) {
     cout << setprecision(5) << "    Key: " << key_timestamp.first << endl;
   }
   cout << "  Batch Smoother Keys: " << endl;
-  BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, batchSmoother.timestamps()) {
+  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: batchSmoother.timestamps()) {
     cout << setprecision(5) << "    Key: " << key_timestamp.first << endl;
   }
 
diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
index ea1381bab..931e85c1a 100644
--- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp
+++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
@@ -133,11 +133,11 @@ int main(int argc, char** argv) {
   // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
   cout << "After 3.0 seconds, " << endl;
   cout << "  Batch Smoother Keys: " << endl;
-  BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, smootherBatch.timestamps()) {
+  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherBatch.timestamps()) {
     cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
   }
   cout << "  iSAM2 Smoother Keys: " << endl;
-  BOOST_FOREACH(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp, smootherISAM2.timestamps()) {
+  for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: smootherISAM2.timestamps()) {
     cout << setprecision(5) << "    Key: " << key_timestamp.first << "  Time: " << key_timestamp.second << endl;
   }
 
diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
index 4062d0659..0ee601d26 100644
--- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
+++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
@@ -43,7 +43,6 @@
 #include <gtsam_unstable/slam/SmartRangeFactor.h>
 
 // Standard headers, added last, so we know headers above work on their own
-#include <boost/foreach.hpp>
 #include <fstream>
 #include <iostream>
 
@@ -152,7 +151,7 @@ int main(int argc, char** argv) {
   typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
   map<size_t, SmartPtr> smartFactors;
   if (smart) {
-    BOOST_FOREACH(size_t jj,ids) {
+    for(size_t jj: ids) {
       smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
       newFactors.push_back(smartFactors[jj]);
     }
@@ -166,7 +165,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;
@@ -219,25 +218,25 @@ int main(int argc, char** argv) {
       if (hasLandmarks) {
         // update landmark estimates
         landmarkEstimates = Values();
-        BOOST_FOREACH(size_t jj,ids)
+        for(size_t jj: ids)
           landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
       }
       newFactors = NonlinearFactorGraph();
       initial = Values();
       if (smart && !hasLandmarks) {
         cout << "initialize from smart landmarks" << endl;
-        BOOST_FOREACH(size_t jj,ids) {
+        for(size_t jj: ids) {
           Point2 landmark = smartFactors[jj]->triangulate(result);
           initial.insert(symbol('L', jj), landmark);
           landmarkEstimates.insert(symbol('L', jj), landmark);
         }
       }
       countK = 0;
-      BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
+      for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
         os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
             << endl;
       if (smart) {
-        BOOST_FOREACH(size_t jj,ids) {
+        for(size_t jj: ids) {
           Point2 landmark = smartFactors[jj]->triangulate(result);
           os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
               << endl;
@@ -247,7 +246,7 @@ int main(int argc, char** argv) {
     i += 1;
     if (i>end) break;
     //--------------------------------- odometry loop -----------------------------------------
-  } // BOOST_FOREACH
+  } // end for
   gttoc_(iSAM);
 
   // Print timings
@@ -257,7 +256,7 @@ int main(int argc, char** argv) {
   Values result = isam.calculateEstimate();
   ofstream os(
       "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
-  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
+  for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
     os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
         << it.value.theta() << endl;
   exit(0);
diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
index 90d92897e..5f33a215b 100644
--- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
+++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
@@ -42,7 +42,6 @@
 #include <gtsam/sam/RangeFactor.h>
 
 // Standard headers, added last, so we know headers above work on their own
-#include <boost/foreach.hpp>
 #include <fstream>
 #include <iostream>
 
@@ -143,7 +142,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;
@@ -194,7 +193,7 @@ int main(int argc, char** argv) {
     }
     i += 1;
     //--------------------------------- odometry loop -----------------------------------------
-  } // BOOST_FOREACH
+  } // end for
   gttoc_(iSAM);
 
   // Print timings
@@ -204,12 +203,12 @@ int main(int argc, char** argv) {
   Values result = isam.calculateEstimate();
   ofstream os2(
       "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
-  BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
+  for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
     os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
         << endl;
   ofstream os(
       "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
-  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
+  for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
     os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
         << it.value.theta() << endl;
   exit(0);
diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp
index 90e3eeea6..f610e4d0f 100644
--- a/gtsam_unstable/geometry/SimPolygon2D.cpp
+++ b/gtsam_unstable/geometry/SimPolygon2D.cpp
@@ -4,7 +4,6 @@
  */
 
 #include <iostream>
-#include <boost/foreach.hpp>
 #include <boost/random/linear_congruential.hpp>
 #include <boost/random/uniform_real.hpp>
 #include <boost/random/normal_distribution.hpp>
@@ -55,7 +54,7 @@ bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
 /* ************************************************************************* */
 void SimPolygon2D::print(const string& s) const {
   cout << "SimPolygon " << s << ": " << endl;
-  BOOST_FOREACH(const Point2& p, landmarks_)
+  for(const Point2& p: landmarks_)
     p.print("   ");
 }
 
@@ -73,7 +72,7 @@ bool SimPolygon2D::contains(const Point2& c) const {
   vector<SimWall2D> edges = walls();
   bool initialized = false;
   bool lastSide = false;
-  BOOST_FOREACH(const SimWall2D& ab, edges) {
+  for(const SimWall2D& ab: edges) {
     // compute cross product of ab and ac
     Point2 dab = ab.b() - ab.a();
     Point2 dac = c - ab.a();
@@ -97,10 +96,10 @@ bool SimPolygon2D::contains(const Point2& c) const {
 
 /* ************************************************************************* */
 bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
-  BOOST_FOREACH(const Point2& a, landmarks_)
+  for(const Point2& a: landmarks_)
     if (p.contains(a))
       return true;
-  BOOST_FOREACH(const Point2& a, p.landmarks_)
+  for(const Point2& a: p.landmarks_)
     if (contains(a))
       return true;
   return false;
@@ -108,7 +107,7 @@ bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
 
 /* ***************************************************************** */
 bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
-  BOOST_FOREACH(const SimPolygon2D& poly, obstacles)
+  for(const SimPolygon2D& poly: obstacles)
     if (poly.contains(p))
       return true;
   return false;
@@ -116,7 +115,7 @@ bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obst
 
 /* ************************************************************************* */
 bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
-  BOOST_FOREACH(const SimPolygon2D& poly, obstacles)
+  for(const SimPolygon2D& poly: obstacles)
     if (poly.overlaps(p))
       return true;
   return false;
@@ -134,7 +133,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
   lms.push_back(Point2(-d2,-d2));
   lms.push_back(Point2( d2,-d2));
 
-  BOOST_FOREACH(const SimPolygon2D& poly, existing_polys)
+  for(const SimPolygon2D& poly: existing_polys)
     lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
 
   for (size_t i=0; i<max_it; ++i) {
@@ -188,7 +187,7 @@ SimPolygon2D SimPolygon2D::randomRectangle(
   lms.push_back(Point2(-d2, d2));
   lms.push_back(Point2(-d2,-d2));
   lms.push_back(Point2( d2,-d2));
-  BOOST_FOREACH(const SimPolygon2D& poly, existing_polys)
+  for(const SimPolygon2D& poly: existing_polys)
     lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
 
   const Point2 lower_corner(-side_len,-side_len);
@@ -320,7 +319,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
 /* ***************************************************************** */
 bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
     const Point2& p, double threshold) {
-  BOOST_FOREACH(const Point2& Sp, S)
+  for(const Point2& Sp: S)
     if (Sp.distance(p) < threshold)
       return true;
   return false;
diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp
index 9087cac2a..e294a360d 100644
--- a/gtsam_unstable/geometry/SimWall2D.cpp
+++ b/gtsam_unstable/geometry/SimWall2D.cpp
@@ -3,7 +3,6 @@
  * @author Alex Cunningham
  */
 
-#include <boost/foreach.hpp>
 
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam_unstable/geometry/SimWall2D.h>
@@ -135,7 +134,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
   SimWall2D traj(test_pose.t(), cur_pose.t());
   bool collision = false;  Point2 intersection(1e+10, 1e+10);
   SimWall2D closest_wall;
-  BOOST_FOREACH(const SimWall2D& wall, walls) {
+  for(const SimWall2D& wall: walls) {
     Point2 cur_intersection;
     if (wall.intersects(traj,cur_intersection)) {
       collision = true;
diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp
index 0d9bbae6e..9fd58af78 100644
--- a/gtsam_unstable/linear/QPSolver.cpp
+++ b/gtsam_unstable/linear/QPSolver.cpp
@@ -30,7 +30,7 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) {
 VectorValues QPSolver::solveWithCurrentWorkingSet(
     const LinearInequalityFactorGraph& workingSet) const {
   GaussianFactorGraph workingGraph = baseGraph_;
-  BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
+  for(const LinearInequality::shared_ptr& factor: workingSet) {
     if (factor->active())
       workingGraph.push_back(factor);
   }
@@ -53,7 +53,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
   if (Aterms.size() > 0) {
     Vector b = Vector::Zero(delta.at(key).size());
     if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
-      BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
+      for(size_t factorIx: costVariableIndex_[key]) {
         GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
         b += factor->gradient(key, delta);
       }
@@ -69,7 +69,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
 GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
     const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const {
   GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
-  BOOST_FOREACH(Key key, constrainedKeys_) {
+  for(Key key: constrainedKeys_) {
     // Each constrained key becomes a factor in the dual graph
     JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta);
     if (!dualFactor->empty())
@@ -219,7 +219,7 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
     const LinearInequalityFactorGraph& inequalities,
     const VectorValues& initialValues) const {
   LinearInequalityFactorGraph workingSet;
-  BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
+  for(const LinearInequality::shared_ptr& factor: inequalities){
     LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
     double error = workingFactor->error(initialValues);
     if (fabs(error)>1e-7){
diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h
index 68713f965..7929bc1cc 100644
--- a/gtsam_unstable/linear/QPSolver.h
+++ b/gtsam_unstable/linear/QPSolver.h
@@ -68,7 +68,7 @@ public:
       const VariableIndex& variableIndex) const {
     std::vector<std::pair<Key, Matrix> > Aterms;
     if (variableIndex.find(key) != variableIndex.end()) {
-      BOOST_FOREACH(size_t factorIx, variableIndex[key]){
+      for(size_t factorIx: variableIndex[key]){
       typename FACTOR::shared_ptr factor = graph.at(factorIx);
       if (!factor->active()) continue;
       Matrix Ai = factor->getA(factor->find(key)).transpose();
diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
index 470701d97..405321ffc 100644
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
@@ -64,7 +64,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
   // Add the new variables to theta
   theta_.insert(newTheta);
   // Add new variables to the end of the ordering
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
+  for(const Values::ConstKeyValuePair& key_value: newTheta) {
     ordering_.push_back(key_value.key);
   }
   // Augment Delta
@@ -87,7 +87,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
       current_timestamp - smootherLag_);
   if (debug) {
     std::cout << "Marginalizable Keys: ";
-    BOOST_FOREACH(Key key, marginalizableKeys) {
+    for(Key key: marginalizableKeys) {
       std::cout << DefaultKeyFormatter(key) << " ";
     }
     std::cout << std::endl;
@@ -123,7 +123,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
 /* ************************************************************************* */
 void BatchFixedLagSmoother::insertFactors(
     const NonlinearFactorGraph& newFactors) {
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
+  for(const NonlinearFactor::shared_ptr& factor: newFactors) {
     Key index;
     // Insert the factor into an existing hole in the factor graph, if possible
     if (availableSlots_.size() > 0) {
@@ -135,7 +135,7 @@ void BatchFixedLagSmoother::insertFactors(
       factors_.push_back(factor);
     }
     // Update the FactorIndex
-    BOOST_FOREACH(Key key, *factor) {
+    for(Key key: *factor) {
       factorIndex_[key].insert(index);
     }
   }
@@ -144,10 +144,10 @@ void BatchFixedLagSmoother::insertFactors(
 /* ************************************************************************* */
 void BatchFixedLagSmoother::removeFactors(
     const std::set<size_t>& deleteFactors) {
-  BOOST_FOREACH(size_t slot, deleteFactors) {
+  for(size_t slot: deleteFactors) {
     if (factors_.at(slot)) {
       // Remove references to this factor from the FactorIndex
-      BOOST_FOREACH(Key key, *(factors_.at(slot))) {
+      for(Key key: *(factors_.at(slot))) {
         factorIndex_[key].erase(slot);
       }
       // Remove the factor from the factor graph
@@ -165,7 +165,7 @@ void BatchFixedLagSmoother::removeFactors(
 /* ************************************************************************* */
 void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
 
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     // Erase the key from the values
     theta_.erase(key);
 
@@ -181,7 +181,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
   eraseKeyTimestampMap(keys);
 
   // Remove marginalized keys from the ordering and delta
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key));
     delta_.erase(key);
   }
@@ -198,7 +198,7 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
 
   if (debug) {
     std::cout << "Marginalizable Keys: ";
-    BOOST_FOREACH(Key key, marginalizeKeys) {
+    for(Key key: marginalizeKeys) {
       std::cout << DefaultKeyFormatter(key) << " ";
     }
     std::cout << std::endl;
@@ -288,7 +288,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
         {
           // for each of the variables, add a prior at the current solution
           double sigma = 1.0 / std::sqrt(lambda);
-          BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) {
+          for(const VectorValues::KeyValuePair& key_value: delta_) {
             size_t dim = key_value.second.size();
             Matrix A = Matrix::Identity(dim, dim);
             Vector b = key_value.second;
@@ -332,7 +332,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
           // Put the linearization points and deltas back for specific variables
           if (enforceConsistency_ && (linearKeys_.size() > 0)) {
             theta_.update(linearKeys_);
-            BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) {
+            for(const Values::ConstKeyValuePair& key_value: linearKeys_) {
               delta_.at(key_value.key) = newDelta.at(key_value.key);
             }
           }
@@ -397,7 +397,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
 
   if (debug) {
     std::cout << "BatchFixedLagSmoother::marginalize  Marginalize Keys: ";
-    BOOST_FOREACH(Key key, marginalizeKeys) {
+    for(Key key: marginalizeKeys) {
       std::cout << DefaultKeyFormatter(key) << " ";
     }
     std::cout << std::endl;
@@ -406,14 +406,14 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
   // Identify all of the factors involving any marginalized variable. These must be removed.
   std::set<size_t> removedFactorSlots;
   VariableIndex variableIndex(factors_);
-  BOOST_FOREACH(Key key, marginalizeKeys) {
+  for(Key key: marginalizeKeys) {
     const FastVector<size_t>& slots = variableIndex[key];
     removedFactorSlots.insert(slots.begin(), slots.end());
   }
 
   if (debug) {
     std::cout << "BatchFixedLagSmoother::marginalize  Removed Factor Slots: ";
-    BOOST_FOREACH(size_t slot, removedFactorSlots) {
+    for(size_t slot: removedFactorSlots) {
       std::cout << slot << " ";
     }
     std::cout << std::endl;
@@ -421,7 +421,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
 
   // Add the removed factors to a factor graph
   NonlinearFactorGraph removedFactors;
-  BOOST_FOREACH(size_t slot, removedFactorSlots) {
+  for(size_t slot: removedFactorSlots) {
     if (factors_.at(slot)) {
       removedFactors.push_back(factors_.at(slot));
     }
@@ -456,7 +456,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
 void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
     const std::string& label) {
   std::cout << label;
-  BOOST_FOREACH(gtsam::Key key, keys) {
+  for(gtsam::Key key: keys) {
     std::cout << " " << gtsam::DefaultKeyFormatter(key);
   }
   std::cout << std::endl;
@@ -466,7 +466,7 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
 void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys,
     const std::string& label) {
   std::cout << label;
-  BOOST_FOREACH(gtsam::Key key, keys) {
+  for(gtsam::Key key: keys) {
     std::cout << " " << gtsam::DefaultKeyFormatter(key);
   }
   std::cout << std::endl;
@@ -477,7 +477,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
     const NonlinearFactor::shared_ptr& factor) {
   std::cout << "f(";
   if (factor) {
-    BOOST_FOREACH(Key key, factor->keys()) {
+    for(Key key: factor->keys()) {
       std::cout << " " << gtsam::DefaultKeyFormatter(key);
     }
   } else {
@@ -490,7 +490,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
 void BatchFixedLagSmoother::PrintSymbolicFactor(
     const GaussianFactor::shared_ptr& factor) {
   std::cout << "f(";
-  BOOST_FOREACH(Key key, factor->keys()) {
+  for(Key key: factor->keys()) {
     std::cout << " " << gtsam::DefaultKeyFormatter(key);
   }
   std::cout << " )" << std::endl;
@@ -500,7 +500,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
 void BatchFixedLagSmoother::PrintSymbolicGraph(
     const NonlinearFactorGraph& graph, const std::string& label) {
   std::cout << label << std::endl;
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) {
+  for(const NonlinearFactor::shared_ptr& factor: graph) {
     PrintSymbolicFactor(factor);
   }
 }
@@ -509,7 +509,7 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(
 void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph,
     const std::string& label) {
   std::cout << label << std::endl;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
+  for(const GaussianFactor::shared_ptr& factor: graph) {
     PrintSymbolicFactor(factor);
   }
 }
@@ -568,7 +568,7 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
     // Wrap in nonlinear container factors
     NonlinearFactorGraph marginalFactors;
     marginalFactors.reserve(marginalLinearFactors.size());
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) {
+    for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
       marginalFactors += boost::make_shared<LinearContainerFactor>(
           gaussianFactor, theta);
       if (debug) {
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index c3e307010..884bba9a3 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -33,7 +33,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
     } else {
       std::cout << "f( ";
     }
-    BOOST_FOREACH(Key key, *factor) {
+    for(Key key: *factor) {
       std::cout << keyFormatter(key) << " ";
     }
     std::cout << ")" << std::endl;
@@ -46,7 +46,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
 void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
     const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
   std::cout << indent << title << std::endl;
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
+  for(const NonlinearFactor::shared_ptr& factor: factors) {
     PrintNonlinearFactor(factor, indent + "  ", keyFormatter);
   }
 }
@@ -55,7 +55,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph
 void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
     const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
   std::cout << indent << title << std::endl;
-  BOOST_FOREACH(size_t slot, slots) {
+  for(size_t slot: slots) {
     PrintNonlinearFactor(factors.at(slot), indent + "  ", keyFormatter);
   }
 }
@@ -74,7 +74,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
     } else {
       std::cout << "g( ";
     }
-    BOOST_FOREACH(Key key, *factor) {
+    for(Key key: *factor) {
       std::cout << keyFormatter(key) << " ";
     }
     std::cout << ")" << std::endl;
@@ -87,7 +87,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
 void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors,
     const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
   std::cout << indent << title << std::endl;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
+  for(const GaussianFactor::shared_ptr& factor: factors) {
     PrintLinearFactor(factor, indent + "  ", keyFormatter);
   }
 }
@@ -139,7 +139,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto
   // Add the new variables to theta
   theta_.insert(newTheta);
   // Add new variables to the end of the ordering
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
+  for(const Values::ConstKeyValuePair& key_value: newTheta) {
     ordering_.push_back(key_value.key);
   }
   // Augment Delta
@@ -222,7 +222,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
 
   // Find the set of new separator keys
   KeySet newSeparatorKeys;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+  for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
     newSeparatorKeys.insert(key_value.key);
   }
 
@@ -236,7 +236,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
     graph.push_back(smootherShortcut_);
     Values values;
     values.insert(smootherSummarizationValues);
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+    for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
       if(!values.exists(key_value.key)) {
         values.insert(key_value.key, key_value.value);
       }
@@ -321,7 +321,7 @@ std::vector<size_t> ConcurrentBatchFilter::insertFactors(const NonlinearFactorGr
   slots.reserve(factors.size());
 
   // Insert the factor into an existing hole in the factor graph, if possible
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
+  for(const NonlinearFactor::shared_ptr& factor: factors) {
     size_t slot;
     if(availableSlots_.size() > 0) {
       slot = availableSlots_.front();
@@ -345,7 +345,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
   gttic(remove_factors);
 
   // For each factor slot to delete...
-  BOOST_FOREACH(size_t slot, slots) {
+  for(size_t slot: slots) {
 
     // Remove the factor from the graph
     factors_.remove(slot);
@@ -431,7 +431,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values
         double sigma = 1.0 / std::sqrt(lambda);
 
         // for each of the variables, add a prior at the current solution
-        BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta) {
+        for(const VectorValues::KeyValuePair& key_value: delta) {
           size_t dim = key_value.second.size();
           Matrix A = Matrix::Identity(dim,dim);
           Vector b = key_value.second;
@@ -471,7 +471,7 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values
           // Put the linearization points and deltas back for specific variables
           if(linearValues.size() > 0) {
             theta.update(linearValues);
-            BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearValues) {
+            for(const Values::ConstKeyValuePair& key_value: linearValues) {
               delta.at(key_value.key) = newDelta.at(key_value.key);
             }
           }
@@ -535,7 +535,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
   // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove)
   std::vector<size_t> removedFactorSlots;
   VariableIndex variableIndex(factors_);
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     const FastVector<size_t>& slots = variableIndex[key];
     removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
   }
@@ -544,14 +544,14 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
   std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
   removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
   // Remove any linear/marginal factor that made it into the set
-  BOOST_FOREACH(size_t index, separatorSummarizationSlots_) {
+  for(size_t index: separatorSummarizationSlots_) {
     removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
   }
 
   // TODO: Make this compact
   if(debug) {
     std::cout << "ConcurrentBatchFilter::moveSeparator  Removed Factor Slots: ";
-    BOOST_FOREACH(size_t slot, removedFactorSlots) {
+    for(size_t slot: removedFactorSlots) {
       std::cout << slot << " ";
     }
     std::cout << std::endl;
@@ -559,7 +559,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
 
   // Add these factors to a factor graph
   NonlinearFactorGraph removedFactors;
-  BOOST_FOREACH(size_t slot, removedFactorSlots) {
+  for(size_t slot: removedFactorSlots) {
     if(factors_.at(slot)) {
       removedFactors.push_back(factors_.at(slot));
     }
@@ -574,10 +574,10 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
 
   // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
   KeySet newSeparatorKeys = removedFactors.keys();
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+  for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
     newSeparatorKeys.insert(key_value.key);
   }
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     newSeparatorKeys.erase(key);
   }
 
@@ -585,7 +585,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
 
   // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
   KeySet shortcutKeys = newSeparatorKeys;
-  BOOST_FOREACH(Key key, smootherSummarization_.keys()) {
+  for(Key key: smootherSummarization_.keys()) {
     shortcutKeys.insert(key);
   }
 
@@ -632,7 +632,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
 
   // Update the separatorValues object (should only contain the new separator keys)
   separatorValues_.clear();
-  BOOST_FOREACH(Key key, separatorSummarization.keys()) {
+  for(Key key: separatorSummarization.keys()) {
     separatorValues_.insert(key, theta_.at(key));
   }
 
@@ -641,12 +641,12 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
   removeFactors(removedFactorSlots);
 
   // Add the linearization point of the moved variables to the smoother cache
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     smootherValues_.insert(key, theta_.at(key));
   }
 
   // Remove marginalized keys from values (and separator)
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     theta_.erase(key);
     ordering_.erase(std::find(ordering_.begin(), ordering_.end(), key));
     delta_.erase(key);
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
index 9a458ee5a..53dd46fea 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
@@ -244,7 +244,7 @@ private:
 template<class Container>
 void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
   std::cout << indent << title;
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     std::cout << " " << keyFormatter(key);
   }
   std::cout << std::endl;
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
index 29ee14aee..d64b697b8 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
@@ -28,7 +28,7 @@ namespace gtsam {
 void ConcurrentBatchSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
   std::cout << s;
   std::cout << "  Factors:" << std::endl;
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) {
+  for(const NonlinearFactor::shared_ptr& factor: factors_) {
     PrintNonlinearFactor(factor, "    ", keyFormatter);
   }
   theta_.print("Values:\n");
@@ -61,7 +61,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF
     theta_.insert(newTheta);
 
     // Add new variables to the end of the ordering
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
+    for(const Values::ConstKeyValuePair& key_value: newTheta) {
       ordering_.push_back(key_value.key);
     }
 
@@ -135,7 +135,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
   removeFactors(filterSummarizationSlots_);
 
   // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues) {
+  for(const Values::ConstKeyValuePair& key_value: smootherValues) {
     std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
     if(iter_inserted.second) {
       // If the insert succeeded
@@ -146,7 +146,7 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa
       iter_inserted.first->value = key_value.value;
     }
   }
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues) {
+  for(const Values::ConstKeyValuePair& key_value: separatorValues) {
     std::pair<Values::iterator, bool> iter_inserted = theta_.tryInsert(key_value.key, key_value.value);
     if(iter_inserted.second) {
       // If the insert succeeded
@@ -188,7 +188,7 @@ std::vector<size_t> ConcurrentBatchSmoother::insertFactors(const NonlinearFactor
   slots.reserve(factors.size());
 
   // Insert the factor into an existing hole in the factor graph, if possible
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
+  for(const NonlinearFactor::shared_ptr& factor: factors) {
     size_t slot;
     if(availableSlots_.size() > 0) {
       slot = availableSlots_.front();
@@ -212,7 +212,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector<size_t>& slots) {
   gttic(remove_factors);
 
   // For each factor slot to delete...
-  BOOST_FOREACH(size_t slot, slots) {
+  for(size_t slot: slots) {
 
     // Remove the factor from the graph
     factors_.remove(slot);
@@ -277,7 +277,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
         dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size());
         {
           // for each of the variables, add a prior at the current solution
-          BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) {
+          for(const VectorValues::KeyValuePair& key_value: delta_) {
             size_t dim = key_value.second.size();
             Matrix A = Matrix::Identity(dim,dim);
             Vector b = key_value.second;
@@ -322,7 +322,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
           // Put the linearization points and deltas back for specific variables
           if(separatorValues_.size() > 0) {
             theta_.update(separatorValues_);
-            BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+            for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
               delta_.at(key_value.key) = newDelta.at(key_value.key);
             }
           }
@@ -366,13 +366,13 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
 
   // Create a nonlinear factor graph without the filter summarization factors
   NonlinearFactorGraph graph(factors_);
-  BOOST_FOREACH(size_t slot, filterSummarizationSlots_) {
+  for(size_t slot: filterSummarizationSlots_) {
     graph.remove(slot);
   }
 
   // Get the set of separator keys
   gtsam::KeySet separatorKeys;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+  for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
     separatorKeys.insert(key_value.key);
   }
 
@@ -389,7 +389,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared
     } else {
       std::cout << "f( ";
     }
-    BOOST_FOREACH(Key key, *factor) {
+    for(Key key: *factor) {
       std::cout << keyFormatter(key) << " ";
     }
     std::cout << ")" << std::endl;
@@ -403,7 +403,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr
   std::cout << indent;
   if(factor) {
     std::cout << "g( ";
-    BOOST_FOREACH(Key key, *factor) {
+    for(Key key: *factor) {
       std::cout << keyFormatter(key) << " ";
     }
     std::cout << ")" << std::endl;
diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
index b893860cc..61f1c889f 100644
--- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
@@ -77,7 +77,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
     // Wrap in nonlinear container factors
     NonlinearFactorGraph marginalFactors;
     marginalFactors.reserve(marginalLinearFactors.size());
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) {
+    for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
       marginalFactors += boost::make_shared<LinearContainerFactor>(gaussianFactor, theta);
     }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
index 1b7f86d5c..3913ba95c 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
@@ -69,17 +69,17 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
     int group = 1;
     // Set all existing variables to Group1
     if(isam2_.getLinearizationPoint().size() > 0) {
-      BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
+      for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) {
         orderingConstraints->operator[](key_value.key) = group;
       }
       ++group;
     }
     // Assign new variables to the root
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
+    for(const Values::ConstKeyValuePair& key_value: newTheta) {
       orderingConstraints->operator[](key_value.key) = group;
     }
     // Set marginalizable variables to Group0
-    BOOST_FOREACH(Key key, *keysToMove){
+    for(Key key: *keysToMove){
       orderingConstraints->operator[](key) = 0;
     }
   }
@@ -92,7 +92,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
   boost::optional<FastList<Key> > additionalKeys = boost::none;
   if(keysToMove && keysToMove->size() > 0) {
     std::set<Key> markedKeys;
-    BOOST_FOREACH(Key key, *keysToMove) {
+    for(Key key: *keysToMove) {
       if(isam2_.getLinearizationPoint().exists(key)) {
         ISAM2Clique::shared_ptr clique = isam2_[key];
         GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
@@ -100,7 +100,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
           markedKeys.insert(*key_iter);
           ++key_iter;
         }
-        BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+        for(const ISAM2Clique::shared_ptr& child: clique->children) {
           RecursiveMarkAffectedKeys(key, child, markedKeys);
         }
       }
@@ -120,7 +120,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
     FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_);
     // Cache these factors for later transmission to the smoother
     NonlinearFactorGraph removedFactors;
-    BOOST_FOREACH(size_t slot, removedFactorSlots) {
+    for(size_t slot: removedFactorSlots) {
       const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
       if(factor) {
         smootherFactors_.push_back(factor);
@@ -128,7 +128,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
       }
     }
     // Cache removed values for later transmission to the smoother
-    BOOST_FOREACH(Key key, *keysToMove) {
+    for(Key key: *keysToMove) {
       smootherValues_.insert(key, isam2_.getLinearizationPoint().at(key));
     }
     gttoc(cache_smoother_factors);
@@ -138,7 +138,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
     FactorIndices deletedFactorsIndices;
     isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
     currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
-    BOOST_FOREACH(size_t index, deletedFactorsIndices) {
+    for(size_t index: deletedFactorsIndices) {
       currentSmootherSummarizationSlots_.erase(std::remove(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end(), index), currentSmootherSummarizationSlots_.end());
     }
     gttoc(marginalize);
@@ -193,7 +193,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
   graph.push_back(smootherShortcut_);
   Values values;
   values.insert(smootherSummarizationValues);
-  BOOST_FOREACH(Key key, newSeparatorKeys) {
+  for(Key key: newSeparatorKeys) {
     if(!values.exists(key)) {
       values.insert(key, isam2_.getLinearizationPoint().at(key));
     }
@@ -201,7 +201,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
 
   // Force iSAM2 not to relinearize anything during this iteration
   FastList<Key> noRelinKeys;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
+  for(const Values::ConstKeyValuePair& key_value: isam2_.getLinearizationPoint()) {
     noRelinKeys.push_back(key_value.key);
   }
 
@@ -232,7 +232,7 @@ void ConcurrentIncrementalFilter::getSummarizedFactors(NonlinearFactorGraph& fil
   filterSummarization = calculateFilterSummarization();
 
   // Copy the current separator values into the output
-  BOOST_FOREACH(Key key, isam2_.getFixedVariables()) {
+  for(Key key: isam2_.getFixedVariables()) {
     filterSummarizationValues.insert(key, isam2_.getLinearizationPoint().at(key));
   }
 
@@ -271,12 +271,12 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons
   if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
 
     // Mark the frontal keys of the current clique
-    BOOST_FOREACH(Key idx, clique->conditional()->frontals()) {
+    for(Key idx: clique->conditional()->frontals()) {
       additionalKeys.insert(idx);
     }
 
     // Recursively mark all of the children
-    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+    for(const ISAM2Clique::shared_ptr& child: clique->children) {
       RecursiveMarkAffectedKeys(key, child, additionalKeys);
     }
   }
@@ -290,7 +290,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam
   // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
   FactorIndices removedFactorSlots;
   const VariableIndex& variableIndex = isam2.getVariableIndex();
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     const FastVector<size_t>& slots = variableIndex[key];
     removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
   }
@@ -300,7 +300,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam
   removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
 
   // Remove any linear/marginal factor that made it into the set
-  BOOST_FOREACH(size_t index, factorsToIgnore) {
+  for(size_t index: factorsToIgnore) {
     removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
   }
 
@@ -313,13 +313,13 @@ void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& rem
 
   // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
   KeySet shortcutKeys;
-  BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
+  for(size_t slot: currentSmootherSummarizationSlots_) {
     const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
     if(factor) {
       shortcutKeys.insert(factor->begin(), factor->end());
     }
   }
-  BOOST_FOREACH(Key key, previousSmootherSummarization_.keys()) {
+  for(Key key: previousSmootherSummarization_.keys()) {
     shortcutKeys.insert(key);
   }
 
@@ -347,15 +347,15 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
 
   // Find all cliques that contain any separator variables
   std::set<ISAM2Clique::shared_ptr> separatorCliques;
-  BOOST_FOREACH(Key key, separatorKeys) {
+  for(Key key: separatorKeys) {
     ISAM2Clique::shared_ptr clique = isam2_[key];
     separatorCliques.insert( clique );
   }
 
   // Create the set of clique keys LC:
   std::vector<Key> cliqueKeys;
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
-    BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
+  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
+    for(Key key: clique->conditional()->frontals()) {
       cliqueKeys.push_back(key);
     }
   }
@@ -363,8 +363,8 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
 
   // Gather all factors that involve only clique keys
   std::set<size_t> cliqueFactorSlots;
-  BOOST_FOREACH(Key key, cliqueKeys) {
-    BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) {
+  for(Key key: cliqueKeys) {
+    for(size_t slot: isam2_.getVariableIndex()[key]) {
       const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
       if(factor) {
         std::set<Key> factorKeys(factor->begin(), factor->end());
@@ -376,29 +376,29 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
   }
 
   // Remove any factor included in the current smoother summarization
-  BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
+  for(size_t slot: currentSmootherSummarizationSlots_) {
     cliqueFactorSlots.erase(slot);
   }
 
   // Create a factor graph from the identified factors
   NonlinearFactorGraph graph;
-  BOOST_FOREACH(size_t slot, cliqueFactorSlots) {
+  for(size_t slot: cliqueFactorSlots) {
     graph.push_back(isam2_.getFactorsUnsafe().at(slot));
   }
 
   // Find the set of children of the separator cliques
   std::set<ISAM2Clique::shared_ptr> childCliques;
   // Add all of the children
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
+  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     childCliques.insert(clique->children.begin(), clique->children.end());
   }
   // Remove any separator cliques that were added because they were children of other separator cliques
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
+  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     childCliques.erase(clique);
   }
 
   // Augment the factor graph with cached factors from the children
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) {
+  for(const ISAM2Clique::shared_ptr& clique: childCliques) {
     LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
     graph.push_back( factor );
   }
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
index 268160451..e95c1c81d 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
@@ -66,7 +66,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
   // Also, mark the separator keys as fixed linearization points
   FastMap<Key,int> constrainedKeys;
   FastList<Key> noRelinKeys;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+  for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
     constrainedKeys[key_value.key] = 1;
     noRelinKeys.push_back(key_value.key);
   }
@@ -82,12 +82,12 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
       Values values(newTheta);
       // Unfortunately, we must be careful here, as some of the smoother values
       // and/or separator values may have been added previously
-      BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) {
+      for(const Values::ConstKeyValuePair& key_value: smootherValues_) {
         if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
           values.insert(key_value.key, smootherValues_.at(key_value.key));
         }
       }
-      BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+      for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
         if(!isam2_.getLinearizationPoint().exists(key_value.key)) {
           values.insert(key_value.key, separatorValues_.at(key_value.key));
         }
@@ -188,15 +188,15 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
 
   // Find all cliques that contain any separator variables
   std::set<ISAM2Clique::shared_ptr> separatorCliques;
-  BOOST_FOREACH(Key key, separatorValues_.keys()) {
+  for(Key key: separatorValues_.keys()) {
     ISAM2Clique::shared_ptr clique = isam2_[key];
     separatorCliques.insert( clique );
   }
 
   // Create the set of clique keys LC:
   std::vector<Key> cliqueKeys;
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
-    BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
+  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
+    for(Key key: clique->conditional()->frontals()) {
       cliqueKeys.push_back(key);
     }
   }
@@ -204,8 +204,8 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
 
   // Gather all factors that involve only clique keys
   std::set<size_t> cliqueFactorSlots;
-  BOOST_FOREACH(Key key, cliqueKeys) {
-    BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) {
+  for(Key key: cliqueKeys) {
+    for(size_t slot: isam2_.getVariableIndex()[key]) {
       const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
       if(factor) {
         std::set<Key> factorKeys(factor->begin(), factor->end());
@@ -217,36 +217,36 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
   }
 
   // Remove any factor included in the filter summarization
-  BOOST_FOREACH(size_t slot, filterSummarizationSlots_) {
+  for(size_t slot: filterSummarizationSlots_) {
     cliqueFactorSlots.erase(slot);
   }
 
   // Create a factor graph from the identified factors
   NonlinearFactorGraph graph;
-  BOOST_FOREACH(size_t slot, cliqueFactorSlots) {
+  for(size_t slot: cliqueFactorSlots) {
     graph.push_back(isam2_.getFactorsUnsafe().at(slot));
   }
 
   // Find the set of children of the separator cliques
   std::set<ISAM2Clique::shared_ptr> childCliques;
   // Add all of the children
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
+  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     childCliques.insert(clique->children.begin(), clique->children.end());
   }
   // Remove any separator cliques that were added because they were children of other separator cliques
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) {
+  for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     childCliques.erase(clique);
   }
 
   // Augment the factor graph with cached factors from the children
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) {
+  for(const ISAM2Clique::shared_ptr& clique: childCliques) {
     LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
     graph.push_back( factor );
   }
 
   // Get the set of separator keys
   KeySet separatorKeys;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
+  for(const Values::ConstKeyValuePair& key_value: separatorValues_) {
     separatorKeys.insert(key_value.key);
   }
 
diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp
index 369db51c3..3eecb58e0 100644
--- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp
@@ -36,7 +36,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
 /* ************************************************************************* */
 void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) {
   // Loop through each key and add/update it in the map
-  BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) {
+  for(const KeyTimestampMap::value_type& key_timestamp: timestamps) {
     // Check to see if this key already exists in the database
     KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first);
 
@@ -65,7 +65,7 @@ void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps)
 
 /* ************************************************************************* */
 void FixedLagSmoother::eraseKeyTimestampMap(const std::set<Key>& keys) {
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     // Erase the key from the Timestamp->Key map
     double timestamp = keyTimestampMap_.at(key);
 
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index 8623136cd..ddfd88cae 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -34,12 +34,12 @@ void recursiveMarkAffectedKeys(const Key& key,
       != clique->conditional()->endParents()) {
 
     // Mark the frontal keys of the current clique
-    BOOST_FOREACH(Key i, clique->conditional()->frontals()) {
+    for(Key i: clique->conditional()->frontals()) {
       additionalKeys.insert(i);
     }
 
     // Recursively mark all of the children
-    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+    for(const ISAM2Clique::shared_ptr& child: clique->children) {
       recursiveMarkAffectedKeys(key, child, additionalKeys);
     }
   }
@@ -93,7 +93,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
 
   if (debug) {
     std::cout << "Marginalizable Keys: ";
-    BOOST_FOREACH(Key key, marginalizableKeys) {
+    for(Key key: marginalizableKeys) {
       std::cout << DefaultKeyFormatter(key) << " ";
     }
     std::cout << std::endl;
@@ -116,9 +116,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
 
   // Mark additional keys between the marginalized keys and the leaves
   std::set<Key> additionalKeys;
-  BOOST_FOREACH(Key key, marginalizableKeys) {
+  for(Key key: marginalizableKeys) {
     ISAM2Clique::shared_ptr clique = isam_[key];
-    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+    for(const ISAM2Clique::shared_ptr& child: clique->children) {
       recursiveMarkAffectedKeys(key, child, additionalKeys);
     }
   }
@@ -180,11 +180,11 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
     constrainedKeys = FastMap<Key, int>();
     // Generate ordering constraints so that the marginalizable variables will be eliminated first
     // Set all variables to Group1
-    BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) {
+    for(const TimestampKeyMap::value_type& timestamp_key: timestampKeyMap_) {
       constrainedKeys->operator[](timestamp_key.second) = 1;
     }
     // Set marginalizable variables to Group0
-    BOOST_FOREACH(Key key, marginalizableKeys) {
+    for(Key key: marginalizableKeys) {
       constrainedKeys->operator[](key) = 0;
     }
   }
@@ -194,7 +194,7 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
 void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
     const std::string& label) {
   std::cout << label;
-  BOOST_FOREACH(Key key, keys) {
+  for(Key key: keys) {
     std::cout << " " << DefaultKeyFormatter(key);
   }
   std::cout << std::endl;
@@ -204,7 +204,7 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
 void IncrementalFixedLagSmoother::PrintSymbolicFactor(
     const GaussianFactor::shared_ptr& factor) {
   std::cout << "f(";
-  BOOST_FOREACH(Key key, factor->keys()) {
+  for(Key key: factor->keys()) {
     std::cout << " " << DefaultKeyFormatter(key);
   }
   std::cout << " )" << std::endl;
@@ -214,7 +214,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(
 void IncrementalFixedLagSmoother::PrintSymbolicGraph(
     const GaussianFactorGraph& graph, const std::string& label) {
   std::cout << label << std::endl;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
+  for(const GaussianFactor::shared_ptr& factor: graph) {
     PrintSymbolicFactor(factor);
   }
 }
@@ -224,7 +224,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const ISAM2& isam,
     const std::string& label) {
   std::cout << label << std::endl;
   if (!isam.roots().empty()) {
-    BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) {
+    for(const ISAM2::sharedClique& root: isam.roots()) {
       PrintSymbolicTreeHelper(root);
     }
   } else
@@ -237,18 +237,18 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(
 
   // Print the current clique
   std::cout << indent << "P( ";
-  BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
+  for(Key key: clique->conditional()->frontals()) {
     std::cout << DefaultKeyFormatter(key) << " ";
   }
   if (clique->conditional()->nrParents() > 0)
     std::cout << "| ";
-  BOOST_FOREACH(Key key, clique->conditional()->parents()) {
+  for(Key key: clique->conditional()->parents()) {
     std::cout << DefaultKeyFormatter(key) << " ";
   }
   std::cout << ")" << std::endl;
 
   // Recursively print all of the children
-  BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
+  for(const ISAM2Clique::shared_ptr& child: clique->children) {
     PrintSymbolicTreeHelper(child, indent + " ");
   }
 }
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
index 84dffe7be..a18c24256 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
@@ -16,7 +16,6 @@
  */
 
 #include <gtsam_unstable/nonlinear/LinearizedFactor.h>
-#include <boost/foreach.hpp>
 #include <iostream>
 
 namespace gtsam {
@@ -27,7 +26,7 @@ LinearizedGaussianFactor::LinearizedGaussianFactor(
 : NonlinearFactor(gaussian->keys())
 {
   // Extract the keys and linearization points
-  BOOST_FOREACH(const Key& key, gaussian->keys()) {
+  for(const Key& key: gaussian->keys()) {
     // extract linearization point
     assert(lin_points.exists(key));
     this->lin_points_.insert(key, lin_points.at(key));
@@ -65,7 +64,7 @@ void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& k
   std::cout << s << std::endl;
 
   std::cout << "Nonlinear Keys: ";
-  BOOST_FOREACH(const Key& key, this->keys())
+  for(const Key& key: this->keys())
     std::cout << keyFormatter(key) << " ";
   std::cout << std::endl;
 
@@ -105,7 +104,7 @@ LinearizedJacobianFactor::linearize(const Values& c) const {
 
   // Create the 'terms' data structure for the Jacobian constructor
   std::vector<std::pair<Key, Matrix> > terms;
-  BOOST_FOREACH(Key key, keys()) {
+  for(Key key: keys()) {
     terms.push_back(std::make_pair(key, this->A(key)));
   }
 
@@ -120,7 +119,7 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const {
 
   Vector errorVector = -b();
 
-  BOOST_FOREACH(Key key, this->keys()) {
+  for(Key key: this->keys()) {
     const Value& newPt = c.at(key);
     const Value& linPt = lin_points_.at(key);
     Vector d = linPt.localCoordinates_(newPt);
@@ -162,7 +161,7 @@ void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& ke
   std::cout << s << std::endl;
 
   std::cout << "Nonlinear Keys: ";
-  BOOST_FOREACH(const Key& key, this->keys())
+  for(const Key& key: this->keys())
     std::cout << keyFormatter(key) << " ";
   std::cout << std::endl;
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
index d359d0eff..74ee23fe5 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
@@ -64,18 +64,18 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
 
   std::set<Key> KeysToKeep;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linPoint) { // we cycle over all the keys of factorGraph
+  for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph
     KeysToKeep.insert(key_value.key);
   } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
-  BOOST_FOREACH(Key key, keysToMarginalize) {
+  for(Key key: keysToMarginalize) {
     KeysToKeep.erase(key);
   } // we removed the keys that we have to marginalize
 
   Ordering ordering;
-  BOOST_FOREACH(Key key, keysToMarginalize) {
+  for(Key key: keysToMarginalize) {
     ordering.push_back(key);
   } // the keys that we marginalize should be at the beginning in the ordering
-  BOOST_FOREACH(Key key, KeysToKeep) {
+  for(Key key: KeysToKeep) {
     ordering.push_back(key);
   }
 
@@ -84,7 +84,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
   GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+  for(const GaussianFactor::shared_ptr& factor: marginal) {
     LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
   }
   return LinearContainerForGaussianMarginals;
@@ -439,7 +439,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
   // ==========================================================
   expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
 
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) {
+  for(const GaussianFactor::shared_ptr& factor: result) {
     expectedGraph.push_back(LinearContainerFactor(factor, partialValues));
   }
 
@@ -451,7 +451,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
   Values expectedValues = optimalValues;
 
   // Check
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     expectedValues.erase(key);
   }
 
@@ -1014,7 +1014,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) {
+  for(const GaussianFactor::shared_ptr& factor: result) {
     expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
 
   }
@@ -1069,7 +1069,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) {
+  for(const GaussianFactor::shared_ptr& factor: result) {
     expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
 
   }
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
index 558654367..dc316e2ac 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
@@ -560,14 +560,14 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
   GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
 
   KeySet eliminateKeys = linearFactors->keys();
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
+  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     eliminateKeys.erase(key_value.key);
   }
   std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end());
   GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;
 
   expectedSmootherSummarization.resize(0);
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result) {
+  for(const GaussianFactor::shared_ptr& factor: result) {
     expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
   }
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index a283ece29..0c922fb9e 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -62,7 +62,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int
 
   // it is the same as the input graph, but we removed the empty factors that may be present in the input graph
   NonlinearFactorGraph graphForISAM2;
-  BOOST_FOREACH(NonlinearFactor::shared_ptr factor,  graph){
+  for(NonlinearFactor::shared_ptr factor:  graph){
     if(factor)
       graphForISAM2.push_back(factor);
   }
@@ -80,18 +80,18 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
 
   std::set<Key> KeysToKeep;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linPoint) { // we cycle over all the keys of factorGraph
+  for(const Values::ConstKeyValuePair& key_value: linPoint) { // we cycle over all the keys of factorGraph
     KeysToKeep.insert(key_value.key);
   } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize
-  BOOST_FOREACH(Key key, keysToMarginalize) {
+  for(Key key: keysToMarginalize) {
     KeysToKeep.erase(key);
   } // we removed the keys that we have to marginalize
 
   Ordering ordering;
-  BOOST_FOREACH(Key key, keysToMarginalize) {
+  for(Key key: keysToMarginalize) {
     ordering.push_back(key);
   } // the keys that we marginalize should be at the beginning in the ordering
-  BOOST_FOREACH(Key key, KeysToKeep) {
+  for(Key key: KeysToKeep) {
     ordering.push_back(key);
   }
 
@@ -101,7 +101,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
   GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+  for(const GaussianFactor::shared_ptr& factor: marginal) {
     LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
   }
 
@@ -417,7 +417,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
   Values expectedValues = optimalValues;
 
   // Check
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     expectedValues.erase(key);
   }
 
@@ -443,7 +443,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
   // ==========================================================
   expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
 
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+  for(const GaussianFactor::shared_ptr& factor: marginal) {
     // the linearization point for the linear container is optional, but it is not used in the filter,
     // therefore if we add it here it will not pass the test
     //    expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
@@ -501,7 +501,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
   Values expectedValues = optimalValues;
 
   // Check
-  BOOST_FOREACH(Key key, keysToMove) {
+  for(Key key: keysToMove) {
     expectedValues.erase(key);
   }
 
@@ -527,7 +527,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
     // ==========================================================
     expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
 
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+    for(const GaussianFactor::shared_ptr& factor: marginal) {
       // the linearization point for the linear container is optional, but it is not used in the filter,
       // therefore if we add it here it will not pass the test
       //    expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
@@ -543,7 +543,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
   Values optimalValues2 = BatchOptimize(newFactors, optimalValues);
   Values expectedValues2 = optimalValues2;
    // Check
-   BOOST_FOREACH(Key key, keysToMove) {
+   for(Key key: keysToMove) {
      expectedValues2.erase(key);
    }
    Values actualValues2 = filter.calculateEstimate();
@@ -1116,7 +1116,7 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
     GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
 
     NonlinearFactorGraph expectedMarginals;
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+    for(const GaussianFactor::shared_ptr& factor: marginal) {
       expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
     }
 
@@ -1167,7 +1167,7 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
   GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+  for(const GaussianFactor::shared_ptr& factor: marginal) {
     expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
   }
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
index 22dd0ccaa..c265bf5d1 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
@@ -512,7 +512,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
 //  Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
   Values expectedLinearizationPoint = filterSeparatorValues;
   Values actualLinearizationPoint;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
+  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
   }
   CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
@@ -580,14 +580,14 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
   KeySet allkeys = LinFactorGraph->keys();
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
+  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     allkeys.erase(key_value.key);
   }
   std::vector<Key> variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
 
   expectedSmootherSummarization.resize(0);
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) {
+  for(const GaussianFactor::shared_ptr& factor: *result.second) {
     expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
   }
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
index 858fbb75c..65f74dc3c 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
@@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
 //  Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1);
   Values expectedLinearizationPoint = filterSeparatorValues;
   Values actualLinearizationPoint;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
+  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key));
   }
   CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
@@ -582,13 +582,13 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
   KeySet allkeys = LinFactorGraph->keys();
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues)
+  for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
     allkeys.erase(key_value.key);
   std::vector<Key> variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
 
   expectedSmootherSummarization.resize(0);
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *result.second) {
+  for(const GaussianFactor::shared_ptr& factor: *result.second) {
     expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
   }
 
diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index ace13dc41..a65e3f1ca 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -13,7 +13,6 @@
 #include <stdexcept>
 #include <iostream>
 #include <boost/tuple/tuple.hpp>
-#include <boost/foreach.hpp>
 #include <boost/shared_array.hpp>
 #include <boost/timer.hpp>
 
@@ -193,7 +192,7 @@ namespace gtsam { namespace partition {
     std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
     int index1, index2;
 
-    BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
+    for(const typename GenericGraph::value_type& factor: graph){
       index1 = dictionary[factor->key1.index];
       index2 = dictionary[factor->key2.index];
       std::cout << "index1: " << index1 << std::endl;
@@ -222,7 +221,7 @@ namespace gtsam { namespace partition {
     sharedInts& adjncy = *ptr_adjncy;
     sharedInts& adjwgt = *ptr_adjwgt;
     int ind_xadj = 0, ind_adjncy = 0;
-    BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) {
+    for(const NeighborsInfo& info: adjacencyMap) {
       *(xadj.get() + ind_xadj) = ind_adjncy;
       std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
       std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
@@ -334,7 +333,7 @@ namespace gtsam { namespace partition {
         << " " << result.B.size() << std::endl;
       int edgeCut = 0;
 
-      BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
+      for(const typename GenericGraph::value_type& factor: graph){
         int key1 = factor->key1.index;
         int key2 = factor->key2.index;
         // print keys and their subgraph assignment
@@ -372,19 +371,19 @@ namespace gtsam { namespace partition {
   // debug functions
   void printIsland(const std::vector<size_t>& island) {
     std::cout << "island: ";
-    BOOST_FOREACH(const size_t key, island)
+    for(const size_t key: island)
       std::cout << key << " ";
     std::cout << std::endl;
   }
 
   void printIslands(const std::list<std::vector<size_t> >& islands) {
-    BOOST_FOREACH(const std::vector<std::size_t>& island, islands)
+    for(const std::vector<std::size_t>& island: islands)
         printIsland(island);
   }
 
   void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
     int numCamera = 0, numLandmark = 0;
-    BOOST_FOREACH(const size_t key, keys)
+    for(const size_t key: keys)
     if (int2symbol[key].chr() == 'x')
       numCamera++;
     else
@@ -403,16 +402,16 @@ namespace gtsam { namespace partition {
     std::vector<size_t>& C = partitionResult.C;
     std::vector<int>& dictionary = workspace.dictionary;
     std::fill(dictionary.begin(), dictionary.end(), -1);
-    BOOST_FOREACH(const size_t a, A)
+    for(const size_t a: A)
       dictionary[a] = 1;
-    BOOST_FOREACH(const size_t b, B)
+    for(const size_t b: B)
       dictionary[b] = 2;
     if (!C.empty())
       throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
 
     // set up landmarks
     size_t i,j;
-    BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) {
+    for(const typename GenericGraph::value_type& factor: graph) {
       i = factor->key1.index;
       j = factor->key2.index;
       if (dictionary[j] == 0) // if the landmark is already in the separator, continue
@@ -427,7 +426,7 @@ namespace gtsam { namespace partition {
 //        std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
     }
 
-    BOOST_FOREACH(const size_t j, landmarkKeys) {
+    for(const size_t j: landmarkKeys) {
       switch(dictionary[j]) {
       case 0: C.push_back(j); break;
       case 1: A.push_back(j); break;
@@ -456,7 +455,7 @@ namespace gtsam { namespace partition {
       // find out all the landmark keys, which are to be eliminated
       cameraKeys.reserve(keys.size());
       landmarkKeys.reserve(keys.size());
-      BOOST_FOREACH(const size_t key, keys) {
+      for(const size_t key: keys) {
         if((*int2symbol)[key].chr() == 'x')
           cameraKeys.push_back(key);
         else
@@ -518,11 +517,11 @@ namespace gtsam { namespace partition {
 //    printNumCamerasLandmarks(result->C, *int2symbol);
 //    std::cout << "no. of island: " << islands.size() << "; ";
 //    std::cout << "island size: ";
-//    BOOST_FOREACH(const Island& island, islands)
+//    for(const Island& island: islands)
 //      std::cout << island.size() << " ";
 //    std::cout << std::endl;
 
-//    BOOST_FOREACH(const Island& island, islands) {
+//    for(const Island& island: islands) {
 //      printNumCamerasLandmarks(island, int2symbol);
 //    }
 #endif
@@ -550,12 +549,12 @@ namespace gtsam { namespace partition {
     // generate the node map
     std::vector<int>& partitionTable = workspace.partitionTable;
     std::fill(partitionTable.begin(), partitionTable.end(), -1);
-    BOOST_FOREACH(const size_t key, result->C)
+    for(const size_t key: result->C)
       partitionTable[key] = 0;
     int idx = 0;
-    BOOST_FOREACH(const Island& island, islands) {
+    for(const Island& island: islands) {
       idx++;
-      BOOST_FOREACH(const size_t key, island) {
+      for(const size_t key: island) {
         partitionTable[key] = idx;
       }
     }
diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp
index b78c1d3dc..3ac77c213 100644
--- a/gtsam_unstable/partition/GenericGraph.cpp
+++ b/gtsam_unstable/partition/GenericGraph.cpp
@@ -6,7 +6,6 @@
  *  Description: generic graph types used in partitioning
  */
 #include <iostream>
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/lexical_cast.hpp>
@@ -98,7 +97,7 @@ namespace gtsam { namespace partition {
       }
 
       // erase unused factors
-      BOOST_FOREACH(const FactorList::iterator& it, toErase)
+      for(const FactorList::iterator& it: toErase)
         factors.erase(it);
 
       if (!succeed) break;
@@ -107,7 +106,7 @@ namespace gtsam { namespace partition {
     list<vector<size_t> > islands;
     map<size_t, vector<size_t> > arrays = dsf.arrays();
     size_t key; vector<size_t> array;
-    BOOST_FOREACH(boost::tie(key, array), arrays)
+    for(boost::tie(key, array): arrays)
       islands.push_back(array);
     return islands;
   }
@@ -116,14 +115,14 @@ namespace gtsam { namespace partition {
   /* ************************************************************************* */
   void print(const GenericGraph2D& graph, const std::string name) {
     cout << name << endl;
-    BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
+    for(const sharedGenericFactor2D& factor_: graph)
       cout << factor_->key1.index << " " << factor_->key2.index << endl;
   }
 
   /* ************************************************************************* */
   void print(const GenericGraph3D& graph, const std::string name) {
     cout << name << endl;
-    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
+    for(const sharedGenericFactor3D& factor_: graph)
       cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
       factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
   }
@@ -174,7 +173,7 @@ namespace gtsam { namespace partition {
       }
 
       // erase unused factors
-      BOOST_FOREACH(const FactorList::iterator& it, toErase)
+      for(const FactorList::iterator& it: toErase)
       factors.erase(it);
 
       if (!succeed) break;
@@ -204,7 +203,7 @@ namespace gtsam { namespace partition {
 
     // compute the constraint number per camera
     std::fill(nrConstraints.begin(),  nrConstraints.end(),    0);
-    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    for(const sharedGenericFactor3D& factor_: graph) {
       const int& key1 = factor_->key1.index;
       const int& key2 = factor_->key2.index;
       if (workspace.dictionary[key1] != -1 &&  workspace.dictionary[key2] != -1 &&
@@ -258,7 +257,7 @@ namespace gtsam { namespace partition {
 
     // check the constraint number of every variable
     // find the camera and landmark keys
-    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    for(const sharedGenericFactor3D& factor_: graph) {
       //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
       if (workspace.dictionary[factor_->key1.index] != -1) {
         if (factor_->key1.type == NODE_POSE_3D)
@@ -287,7 +286,7 @@ namespace gtsam { namespace partition {
     // add singular variables directly as islands
     if (!singularCameras.empty()) {
       if (verbose) cout << "singular cameras:";
-      BOOST_FOREACH(const size_t i, singularCameras) {
+      for(const size_t i: singularCameras) {
         islands.push_back(vector<size_t>(1, i)); // <---------------------------
         if (verbose) cout << i << " ";
       }
@@ -295,7 +294,7 @@ namespace gtsam { namespace partition {
     }
     if (!singularLandmarks.empty()) {
       if (verbose) cout << "singular landmarks:";
-      BOOST_FOREACH(const size_t i, singularLandmarks) {
+      for(const size_t i: singularLandmarks) {
         islands.push_back(vector<size_t>(1, i)); // <---------------------------
         if (verbose) cout << i << " ";
       }
@@ -306,10 +305,10 @@ namespace gtsam { namespace partition {
     // regenerating islands
     map<size_t, vector<size_t> > labelIslands = dsf.arrays();
     size_t label; vector<size_t> island;
-    BOOST_FOREACH(boost::tie(label, island), labelIslands) {
+    for(boost::tie(label, island): labelIslands) {
       vector<size_t> filteredIsland; // remove singular cameras from array
       filteredIsland.reserve(island.size());
-      BOOST_FOREACH(const size_t key, island) {
+      for(const size_t key: island) {
         if ((isCamera[key]   && singularCameras.find(key) == singularCameras.end()) ||        // not singular
             (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) ||    // not singular
             (!isCamera[key] && !isLandmark[key])) {   // the key is not involved in any factor, so the type is undertermined
@@ -321,7 +320,7 @@ namespace gtsam { namespace partition {
 
     // sanity check
     size_t nrKeys = 0;
-    BOOST_FOREACH(const vector<size_t>& island, islands)
+    for(const vector<size_t>& island: islands)
       nrKeys += island.size();
     if (nrKeys != keys.size())  {
       cout << nrKeys << " vs " << keys.size() << endl;
@@ -363,7 +362,7 @@ namespace gtsam { namespace partition {
     // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
     vector<int> cameraToCamera(dictionary.size(), -1);
     size_t key_i, key_j;
-    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    for(const sharedGenericFactor3D& factor_: graph) {
       if (factor_->key1.type == NODE_POSE_3D) {
         if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
           cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
@@ -382,7 +381,7 @@ namespace gtsam { namespace partition {
     }
 
     // sort the landmark keys for the late getNrCommonLandmarks call
-    BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
+    for(vector<LandmarkKey> &landmarks: cameraToLandmarks){
       if (!landmarks.empty())
         std::sort(landmarks.begin(), landmarks.end());
     }
@@ -417,7 +416,7 @@ namespace gtsam { namespace partition {
     const vector<int>& dictionary = workspace.dictionary;
     vector<bool> isValidCamera(workspace.dictionary.size(), false);
     vector<bool> isValidLandmark(workspace.dictionary.size(), false);
-    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    for(const sharedGenericFactor3D& factor_: graph) {
       assert(factor_->key1.type == NODE_POSE_3D);
       //assert(factor_->key2.type == NODE_LANDMARK_3D);
       const size_t& key1 = factor_->key1.index;
@@ -463,7 +462,7 @@ namespace gtsam { namespace partition {
     }
 
     // debug info
-    BOOST_FOREACH(const size_t key, frontals) {
+    for(const size_t key: frontals) {
       if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
         cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
     }
diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h
index 6f1818a3e..28076bda0 100644
--- a/gtsam_unstable/partition/NestedDissection-inl.h
+++ b/gtsam_unstable/partition/NestedDissection-inl.h
@@ -70,11 +70,11 @@ namespace gtsam { namespace partition {
   boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
       const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
     OrderedSymbols frontalKeys;
-    BOOST_FOREACH(const size_t index, frontals)
+    for(const size_t index: frontals)
       frontalKeys.push_back(int2symbol_[index]);
 
     UnorderedSymbols sepKeys;
-    BOOST_FOREACH(const size_t index, sep)
+    for(const size_t index: sep)
       sepKeys.insert(int2symbol_[index]);
 
     return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
@@ -129,7 +129,7 @@ namespace gtsam { namespace partition {
     // make three lists of variables A, B, and C
     int partition;
     childFrontals.resize(numSubmaps);
-    BOOST_FOREACH(const size_t key, keys){
+    for(const size_t key: keys){
       partition = partitionTable[key];
       switch (partition) {
       case -1: break;                                        // the separator of the separator variables
@@ -144,13 +144,13 @@ namespace gtsam { namespace partition {
     childSeps.reserve(numSubmaps);
     frontalFactors.resize(numSubmaps);
     frontalUnaryFactors.resize(numSubmaps);
-    BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
+    for(typename GenericGraph::value_type factor: fg)
       processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
-    BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
+    for(const set<size_t>& childSep: childSeps_)
       childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
 
     // add unary factor to the current cluster or pass it to one of the child clusters
-    BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
+    for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors) {
       partition = partitionTable[unaryFactor_->key.index];
       if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
       else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
@@ -164,7 +164,7 @@ namespace gtsam { namespace partition {
     NLG sepFactors;
     typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
     while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
-    BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
+    for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors)
       sepFactors.push_back(fg_[unaryFactor_->index]);
     return sepFactors;
   }
diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp
index 9bdf40026..8c8f12d74 100644
--- a/gtsam_unstable/partition/tests/testFindSeparator.cpp
+++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp
@@ -10,7 +10,6 @@
 #include <boost/assign/std/set.hpp> // for operator +=
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 #include <CppUnitLite/TestHarness.h>
 
@@ -89,10 +88,10 @@ TEST ( Partition, edgePartitionByMetis )
   vector<size_t> A_expected; A_expected += 0, 1; // frontal
   vector<size_t> B_expected; B_expected += 2, 3; // frontal
   vector<size_t> C_expected;    // separator
-//  BOOST_FOREACH(const size_t a, actual->A)
+//  for(const size_t a: actual->A)
 //    cout << a << " ";
 //  cout << endl;
-//  BOOST_FOREACH(const size_t b, actual->B)
+//  for(const size_t b: actual->B)
 //    cout << b << " ";
 //  cout << endl;
 
diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp
index c6e432902..8a32837f4 100644
--- a/gtsam_unstable/partition/tests/testGenericGraph.cpp
+++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp
@@ -6,16 +6,17 @@
  *  Description: unit tests for generic graph
  */
 
-#include <map>
+#include <gtsam_unstable/partition/GenericGraph.h>
+
+#include <CppUnitLite/TestHarness.h>
+
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/set.hpp> // for operator +=
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
-#include <CppUnitLite/TestHarness.h>
 
-#include <gtsam_unstable/partition/GenericGraph.h>
+#include <map>
 
 using namespace std;
 using namespace gtsam;
diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp
index d7035c2d8..acaa5557e 100644
--- a/gtsam_unstable/partition/tests/testNestedDissection.cpp
+++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp
@@ -10,7 +10,6 @@
 #include <boost/assign/std/set.hpp> // for operator +=
 #include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
-#include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 #include <CppUnitLite/TestHarness.h>
 
diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp
index 8519f6f8d..df6b1e50d 100644
--- a/gtsam_unstable/slam/DummyFactor.cpp
+++ b/gtsam_unstable/slam/DummyFactor.cpp
@@ -8,7 +8,6 @@
 #include <gtsam_unstable/slam/DummyFactor.h>
 
 #include <boost/assign/list_of.hpp>
-#include <boost/foreach.hpp>
 
 using namespace boost::assign;
 
@@ -29,7 +28,7 @@ DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t d
 /* ************************************************************************* */
 void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
   std::cout << s << "  DummyFactor dim = " << rowDim_ << ", keys = { ";
-  BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
+  for(Key key: this->keys()) { std::cout << keyFormatter(key) << " "; }
   std::cout << "}" << std::endl;
 }
 
diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp
index 94126a68c..6112a398f 100644
--- a/gtsam_unstable/slam/Mechanization_bRn2.cpp
+++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp
@@ -6,7 +6,6 @@
  */
 
 #include "Mechanization_bRn2.h"
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h
index 3db302d0b..0447f2e39 100644
--- a/gtsam_unstable/slam/SmartRangeFactor.h
+++ b/gtsam_unstable/slam/SmartRangeFactor.h
@@ -14,7 +14,6 @@
 #include <gtsam/inference/Key.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/timing.h>
-#include <boost/foreach.hpp>
 #include <map>
 
 namespace gtsam {
@@ -100,7 +99,7 @@ public:
     boost::optional<Circle2> best_circle;
 
     // loop over all circles
-    BOOST_FOREACH(const Circle2& it, circles) {
+    for(const Circle2& it: circles) {
       // distance between circle centers.
       double d = circle1.center.dist(it.center);
       if (d < 1e-9)
@@ -123,7 +122,7 @@ public:
     // pick winner based on other measurements
     double error1 = 0, error2 = 0;
     Point2 p1 = intersections.front(), p2 = intersections.back();
-    BOOST_FOREACH(const Circle2& it, circles) {
+    for(const Circle2& it: circles) {
       error1 += it.center.dist(p1);
       error2 += it.center.dist(p2);
     }
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 56e22aae2..bede012d8 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -254,7 +254,7 @@ public:
       }
 
       Point3 pw_sum(0,0,0);
-      BOOST_FOREACH(const Point3& pw, reprojections) {
+      for(const Point3& pw: reprojections) {
         pw_sum = pw_sum + pw;
       }
       // average reprojected landmark
@@ -339,9 +339,9 @@ public:
 
     if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
       // failed: return"empty" Hessian
-      BOOST_FOREACH(Matrix& m, Gs)
+      for(Matrix& m: Gs)
         m = Matrix::Zero(Base::Dim, Base::Dim);
-      BOOST_FOREACH(Vector& v, gs)
+      for(Vector& v: gs)
         v = Vector::Zero(Base::Dim);
       return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
           Gs, gs, 0.0);
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
index c2526fd74..65134cb96 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -122,7 +122,7 @@ public:
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
-    BOOST_FOREACH(const boost::shared_ptr<Cal3_S2Stereo>& K, K_all_)
+    for(const boost::shared_ptr<Cal3_S2Stereo>& K: K_all_)
     K->print("calibration = ");
     Base::print("", keyFormatter);
   }
@@ -160,7 +160,7 @@ public:
    Base::Cameras cameras(const Values& values) const {
     Base::Cameras cameras;
     size_t i=0;
-    BOOST_FOREACH(const Key& k, this->keys_) {
+    for(const Key& k: this->keys_) {
       const Pose3& pose = values.at<Pose3>(k);
       StereoCamera camera(pose, K_all_[i++]);
       cameras.push_back(camera);
diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp
index f0bbb9072..f39bdda59 100644
--- a/gtsam_unstable/timing/timeDSFvariants.cpp
+++ b/gtsam_unstable/timing/timeDSFvariants.cpp
@@ -23,7 +23,6 @@
 #include <boost/random.hpp>
 #include <boost/timer.hpp>
 #include <boost/format.hpp>
-#include <boost/foreach.hpp>
 #include <boost/assign/std/vector.hpp>
 
 #include <iostream>
@@ -46,7 +45,7 @@ int main(int argc, char* argv[]) {
   // loop over number of images
   vector<size_t> ms;
   ms += 10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000;
-  BOOST_FOREACH(size_t m,ms) {
+  for(size_t m: ms) {
     // We use volatile here to make these appear to the optimizing compiler as
     // if their values are only known at run-time.
     volatile size_t n = 500; // number of points per image
@@ -74,7 +73,7 @@ int main(int argc, char* argv[]) {
       // DSFBase version
       timer tim;
       DSFBase dsf(N); // Allow for N keys
-      BOOST_FOREACH(const Match& m, matches)
+      for(const Match& m: matches)
         dsf.merge(m.first, m.second);
       os << tim.elapsed() << ",";
       cout << format("DSFBase: %1% s") % tim.elapsed() << endl;
@@ -84,7 +83,7 @@ int main(int argc, char* argv[]) {
       // DSFMap version
       timer tim;
       DSFMap<size_t> dsf;
-      BOOST_FOREACH(const Match& m, matches)
+      for(const Match& m: matches)
         dsf.merge(m.first, m.second);
       os << tim.elapsed() << endl;
       cout << format("DSFMap: %1% s") % tim.elapsed() << endl;
@@ -96,7 +95,7 @@ int main(int argc, char* argv[]) {
       DSF<size_t> dsf;
       for (size_t j = 0; j < N; j++)
         dsf = dsf.makeSet(j);
-      BOOST_FOREACH(const Match& m, matches)
+      for(const Match& m: matches)
         dsf = dsf.makeUnion(m.first, m.second);
       os << tim.elapsed() << endl;
       cout << format("DSF functional: %1% s") % tim.elapsed() << endl;
@@ -108,7 +107,7 @@ int main(int argc, char* argv[]) {
       DSF<size_t> dsf;
       for (size_t j = 0; j < N; j++)
         dsf.makeSetInPlace(j);
-      BOOST_FOREACH(const Match& m, matches)
+      for(const Match& m: matches)
         dsf.makeUnionInPlace(m.first, m.second);
       os << tim.elapsed() << ",";
       cout << format("DSF in-place: %1% s") % tim.elapsed() << endl;
diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp
index e3c17fa3f..8b2aa3ae7 100644
--- a/tests/testGaussianBayesTreeB.cpp
+++ b/tests/testGaussianBayesTreeB.cpp
@@ -26,7 +26,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
@@ -220,7 +219,7 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts )
 //  Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
 //  Permutation toFrontInverse(*toFront.inverse());
 //  varIndex.permute(toFront);
-//  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+//  for(const GaussianFactor::shared_ptr& factor: marginal) {
 //    factor->permuteWithInverse(toFrontInverse); }
 //  GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
 //  actual.permuteWithInverse(toFront);
diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp
index 10de57435..eec62ff9c 100644
--- a/tests/testGaussianFactorGraphB.cpp
+++ b/tests/testGaussianFactorGraphB.cpp
@@ -26,7 +26,6 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/set.hpp> // for operator +=
@@ -585,7 +584,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
   GaussianBayesTree actBT = *lfg.eliminateMultifrontal();
 
   // Check that all sigmas in an unconstrained bayes tree are set to one
-  BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) {
+  for(const GaussianBayesTree::sharedClique& clique: actBT.nodes() | br::map_values) {
     GaussianConditional::shared_ptr conditional = clique->conditional();
     //size_t dim = conditional->rows();
     //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol));
diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp
index 7f53406c8..07323e0fc 100644
--- a/tests/testGaussianISAM.cpp
+++ b/tests/testGaussianISAM.cpp
@@ -22,7 +22,6 @@
 #include <gtsam/linear/GaussianISAM.h>
 #include <gtsam/inference/Ordering.h>
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 #include <boost/range/adaptor/map.hpp>
@@ -46,7 +45,7 @@ TEST( ISAM, iSAM_smoother )
 
   // run iSAM for every factor
   GaussianISAM actual;
-  BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
+  for(boost::shared_ptr<GaussianFactor> factor: smoother) {
     GaussianFactorGraph factorGraph;
     factorGraph.push_back(factor);
     actual.update(factorGraph);
@@ -56,7 +55,7 @@ TEST( ISAM, iSAM_smoother )
   GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
 
   // Verify sigmas in the bayes tree
-  BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, expected.nodes() | br::map_values) {
+  for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) {
     GaussianConditional::shared_ptr conditional = clique->conditional();
     EXPECT(!conditional->get_model());
   }
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index d358caa35..f56b458be 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -23,7 +23,6 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/treeTraversal-inst.h>
-#include <boost/foreach.hpp>
 #include <boost/assign/list_of.hpp>
 #include <gtsam/base/deprecated/LieScalar.h>
 using namespace boost::assign;
@@ -212,7 +211,7 @@ struct ConsistencyVisitor
       if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
         consistent = false;
     }
-    BOOST_FOREACH(Key j, node->conditional()->frontals())
+    for(Key j: node->conditional()->frontals())
     {
       if(isam.nodes().at(j).get() != node.get())
         consistent = false;
@@ -251,7 +250,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
   // Check gradient at each node
   bool nodeGradientsOk = true;
   typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) {
+  for(const sharedClique& clique: isam.nodes() | br::map_values) {
     // Compute expected gradient
     GaussianFactorGraph jfg;
     jfg += clique->conditional();
@@ -483,7 +482,7 @@ TEST(ISAM2, swapFactors)
 
   // Check gradient at each node
   typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) {
+  for(const sharedClique& clique: isam.nodes() | br::map_values) {
     // Compute expected gradient
     GaussianFactorGraph jfg;
     jfg += clique->conditional();
@@ -608,7 +607,7 @@ TEST(ISAM2, constrained_ordering)
 
   // Check gradient at each node
   typedef ISAM2::sharedClique sharedClique;
-  BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) {
+  for(const sharedClique& clique: isam.nodes() | br::map_values) {
     // Compute expected gradient
     GaussianFactorGraph jfg;
     jfg += clique->conditional();
@@ -650,7 +649,7 @@ namespace {
   bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
     Matrix expectedAugmentedHessian, expected3AugmentedHessian;
     vector<Key> toKeep;
-    BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys)
+    for(Key j: isam.getDelta() | br::map_keys)
       if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
         toKeep.push_back(j);
 
diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp
index aea18c90d..410617cbf 100644
--- a/tests/testGeneralSFMFactorB.cpp
+++ b/tests/testGeneralSFMFactorB.cpp
@@ -29,7 +29,6 @@
 #include <CppUnitLite/Test.h>
 #include <CppUnitLite/TestRegistry.h>
 #include <CppUnitLite/TestResult.h>
-#include <boost/foreach.hpp>
 #include <stddef.h>
 #include <stdexcept>
 #include <string>
@@ -51,7 +50,7 @@ TEST(PinholeCamera, BAL) {
   NonlinearFactorGraph graph;
 
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
+    for (const SfM_Measurement& m: db.tracks[j].measurements)
       graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
   }
 
diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index 6a49f66d3..accf9a65e 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -27,7 +27,6 @@
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp>
 using namespace boost::assign;
@@ -57,7 +56,7 @@ TEST( SubgraphPreconditioner, planarOrdering ) {
 /** unnormalized error */
 static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
   double total_error = 0.;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
+  for(const GaussianFactor::shared_ptr& factor: fg)
     total_error += factor->error(x);
   return total_error;
 }
diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp
index f5d0384f2..aeeed1b9f 100644
--- a/tests/testSubgraphSolver.cpp
+++ b/tests/testSubgraphSolver.cpp
@@ -28,7 +28,6 @@
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp>
 using namespace boost::assign;
@@ -41,7 +40,7 @@ using namespace example;
 /** unnormalized error */
 static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
   double total_error = 0.;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
+  for(const GaussianFactor::shared_ptr& factor: fg)
     total_error += factor->error(x);
   return total_error;
 }
diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h
index 08e9d8f4b..5e2664d26 100644
--- a/timing/DummyFactor.h
+++ b/timing/DummyFactor.h
@@ -40,7 +40,7 @@ public:
   void multiplyHessian(double alpha, const VectorValues& x,
       VectorValues& y) const {
 
-    BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) {
+    for(const KeyMatrix2D& Fi: this->Fblocks_) {
       static const Vector empty;
       Key key = Fi.first;
       std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
diff --git a/timing/timeBatch.cpp b/timing/timeBatch.cpp
index a1e5f359b..4ed1a4555 100644
--- a/timing/timeBatch.cpp
+++ b/timing/timeBatch.cpp
@@ -57,7 +57,7 @@ int main(int argc, char *argv[]) {
     // Compute marginals
     Marginals marginals(graph, optimizer.values());
     int i=0;
-    BOOST_FOREACH(Key key, initial.keys()) {
+    for(Key key: initial.keys()) {
       gttic_(marginalInformation);
       Matrix info = marginals.marginalInformation(key);
       gttoc_(marginalInformation);
diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp
index a3157729e..cd11f6360 100644
--- a/timing/timeGaussianFactorGraph.cpp
+++ b/timing/timeGaussianFactorGraph.cpp
@@ -16,7 +16,6 @@
  */
 
 #include <time.h>
-#include <boost/foreach.hpp>
 #include <boost/assign/std/list.hpp> // for operator += in Ordering
 #include <CppUnitLite/TestHarness.h>
 #include <tests/smallExample.h>
@@ -82,7 +81,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
 //    // create an internal ordering to render Ab
 //    Ordering render;
 //    render += key;
-//    BOOST_FOREACH(const Symbol& k, joint_factor->keys())
+//    for(const Symbol& k: joint_factor->keys())
 //    if (k != key) render += k;
 //
 //    Matrix Ab = joint_factor->matrix_augmented(render,false);
diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp
index b5dd37516..61e129699 100644
--- a/timing/timeIncremental.cpp
+++ b/timing/timeIncremental.cpp
@@ -28,6 +28,7 @@
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/serialization/export.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -60,7 +61,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 += nlf->dim();
   }
   double dof = graph_dim - config.dim(); // kaess: changed to dim
@@ -225,9 +226,9 @@ int main(int argc, char *argv[]) {
   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 key2: boost::adaptors::reverse(values.keys())) {
         if(i != j) {
           gttic_(jointMarginalInformation);
           std::vector<Key> keys(2);
@@ -246,7 +247,7 @@ int main(int argc, char *argv[]) {
         break;
     }
     tictoc_print_();
-    BOOST_FOREACH(Key key, values.keys()) {
+    for(Key key: values.keys()) {
       gttic_(marginalInformation);
       Matrix info = marginals.marginalInformation(key);
       gttoc_(marginalInformation);
diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp
index 3a4da89b5..d0b6b263c 100644
--- a/timing/timeLago.cpp
+++ b/timing/timeLago.cpp
@@ -45,7 +45,7 @@ int main(int argc, char *argv[]) {
   Sampler sampler(42u);
   Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
   SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
-  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses)
+  for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: poses)
     initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
 
   // Add prior on the pose having index (key) = 0
diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp
index a2d11a756..a2010891b 100644
--- a/timing/timeMatrixOps.cpp
+++ b/timing/timeMatrixOps.cpp
@@ -20,7 +20,6 @@
 #include <boost/timer.hpp>
 #include <boost/format.hpp>
 #include <boost/lambda/lambda.hpp>
-#include <boost/foreach.hpp>
 
 #include <gtsam/base/Matrix.h>
 
@@ -173,29 +172,29 @@ int main(int argc, char* argv[]) {
       // Do a few initial assignments to let any cache effects stabilize
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
-        BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); }
 
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
-        BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); }
       basicTime = tim.elapsed();
       cout << format("  Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl;
 
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
-        BOOST_FOREACH(const ij_t& ij, ijs) { full(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); }
       fullTime = tim.elapsed();
       cout << format("  Full:  %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl;
 
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj()));
       for(size_t rep=0; rep<1000; ++rep)
-        BOOST_FOREACH(const ij_t& ij, ijs) { top(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); }
       topTime = tim.elapsed();
       cout << format("  Top:   %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl;
 
       for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols()));
       for(size_t rep=0; rep<1000; ++rep)
-        BOOST_FOREACH(const ij_t& ij, ijs) { block(ij.first, ij.second) = rng(); }
+        for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); }
       blockTime = tim.elapsed();
       cout << format("  Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl;
 
diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp
index 6308a1d61..c1e695b3a 100644
--- a/timing/timeSFMBAL.cpp
+++ b/timing/timeSFMBAL.cpp
@@ -23,7 +23,6 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point3.h>
 
-#include <boost/foreach.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -38,7 +37,7 @@ int main(int argc, char* argv[]) {
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
+    for (const SfM_Measurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j)));
@@ -47,9 +46,9 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  BOOST_FOREACH (const SfM_Camera& camera, db.cameras)
+  for (const SfM_Camera& camera: db.cameras)
     initial.insert(C(i++), camera);
-  BOOST_FOREACH (const SfM_Track& track, db.tracks)
+  for (const SfM_Track& track: db.tracks)
     initial.insert(P(j++), track.p);
 
   return optimize(db, graph, initial);
diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp
index 4545abc21..867953257 100644
--- a/timing/timeSFMBALautodiff.cpp
+++ b/timing/timeSFMBALautodiff.cpp
@@ -22,7 +22,6 @@
 #include <gtsam/nonlinear/AdaptAutoDiff.h>
 #include <gtsam/3rdparty/ceres/example.h>
 
-#include <boost/foreach.hpp>
 #include <stddef.h>
 #include <stdexcept>
 #include <string>
@@ -46,7 +45,7 @@ int main(int argc, char* argv[]) {
   // Build graph
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
+    for (const SfM_Measurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Expression<Vector9> camera_(C(i));
@@ -59,14 +58,14 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
+  for (const SfM_Camera& camera: db.cameras) {
     // readBAL converts to GTSAM format, so we need to convert back !
     Pose3 openGLpose = gtsam2openGL(camera.pose());
     Vector9 v9;
     v9 << Pose3::Logmap(openGLpose), camera.calibration().vector();
     initial.insert(C(i++), v9);
   }
-  BOOST_FOREACH (const SfM_Track& track, db.tracks) {
+  for (const SfM_Track& track: db.tracks) {
     Vector3 v3 = track.p.vector();
     initial.insert(P(j++), v3);
   }
diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp
index 32fae4ac2..33680e813 100644
--- a/timing/timeSFMBALcamTnav.cpp
+++ b/timing/timeSFMBALcamTnav.cpp
@@ -23,7 +23,6 @@
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point3.h>
 
-#include <boost/foreach.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -35,7 +34,7 @@ int main(int argc, char* argv[]) {
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
-    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
+    for (const SfM_Measurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Pose3_ camTnav_(C(i));
@@ -50,12 +49,12 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
+  for (const SfM_Camera& camera: db.cameras) {
     initial.insert(C(i), camera.pose().inverse());  // inverse !!!
     initial.insert(K(i), camera.calibration());
     i += 1;
   }
-  BOOST_FOREACH (const SfM_Track& track, db.tracks)
+  for (const SfM_Track& track: db.tracks)
     initial.insert(P(j++), track.p);
 
   bool separateCalibration = true;
diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp
index 0d0eb4f65..dad1edd4e 100644
--- a/timing/timeSFMBALnavTcam.cpp
+++ b/timing/timeSFMBALnavTcam.cpp
@@ -23,7 +23,6 @@
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point3.h>
 
-#include <boost/foreach.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -36,7 +35,7 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
     Point3_ nav_point_(P(j));
-    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
+    for (const SfM_Measurement& m: db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Pose3_ navTcam_(C(i));
@@ -50,12 +49,12 @@ int main(int argc, char* argv[]) {
 
   Values initial;
   size_t i = 0, j = 0;
-  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
+  for (const SfM_Camera& camera: db.cameras) {
     initial.insert(C(i), camera.pose());
     initial.insert(K(i), camera.calibration());
     i += 1;
   }
-  BOOST_FOREACH (const SfM_Track& track, db.tracks)
+  for (const SfM_Track& track: db.tracks)
     initial.insert(P(j++), track.p);
 
   bool separateCalibration = true;
diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp
index 9be55f831..e64c34340 100644
--- a/timing/timeSchurFactors.cpp
+++ b/timing/timeSchurFactors.cpp
@@ -150,7 +150,7 @@ int main(void) {
     ms += m;
   //for (size_t m=10;m<=100;m+=10) ms += m;
   // loop over number of images
-  BOOST_FOREACH(size_t m,ms)
+  for(size_t m: ms)
     timeAll<PinholePose<Cal3Bundler> >(m, NUM_ITERATIONS);
 }
 
diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp
index c4196f414..7a4413ac7 100644
--- a/timing/timeiSAM2Chain.cpp
+++ b/timing/timeiSAM2Chain.cpp
@@ -93,7 +93,7 @@ int main(int argc, char *argv[]) {
 
   // Write times
   ofstream timesFile("times.txt");
-  BOOST_FOREACH(double t, times) {
+  for(double t: times) {
     timesFile << t << "\n"; }
 
   return 0;
diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
index 848998eb0..cde04afe0 100644
--- a/wrap/Argument.cpp
+++ b/wrap/Argument.cpp
@@ -18,7 +18,6 @@
 
 #include "Argument.h"
 
-#include <boost/foreach.hpp>
 #include <boost/regex.hpp>
 #include <boost/lexical_cast.hpp>
 
@@ -40,7 +39,7 @@ Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
 ArgumentList ArgumentList::expandTemplate(
     const TemplateSubstitution& ts) const {
   ArgumentList instArgList;
-  BOOST_FOREACH(const Argument& arg, *this) {
+  for(const Argument& arg: *this) {
     Argument instArg = arg.expandTemplate(ts);
     instArgList.push_back(instArg);
   }
@@ -50,7 +49,7 @@ ArgumentList ArgumentList::expandTemplate(
 /* ************************************************************************* */
 string Argument::matlabClass(const string& delim) const {
   string result;
-  BOOST_FOREACH(const string& ns, type.namespaces())
+  for(const string& ns: type.namespaces())
     result += ns + delim;
   if (type.name() == "string" || type.name() == "unsigned char"
       || type.name() == "char")
@@ -110,7 +109,7 @@ void Argument::proxy_check(FileWriter& proxyFile, const string& s) const {
 string ArgumentList::types() const {
   string str;
   bool first = true;
-  BOOST_FOREACH(Argument arg, *this) {
+  for(Argument arg: *this) {
     if (!first)
       str += ",";
     str += arg.type.name();
@@ -124,8 +123,8 @@ string ArgumentList::signature() const {
   string sig;
   bool cap = false;
 
-  BOOST_FOREACH(Argument arg, *this) {
-    BOOST_FOREACH(char ch, arg.type.name())
+  for(Argument arg: *this) {
+    for(char ch: arg.type.name())
       if (isupper(ch)) {
         sig += ch;
         //If there is a capital letter, we don't want to read it below
@@ -144,7 +143,7 @@ string ArgumentList::signature() const {
 string ArgumentList::names() const {
   string str;
   bool first = true;
-  BOOST_FOREACH(Argument arg, *this) {
+  for(Argument arg: *this) {
     if (!first)
       str += ",";
     str += arg.name;
@@ -155,7 +154,7 @@ string ArgumentList::names() const {
 
 /* ************************************************************************* */
 bool ArgumentList::allScalar() const {
-  BOOST_FOREACH(Argument arg, *this)
+  for(Argument arg: *this)
     if (!arg.isScalar())
       return false;
   return true;
@@ -164,7 +163,7 @@ bool ArgumentList::allScalar() const {
 /* ************************************************************************* */
 void ArgumentList::matlab_unwrap(FileWriter& file, int start) const {
   int index = start;
-  BOOST_FOREACH(Argument arg, *this) {
+  for(Argument arg: *this) {
     stringstream buf;
     buf << "in[" << index << "]";
     arg.matlab_unwrap(file, buf.str());
@@ -176,7 +175,7 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const {
 void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
   file.oss << name << "(";
   bool first = true;
-  BOOST_FOREACH(Argument arg, *this) {
+  for(Argument arg: *this) {
     if (!first)
       file.oss << ", ";
     file.oss << arg.type.name() << " " << arg.name;
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index d7a3b6ee4..3a12290eb 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -20,7 +20,6 @@
 #include "utilities.h"
 #include "Argument.h"
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <boost/range/algorithm/copy.hpp>
@@ -157,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
       << "    function disp(obj), obj.display; end\n    %DISP Calls print on the object\n";
 
   // Methods 
-  BOOST_FOREACH(const Methods::value_type& name_m, methods_) {
+  for(const Methods::value_type& name_m: methods_) {
     const Method& m = name_m.second;
     m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
         matlabUniqueName, wrapperName, typeAttributes, functionNames);
@@ -172,7 +171,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
   proxyFile.oss << "  methods(Static = true)\n";
 
   // Static methods 
-  BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
+  for(const StaticMethods::value_type& name_m: static_methods) {
     const StaticMethod& m = name_m.second;
     m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
         matlabUniqueName, wrapperName, typeAttributes, functionNames);
@@ -301,7 +300,7 @@ Class Class::expandTemplate(const TemplateSubstitution& ts) const {
 vector<Class> Class::expandTemplate(Str templateArg,
     const vector<Qualified>& instantiations) const {
   vector<Class> result;
-  BOOST_FOREACH(const Qualified& instName, instantiations) {
+  for(const Qualified& instName: instantiations) {
     Qualified expandedClass = (Qualified) (*this);
     expandedClass.expand(instName.name());
     const TemplateSubstitution ts(templateArg, instName, expandedClass);
@@ -319,7 +318,7 @@ vector<Class> Class::expandTemplate(Str templateArg,
 vector<Class> Class::expandTemplate(Str templateArg,
     const vector<int>& integers) const {
   vector<Class> result;
-  BOOST_FOREACH(int i, integers) {
+  for(int i: integers) {
     Qualified expandedClass = (Qualified) (*this);
     stringstream ss; ss << i;
     string instName = ss.str();
@@ -342,7 +341,7 @@ void Class::addMethod(bool verbose, bool is_const, Str methodName,
   if (tmplate.valid()) {
     // Create method to expand
     // For all values of the template argument, create a new method
-    BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) {
+    for(const Qualified& instName: tmplate.argValues()) {
       const TemplateSubstitution ts(tmplate.argName(), instName, *this);
       // substitute template in arguments
       ArgumentList expandedArgs = argumentList.expandTemplate(ts);
@@ -413,7 +412,7 @@ void Class::appendInheritedMethods(const Class& cls,
   if (cls.parentClass) {
 
     // Find parent
-    BOOST_FOREACH(const Class& parent, classes) {
+    for(const Class& parent: classes) {
       // We found a parent class for our parent, TODO improve !
       if (parent.name() == cls.parentClass->name()) {
         methods_.insert(parent.methods_.begin(), parent.methods_.end());
@@ -426,7 +425,7 @@ void Class::appendInheritedMethods(const Class& cls,
 /* ************************************************************************* */
 string Class::getTypedef() const {
   string result;
-  BOOST_FOREACH(Str namesp, namespaces()) {
+  for(Str namesp: namespaces()) {
     result += ("namespace " + namesp + " { ");
   }
   result += ("typedef " + typedefName + " " + name() + ";");
@@ -446,12 +445,12 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
 
   if (!methods_.empty())
     proxyFile.oss << "%\n%-------Methods-------\n";
-  BOOST_FOREACH(const Methods::value_type& name_m, methods_)
+  for(const Methods::value_type& name_m: methods_)
     name_m.second.comment_fragment(proxyFile);
 
   if (!static_methods.empty())
     proxyFile.oss << "%\n%-------Static Methods-------\n";
-  BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods)
+  for(const StaticMethods::value_type& name_m: static_methods)
     name_m.second.comment_fragment(proxyFile);
 
   if (hasSerialization) {
@@ -653,9 +652,9 @@ string Class::getSerializationExport() const {
 void Class::python_wrapper(FileWriter& wrapperFile) const {
   wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n";
   constructor.python_wrapper(wrapperFile, name());
-  BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values)
+  for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
     m.python_wrapper(wrapperFile, name());
-  BOOST_FOREACH(const Method& m, methods_ | boost::adaptors::map_values)
+  for(const Method& m: methods_ | boost::adaptors::map_values)
     m.python_wrapper(wrapperFile, name());
   wrapperFile.oss << ";\n\n";
 }
diff --git a/wrap/Class.h b/wrap/Class.h
index 2f7457f06..e69c38f41 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -39,7 +39,6 @@
 
 namespace bl = boost::lambda;
 
-#include <boost/foreach.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <boost/optional.hpp>
 
@@ -145,9 +144,9 @@ public:
   friend std::ostream& operator<<(std::ostream& os, const Class& cls) {
     os << "class " << cls.name() << "{\n";
     os << cls.constructor << ";\n";
-    BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values)
+    for(const StaticMethod& m: cls.static_methods | boost::adaptors::map_values)
       os << m << ";\n";
-    BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values)
+    for(const Method& m: cls.methods_ | boost::adaptors::map_values)
       os << m << ";\n";
     os << "};" << std::endl;
     return os;
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
index 35cc0fa53..77d831e0a 100644
--- a/wrap/Constructor.cpp
+++ b/wrap/Constructor.cpp
@@ -20,7 +20,6 @@
 #include <fstream>
 #include <algorithm>
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 
 #include "utilities.h"
diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp
index 9fe9aecc2..9e6495602 100644
--- a/wrap/Deconstructor.cpp
+++ b/wrap/Deconstructor.cpp
@@ -19,7 +19,6 @@
 #include <iostream>
 #include <fstream>
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 
 #include "utilities.h"
diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h
index 30e27764b..80d974d88 100644
--- a/wrap/FullyOverloadedFunction.h
+++ b/wrap/FullyOverloadedFunction.h
@@ -44,7 +44,7 @@ public:
 
   void verifyReturnTypes(const std::vector<std::string>& validtypes,
       const std::string& s) const {
-    BOOST_FOREACH(const ReturnValue& retval, returnVals_) {
+    for(const ReturnValue& retval: returnVals_) {
       retval.type1.verify(validtypes, s);
       if (retval.isPair)
         retval.type2.verify(validtypes, s);
@@ -55,7 +55,7 @@ public:
   std::vector<ReturnValue> expandReturnValuesTemplate(
       const TemplateSubstitution& ts) const {
     std::vector<ReturnValue> result;
-    BOOST_FOREACH(const ReturnValue& retVal, returnVals_) {
+    for(const ReturnValue& retVal: returnVals_) {
       ReturnValue instRetVal = retVal.expandTemplate(ts);
       result.push_back(instRetVal);
     }
@@ -73,7 +73,7 @@ public:
   // emit a list of comments, one for each overload
   void usage_fragment(FileWriter& proxyFile, const std::string& name) const {
     unsigned int argLCount = 0;
-    BOOST_FOREACH(ArgumentList argList, argLists_) {
+    for(ArgumentList argList: argLists_) {
       argList.emit_prototype(proxyFile, name);
       if (argLCount != nrOverloads() - 1)
         proxyFile.oss << ", ";
@@ -87,7 +87,7 @@ public:
   // emit a list of comments, one for each overload
   void comment_fragment(FileWriter& proxyFile, const std::string& name) const {
     size_t i = 0;
-    BOOST_FOREACH(ArgumentList argList, argLists_) {
+    for(ArgumentList argList: argLists_) {
       proxyFile.oss << "%";
       argList.emit_prototype(proxyFile, name);
       proxyFile.oss << " : returns " << returnVals_[i++].return_type(false)
@@ -125,7 +125,7 @@ template<class F>
 inline void verifyReturnTypes(const std::vector<std::string>& validTypes,
     const std::map<std::string, F>& vt) {
   typedef typename std::map<std::string, F>::value_type NamedMethod;
-  BOOST_FOREACH(const NamedMethod& namedMethod, vt)
+  for(const NamedMethod& namedMethod: vt)
     namedMethod.second.verifyReturnTypes(validTypes);
 }
 
diff --git a/wrap/Function.cpp b/wrap/Function.cpp
index 7e4ded040..8da667fae 100644
--- a/wrap/Function.cpp
+++ b/wrap/Function.cpp
@@ -18,7 +18,6 @@
 #include "Function.h"
 #include "utilities.h"
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 #include <boost/algorithm/string.hpp>
 
diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp
index 013ef6d28..3f667e2c9 100644
--- a/wrap/GlobalFunction.cpp
+++ b/wrap/GlobalFunction.cpp
@@ -8,7 +8,6 @@
 #include "GlobalFunction.h"
 #include "utilities.h"
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 
 namespace wrap {
@@ -44,7 +43,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath,
   }
 
   size_t lastcheck = grouped_functions.size();
-  BOOST_FOREACH(const GlobalFunctionMap::value_type& p, grouped_functions) {
+  for(const GlobalFunctionMap::value_type& p: grouped_functions) {
     p.second.generateSingleFunction(toolboxPath, wrapperName, typeAttributes,
         file, functionNames);
     if (--lastcheck != 0)
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index 2ad6e8259..f8c03b0c6 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -18,7 +18,6 @@
 #include "Method.h"
 #include "utilities.h"
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 #include <boost/algorithm/string.hpp>
 
diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp
index 9b5f7135f..84e6c67d9 100644
--- a/wrap/MethodBase.cpp
+++ b/wrap/MethodBase.cpp
@@ -19,7 +19,6 @@
 #include "Method.h"
 #include "utilities.h"
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 #include <boost/algorithm/string.hpp>
 
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 9d505a525..a115c5e10 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -22,7 +22,6 @@
 #include "TypeAttributesTable.h" 
 #include "utilities.h"
 
-#include <boost/foreach.hpp> 
 #include <boost/filesystem.hpp> 
 #include <boost/lexical_cast.hpp> 
  
@@ -58,7 +57,7 @@ static void handle_possible_template(vector<Class>& classes, const Class& cls,
     vector<Class> classInstantiations =
         (t.nrValues() > 0) ? cls.expandTemplate(arg, t.argValues()) :
             cls.expandTemplate(arg, t.intList());
-    BOOST_FOREACH(const Class& c, classInstantiations)
+    for(const Class& c: classInstantiations)
       classes.push_back(c);
   }
 }
@@ -177,11 +176,11 @@ void Module::parseMarkup(const std::string& data) {
   }
 
   // Post-process classes for serialization markers
-  BOOST_FOREACH(Class& cls, classes)
+  for(Class& cls: classes)
     cls.erase_serialization();
 
   // Explicitly add methods to the classes from parents so it shows in documentation
-  BOOST_FOREACH(Class& cls, classes)
+  for(Class& cls: classes)
     cls.appendInheritedMethods(cls, classes);
 
   // Expand templates - This is done first so that template instantiations are
@@ -199,7 +198,7 @@ void Module::parseMarkup(const std::string& data) {
   verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
 
   hasSerialiable = false;
-  BOOST_FOREACH(const Class& cls, expandedClasses)
+  for(const Class& cls: expandedClasses)
     cls.verifyAll(validTypes,hasSerialiable);
 
   // Create type attributes table and check validity
@@ -224,7 +223,6 @@ void Module::matlab_code(const string& toolboxPath) const {
   FileWriter wrapperFile(wrapperFileName, verbose, "//");
   wrapperFile.oss << "#include <wrap/matlab.h>\n";
   wrapperFile.oss << "#include <map>\n";
-  wrapperFile.oss << "#include <boost/foreach.hpp>\n";
   wrapperFile.oss << "\n";
 
   // Include boost.serialization archive headers before other class headers
@@ -239,14 +237,14 @@ void Module::matlab_code(const string& toolboxPath) const {
 
   // create typedef classes - we put this at the top of the wrap file so that
   // collectors and method arguments can use these typedefs
-  BOOST_FOREACH(const Class& cls, expandedClasses)
+  for(const Class& cls: expandedClasses)
     if(!cls.typedefName.empty())
       wrapperFile.oss << cls.getTypedef() << "\n";
   wrapperFile.oss << "\n";
 
   // Generate boost.serialization export flags (needs typedefs from above)
   if (hasSerialiable) {
-    BOOST_FOREACH(const Class& cls, expandedClasses)
+    for(const Class& cls: expandedClasses)
       if(cls.isSerializable)
         wrapperFile.oss << cls.getSerializationExport() << "\n";
     wrapperFile.oss << "\n";
@@ -261,11 +259,11 @@ void Module::matlab_code(const string& toolboxPath) const {
   vector<string> functionNames; // Function names stored by index for switch
 
   // create proxy class and wrapper code
-  BOOST_FOREACH(const Class& cls, expandedClasses)
+  for(const Class& cls: expandedClasses)
     cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
 
   // create matlab files and wrapper code for global functions
-  BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions)
+  for(const GlobalFunctions::value_type& p: global_functions)
     p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
 
   // finish wrapper file
@@ -321,7 +319,7 @@ vector<Class> Module::ExpandTypedefInstantiations(const vector<Class>& classes,
  
   vector<Class> expandedClasses = classes; 
  
-  BOOST_FOREACH(const TemplateInstantiationTypedef& inst, instantiations) { 
+  for(const TemplateInstantiationTypedef& inst: instantiations) {
     // Add the new class to the list 
     expandedClasses.push_back(inst.findAndExpand(classes)); 
   } 
@@ -339,7 +337,7 @@ vector<Class> Module::ExpandTypedefInstantiations(const vector<Class>& classes,
 /* ************************************************************************* */ 
 vector<string> Module::GenerateValidTypes(const vector<Class>& classes, const vector<ForwardDeclaration> forwardDeclarations) { 
   vector<string> validTypes; 
-  BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDeclarations) { 
+  for(const ForwardDeclaration& fwDec: forwardDeclarations) {
     validTypes.push_back(fwDec.name);
   } 
   validTypes.push_back("void"); 
@@ -353,7 +351,7 @@ vector<string> Module::GenerateValidTypes(const vector<Class>& classes, const ve
   validTypes.push_back("Vector"); 
   validTypes.push_back("Matrix"); 
   //Create a list of parsed classes for dependency checking 
-  BOOST_FOREACH(const Class& cls, classes) { 
+  for(const Class& cls: classes) {
     validTypes.push_back(cls.qualifiedName("::")); 
   } 
  
@@ -363,7 +361,7 @@ vector<string> Module::GenerateValidTypes(const vector<Class>& classes, const ve
 /* ************************************************************************* */ 
 void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes) { 
   // Generate all collectors 
-  BOOST_FOREACH(const Class& cls, classes) { 
+  for(const Class& cls: classes) {
     const string matlabUniqueName = cls.qualifiedName(), 
       cppName = cls.qualifiedName("::"); 
     wrapperFile.oss << "typedef std::set<boost::shared_ptr<" << cppName << ">*> " 
@@ -379,7 +377,7 @@ void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::st
     "  mstream mout;\n" // Send stdout to MATLAB console 
     "  std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n" 
     "  bool anyDeleted = false;\n"; 
-  BOOST_FOREACH(const Class& cls, classes) { 
+  for(const Class& cls: classes) {
     const string matlabUniqueName = cls.qualifiedName(); 
     const string cppName = cls.qualifiedName("::"); 
     const string collectorType = "Collector_" + matlabUniqueName; 
@@ -411,7 +409,7 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul
     "  const mxArray *alreadyCreated = mexGetVariablePtr(\"global\", \"gtsam_" + moduleName + "_rttiRegistry_created\");\n" 
     "  if(!alreadyCreated) {\n" 
     "    std::map<std::string, std::string> types;\n"; 
-  BOOST_FOREACH(const Class& cls, classes) { 
+  for(const Class& cls: classes) {
     if(cls.isVirtual) 
       wrapperFile.oss << 
       "    types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName(".") << "\"));\n"; 
@@ -423,7 +421,7 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul
     "    if(!registry)\n" 
     "      registry = mxCreateStructMatrix(1, 1, 0, NULL);\n" 
     "    typedef std::pair<std::string, std::string> StringPair;\n" 
-    "    BOOST_FOREACH(const StringPair& rtti_matlab, types) {\n" 
+    "    for(const StringPair& rtti_matlab: types) {\n"
     "      int fieldId = mxAddField(registry, rtti_matlab.first.c_str());\n" 
     "      if(fieldId < 0)\n" 
     "        mexErrMsgTxt(\"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly\");\n" 
@@ -458,11 +456,11 @@ void Module::python_wrapper(const string& toolboxPath) const {
   wrapperFile.oss << "{\n";
 
   // write out classes
-  BOOST_FOREACH(const Class& cls, expandedClasses)
+  for(const Class& cls: expandedClasses)
     cls.python_wrapper(wrapperFile);
 
   // write out global functions
-  BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions)
+  for(const GlobalFunctions::value_type& p: global_functions)
     p.second.python_wrapper(wrapperFile);
 
   // finish wrapper file
diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h
index 7718bc139..55e581ad8 100644
--- a/wrap/OverloadedFunction.h
+++ b/wrap/OverloadedFunction.h
@@ -49,7 +49,7 @@ public:
   std::vector<ArgumentList> expandArgumentListsTemplate(
       const TemplateSubstitution& ts) const {
     std::vector<ArgumentList> result;
-    BOOST_FOREACH(const ArgumentList& argList, argLists_) {
+    for(const ArgumentList& argList: argLists_) {
       ArgumentList instArgList = argList.expandTemplate(ts);
       result.push_back(instArgList);
     }
@@ -63,8 +63,8 @@ public:
 
   void verifyArguments(const std::vector<std::string>& validArgs,
       const std::string s) const {
-    BOOST_FOREACH(const ArgumentList& argList, argLists_) {
-      BOOST_FOREACH(Argument arg, argList) {
+    for(const ArgumentList& argList: argLists_) {
+      for(Argument arg: argList) {
         std::string fullType = arg.type.qualifiedName("::");
         if (find(validArgs.begin(), validArgs.end(), fullType)
             == validArgs.end())
@@ -75,7 +75,7 @@ public:
 
   friend std::ostream& operator<<(std::ostream& os,
       const ArgumentOverloads& overloads) {
-    BOOST_FOREACH(const ArgumentList& argList, overloads.argLists_)
+    for(const ArgumentList& argList: overloads.argLists_)
       os << argList << std::endl;
     return os;
   }
@@ -106,7 +106,7 @@ static std::map<std::string, F> expandMethodTemplate(
     const std::map<std::string, F>& methods, const TemplateSubstitution& ts) {
   std::map<std::string, F> result;
   typedef std::pair<const std::string, F> NamedMethod;
-  BOOST_FOREACH(NamedMethod namedMethod, methods) {
+  for(NamedMethod namedMethod: methods) {
     F instMethod = namedMethod.second;
     instMethod.expandTemplate(ts);
     namedMethod.second = instMethod;
@@ -119,7 +119,7 @@ template<class F>
 inline void verifyArguments(const std::vector<std::string>& validArgs,
     const std::map<std::string, F>& vt) {
   typedef typename std::map<std::string, F>::value_type NamedMethod;
-  BOOST_FOREACH(const NamedMethod& namedMethod, vt)
+  for(const NamedMethod& namedMethod: vt)
     namedMethod.second.verifyArguments(validArgs);
 }
 
diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp
index 41fd51680..1a1f1bc26 100644
--- a/wrap/ReturnType.cpp
+++ b/wrap/ReturnType.cpp
@@ -6,7 +6,6 @@
 
 #include "ReturnType.h"
 #include "utilities.h"
-#include <boost/foreach.hpp>
 #include <iostream>
 
 using namespace std;
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 596acb2c3..1405e8e2b 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -8,7 +8,6 @@
 
 #include "ReturnValue.h"
 #include "utilities.h"
-#include <boost/foreach.hpp>
 #include <iostream>
 
 using namespace std;
diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp
index e4e7c89ae..23dc93d72 100644
--- a/wrap/StaticMethod.cpp
+++ b/wrap/StaticMethod.cpp
@@ -19,7 +19,6 @@
 #include "StaticMethod.h"
 #include "utilities.h"
 
-#include <boost/foreach.hpp>
 #include <boost/lexical_cast.hpp>
 #include <boost/algorithm/string.hpp>
 
diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp
index e925fd381..4c77d4e76 100644
--- a/wrap/TemplateInstantiationTypedef.cpp
+++ b/wrap/TemplateInstantiationTypedef.cpp
@@ -20,7 +20,6 @@
 
 #include "utilities.h"
 #include <iostream>
-#include <boost/foreach.hpp>
 #include <boost/optional.hpp>
 
 using namespace std;
@@ -31,7 +30,7 @@ Class TemplateInstantiationTypedef::findAndExpand(
     const vector<Class>& classes) const {
   // Find matching class
   boost::optional<Class const &> matchedClass;
-  BOOST_FOREACH(const Class& cls, classes) {
+  for(const Class& cls: classes) {
     if (cls.name() == class_.name() && cls.namespaces() == class_.namespaces()
         && cls.templateArgs.size() == typeList.size()) {
       matchedClass.reset(cls);
diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp
index 27de70d0e..f98e9b760 100644
--- a/wrap/TypeAttributesTable.cpp
+++ b/wrap/TypeAttributesTable.cpp
@@ -20,7 +20,6 @@
 #include "Class.h"
 #include "utilities.h"
 
-#include <boost/foreach.hpp>
 #include <boost/range/adaptor/map.hpp>
 #include <boost/range/algorithm/copy.hpp>
 
@@ -45,7 +44,7 @@ const TypeAttributes& TypeAttributesTable::attributes(const string& key) const {
 
 /* ************************************************************************* */
 void TypeAttributesTable::addClasses(const vector<Class>& classes) {
-  BOOST_FOREACH(const Class& cls, classes) {
+  for(const Class& cls: classes) {
     if (!table_.insert(
         make_pair(cls.qualifiedName("::"), TypeAttributes(cls.isVirtual))).second)
       throw DuplicateDefinition("class " + cls.qualifiedName("::"));
@@ -55,7 +54,7 @@ void TypeAttributesTable::addClasses(const vector<Class>& classes) {
 /* ************************************************************************* */
 void TypeAttributesTable::addForwardDeclarations(
     const vector<ForwardDeclaration>& forwardDecls) {
-  BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDecls) {
+  for(const ForwardDeclaration& fwDec: forwardDecls) {
     if (!table_.insert(make_pair(fwDec.name, TypeAttributes(fwDec.isVirtual))).second)
       throw DuplicateDefinition("class " + fwDec.name);
   }
@@ -63,7 +62,7 @@ void TypeAttributesTable::addForwardDeclarations(
 
 /* ************************************************************************* */
 void TypeAttributesTable::checkValidity(const vector<Class>& classes) const {
-  BOOST_FOREACH(const Class& cls, classes) {
+  for(const Class& cls: classes) {
 
     boost::optional<string> parent = cls.qualifiedParent();
     if (parent) {
diff --git a/wrap/matlab.h b/wrap/matlab.h
index 1639876cc..162aaa0e2 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -34,7 +34,6 @@ extern "C" {
 
 #include <boost/shared_ptr.hpp>
 #include <boost/make_shared.hpp>
-#include <boost/foreach.hpp>
 
 #include <list>
 #include <string>
diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp
index 226030a0d..ef9051d14 100644
--- a/wrap/tests/expected/geometry_wrapper.cpp
+++ b/wrap/tests/expected/geometry_wrapper.cpp
@@ -1,6 +1,5 @@
 #include <wrap/matlab.h>
 #include <map>
-#include <boost/foreach.hpp>
 
 #include <boost/serialization/export.hpp>
 #include <boost/archive/text_iarchive.hpp>
@@ -116,7 +115,7 @@ void _geometry_RTTIRegister() {
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
-    BOOST_FOREACH(const StringPair& rtti_matlab, types) {
+    for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
       if(fieldId < 0)
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp
index 4b41f3958..8526900a7 100644
--- a/wrap/tests/expected2/geometry_wrapper.cpp
+++ b/wrap/tests/expected2/geometry_wrapper.cpp
@@ -1,6 +1,5 @@
 #include <wrap/matlab.h>
 #include <map>
-#include <boost/foreach.hpp>
 
 #include <folder/path/to/Test.h>
 
@@ -91,7 +90,7 @@ void _geometry_RTTIRegister() {
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
-    BOOST_FOREACH(const StringPair& rtti_matlab, types) {
+    for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
       if(fieldId < 0)
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
index 6d22e1625..0f5c70348 100644
--- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
+++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
@@ -1,6 +1,5 @@
 #include <wrap/matlab.h>
 #include <map>
-#include <boost/foreach.hpp>
 
 #include <path/to/ns1.h>
 #include <path/to/ns1/ClassB.h>
@@ -81,7 +80,7 @@ void _testNamespaces_RTTIRegister() {
     if(!registry)
       registry = mxCreateStructMatrix(1, 1, 0, NULL);
     typedef std::pair<std::string, std::string> StringPair;
-    BOOST_FOREACH(const StringPair& rtti_matlab, types) {
+    for(const StringPair& rtti_matlab: types) {
       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
       if(fieldId < 0)
         mexErrMsgTxt("gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp
index 57a5bce29..32ee85eee 100644
--- a/wrap/utilities.cpp
+++ b/wrap/utilities.cpp
@@ -19,7 +19,6 @@
 #include <iostream>
 #include <cstdlib>
 
-#include <boost/foreach.hpp>
 #include <boost/filesystem.hpp>
 
 #include "utilities.h"
@@ -71,9 +70,9 @@ bool assert_equal(const vector<string>& expected, const vector<string>& actual)
   }
   if(!match) {
     cout << "expected: " << endl;
-    BOOST_FOREACH(const vector<string>::value_type& a, expected) { cout << "["  << a << "] "; }
+    for(const vector<string>::value_type& a: expected) { cout << "["  << a << "] "; }
     cout << "\nactual: " << endl;
-    BOOST_FOREACH(const vector<string>::value_type& a, actual) { cout << "["  << a << "] "; }
+    for(const vector<string>::value_type& a: actual) { cout << "["  << a << "] "; }
     cout << endl;
     return false;
   }
@@ -132,7 +131,7 @@ void createNamespaceStructure(const std::vector<std::string>& namespaces,
     const std::string& toolboxPath) {
   using namespace boost::filesystem;
   path curPath = toolboxPath;
-  BOOST_FOREACH(const string& subdir, namespaces) {
+  for(const string& subdir: namespaces) {
 //    curPath /= "+" + subdir; // original - resulted in valgrind error
     curPath = curPath / string(string("+") + subdir);
     if(!is_directory(curPath)) {
diff --git a/wrap/utilities.h b/wrap/utilities.h
index fbc8dc7a2..d8108d6a5 100644
--- a/wrap/utilities.h
+++ b/wrap/utilities.h
@@ -21,7 +21,6 @@
 #include "FileWriter.h"
 
 #include <boost/format.hpp>
-#include <boost/foreach.hpp>
 
 #include <string>
 #include <vector>