diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index b45cb2591..d28b21428 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -23,10 +23,9 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LinearContainerFactor.h>
-#include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Symbol.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/inference/Key.h>
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -96,19 +95,19 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
     ordering.push_back(key);
   }
 
-  GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint, ordering);
 
-  // Create the set of marginalizable variables
-  std::vector<Index> linearIndices;
-  BOOST_FOREACH(Key key, keysToMarginalize) {
-    linearIndices.push_back(ordering.at(key));
+  GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
+
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
+
+  NonlinearFactorGraph LinearContainerForGaussianMarginals;
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+    LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, linPoint));
   }
 
-  std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> marginal = linearGraph.eliminate(linearIndices, EliminateCholesky);
-
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
   BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal.second) {
-    LinearContainerForGaussianMarginals.add(LinearContainerFactor(factor, ordering, linPoint));
+    LinearContainerForGaussianMarginals.push_back(LinearContainerFactor(factor, ordering, linPoint));
   }
   return LinearContainerForGaussianMarginals;
 }
@@ -158,8 +157,8 @@ TEST( ConcurrentIncrementalFilter, getFactors )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors1;
-  newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
   newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
@@ -175,8 +174,8 @@ TEST( ConcurrentIncrementalFilter, getFactors )
 
   // Add some more factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
   newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
@@ -208,8 +207,8 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors1;
-  newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
   newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
@@ -225,8 +224,8 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
 
   // Add some more factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
   newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
@@ -242,12 +241,6 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
   CHECK(assert_equal(expected3, actual3));
 }
 
-/* ************************************************************************* */
-TEST( ConcurrentIncrementalFilter, getOrdering )
-{
-  // TODO: Think about how to check ordering...
-}
-
 /* ************************************************************************* */
 TEST( ConcurrentIncrementalFilter, getDelta )
 {
@@ -270,8 +263,8 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
   newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -290,8 +283,8 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate )
 
   // Add some more factors to the filter
   NonlinearFactorGraph newFactors3;
-  newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues3;
   newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@@ -354,8 +347,8 @@ TEST( ConcurrentIncrementalFilter, update_multiple )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
   newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -374,8 +367,8 @@ TEST( ConcurrentIncrementalFilter, update_multiple )
 
   // Add some more factors to the filter
   NonlinearFactorGraph newFactors3;
-  newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues3;
   newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@@ -404,10 +397,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
-  newFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues;
   newValues.insert(1, Pose3().compose(poseError));
   newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -434,9 +427,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
 
   // ----------------------------------------------------------------------------------------------
   NonlinearFactorGraph partialGraph;
-  partialGraph.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  partialGraph.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
-  partialGraph.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
 
   Values partialValues;
   // ISAM2 after the update is computing the delta, without applying them to the current lin point
@@ -444,19 +437,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
   partialValues.insert(2, newValues.at(2));
   partialValues.insert(3, newValues.at(3));
 
-  // Create an ordering
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
-  ordering.push_back(3);
 
-  // Create the set of marginalizable variables
-  std::vector<Index> linearIndices;
-  linearIndices.push_back(ordering.at(1));
-  linearIndices.push_back(ordering.at(2));
+  GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
 
-  GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering);
-  std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky);
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph expectedGraph;
 
@@ -468,13 +452,13 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
   expectedGraph.push_back(NonlinearFactor::shared_ptr());
   // ==========================================================
-  expectedGraph.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
 
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
+  BOOST_FOREACH(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.add(LinearContainerFactor(factor, ordering, partialValues));
-    expectedGraph.add(LinearContainerFactor(factor, ordering));
+    //    expectedGraph.push_back(LinearContainerFactor(factor, ordering, partialValues));
+    expectedGraph.push_back(LinearContainerFactor(factor));
   }
 
   // ----------------------------------------------------------------------------------------------
@@ -503,10 +487,10 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
 
   // Add some factors to the filter
   NonlinearFactorGraph newFactors;
-  newFactors.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
-  newFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues;
   newValues.insert(1, Pose3().compose(poseError));
   newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -534,9 +518,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
 
   // ----------------------------------------------------------------------------------------------
   NonlinearFactorGraph partialGraph;
-  partialGraph.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  partialGraph.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
-  partialGraph.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
 
   Values partialValues;
   // ISAM2 after the update is computing the delta, without applying them to the current lin point
@@ -544,38 +528,30 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
   partialValues.insert(2, optimalValues.at(2));
   partialValues.insert(3, optimalValues.at(3));
 
-  // Create an ordering
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
-  ordering.push_back(3);
 
-  // Create the set of marginalizable variables
-  std::vector<Index> linearIndices;
-  linearIndices.push_back(ordering.at(1));
-  linearIndices.push_back(ordering.at(2));
 
-  GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues, ordering);
-  std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearPartialGraph.eliminate(linearIndices, EliminateCholesky);
+  GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
 
-  NonlinearFactorGraph expectedGraph;
+    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
 
-  // These three lines create three empty factors in expectedGraph:
-  // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
-  // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
-  // substituting them with a linear marginal
-  expectedGraph.push_back(NonlinearFactor::shared_ptr());
-  expectedGraph.push_back(NonlinearFactor::shared_ptr());
-  expectedGraph.push_back(NonlinearFactor::shared_ptr());
-  // ==========================================================
-  expectedGraph.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+    NonlinearFactorGraph expectedGraph;
 
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
-    // 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.add(LinearContainerFactor(factor, ordering, partialValues));
-    expectedGraph.add(LinearContainerFactor(factor, ordering));
-  }
+    // These three lines create three empty factors in expectedGraph:
+    // this is done since the equal function in NonlinearFactorGraph also cares about the ordering of the factors
+    // and in the actualGraph produced by the HMF we first insert 5 nonlinear factors, then we delete 3 of them, by
+    // substituting them with a linear marginal
+    expectedGraph.push_back(NonlinearFactor::shared_ptr());
+    expectedGraph.push_back(NonlinearFactor::shared_ptr());
+    expectedGraph.push_back(NonlinearFactor::shared_ptr());
+    // ==========================================================
+    expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+
+    BOOST_FOREACH(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));
+      expectedGraph.push_back(LinearContainerFactor(factor));
+    }
 
   // ----------------------------------------------------------------------------------------------
 
@@ -650,8 +626,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 )
   // Insert factors into the filter, but do not marginalize out any variables.
   // The synchronization should still be empty
   NonlinearFactorGraph newFactors;
-  newFactors.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues;
   newValues.insert(1, Pose3().compose(poseError));
   newValues.insert(2, newValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -813,7 +789,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 )
   expectedFilterSeparatorValues.insert(2, newValues.at(2));
   // ------------------------------------------------------------------------------
   NonlinearFactorGraph partialGraph;
-  partialGraph.add(factor3);
+  partialGraph.push_back(factor3);
 
   Values partialValues;
   partialValues.insert(2, newValues.at<Pose3>(2));
@@ -902,7 +878,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 )
   expectedFilterSeparatorValues.insert(2, newValues.at<Pose3>(2));
   // ------------------------------------------------------------------------------
   NonlinearFactorGraph partialGraphFilter;
-  partialGraphFilter.add(factor3);
+  partialGraphFilter.push_back(factor3);
 
   Values partialValuesFilter;
   partialValuesFilter.insert(2, newValues.at<Pose3>(2));
@@ -995,8 +971,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
   filter.postsync();
 
   NonlinearFactorGraph expectedSmootherFactors;
-  expectedSmootherFactors.add(factor1);
-  expectedSmootherFactors.add(factor2);
+  expectedSmootherFactors.push_back(factor1);
+  expectedSmootherFactors.push_back(factor2);
 
   Values optimalValues = BatchOptimize(newFactors, newValues, 1);
   Values expectedSmootherValues;
@@ -1026,11 +1002,11 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
   // currently the smoother contains factor 1 and 2 and node 1 and 2
 
   NonlinearFactorGraph partialGraph;
-  partialGraph.add(factor1);
-  partialGraph.add(factor2);
+  partialGraph.push_back(factor1);
+  partialGraph.push_back(factor2);
 
   // we also assume that the smoother received an extra factor (e.g., a prior on 1)
-  partialGraph.add(factor1);
+  partialGraph.push_back(factor1);
 
   Values partialValues;
   // Incrementaloptimization updates all linearization points but the ones of the separator
@@ -1059,8 +1035,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
   filter.postsync();
 
   NonlinearFactorGraph expectedSmootherFactors2;
-  expectedSmootherFactors2.add(factor3);
-  expectedSmootherFactors2.add(factor4);
+  expectedSmootherFactors2.push_back(factor3);
+  expectedSmootherFactors2.push_back(factor4);
 
   Values expectedSmootherValues2;
   expectedSmootherValues2.insert(2, newValues.at(2));
@@ -1075,7 +1051,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
   // ------------------------------------------------------------------------------
   // This cannot be nonempty for the first call to synchronize
   NonlinearFactorGraph partialGraphFilter;
-  partialGraphFilter.add(factor5);
+  partialGraphFilter.push_back(factor5);
 
 
   Values partialValuesFilter;
@@ -1096,8 +1072,8 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
   // Now we should check that the smooother summarization on the old separator is correctly propagated
   // on the new separator by the filter
   NonlinearFactorGraph partialGraphTransition;
-  partialGraphTransition.add(factor3);
-  partialGraphTransition.add(factor4);
+  partialGraphTransition.push_back(factor3);
+  partialGraphTransition.push_back(factor4);
   partialGraphTransition.push_back(smootherSummarization2);
 
   Values partialValuesTransition;
@@ -1119,7 +1095,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
   expectedFilterGraph.push_back(factorEmpty);
   expectedFilterGraph.push_back(factorEmpty);
   expectedFilterGraph.push_back(factorEmpty);
-  expectedFilterGraph.add(factor5);
+  expectedFilterGraph.push_back(factor5);
   expectedFilterGraph.push_back(factorEmpty);
   expectedFilterGraph.push_back(factorEmpty);
   expectedFilterGraph.push_back(factorEmpty);
@@ -1143,10 +1119,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
   NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
 
   NonlinearFactorGraph factorGraph;
-  factorGraph.add(factor1);
-  factorGraph.add(factor2);
-  factorGraph.add(factor3);
-  factorGraph.add(factor4);
+  factorGraph.push_back(factor1);
+  factorGraph.push_back(factor2);
+  factorGraph.push_back(factor3);
+  factorGraph.push_back(factor4);
 
   Values newValues;
   Pose3 value1 = Pose3().compose(poseError);
@@ -1157,26 +1133,18 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
   newValues.insert(2, value2);
   newValues.insert(3, value3);
 
-  // We first manually
-  // Create an ordering
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
-  ordering.push_back(3);
-
-  GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering);
-
   // Create the set of marginalizable variables
-  std::vector<Index> linearIndices;
-  linearIndices.push_back(ordering.at(1));
+  std::vector<Key> linearIndices;
+  linearIndices.push_back(1);
 
-  std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky);
+  GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
 
-  NonlinearFactorGraph expectedMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
-    expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues));
+    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
 
-  }
+    NonlinearFactorGraph expectedMarginals;
+    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+      expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
+    }
 
   FastList<Key> keysToMarginalize;
   keysToMarginalize.push_back(1);
@@ -1201,10 +1169,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
   NonlinearFactor::shared_ptr factor4(new BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
 
   NonlinearFactorGraph factorGraph;
-  factorGraph.add(factor1);
-  factorGraph.add(factor2);
-  factorGraph.add(factor3);
-  factorGraph.add(factor4);
+  factorGraph.push_back(factor1);
+  factorGraph.push_back(factor2);
+  factorGraph.push_back(factor3);
+  factorGraph.push_back(factor4);
 
   Values newValues;
   Pose3 value1 = Pose3().compose(poseError);
@@ -1215,28 +1183,24 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
   newValues.insert(2, value2);
   newValues.insert(3, value3);
 
-  // We first manually
-  // Create an ordering
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
-  ordering.push_back(3);
 
-  GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues, ordering);
 
-  // Create the set of marginalizable variables
-  std::vector<Index> linearIndices;
-  linearIndices.push_back(ordering.at(1));
-  linearIndices.push_back(ordering.at(2));
+    // Create the set of marginalizable variables
+  std::vector<Key> linearIndices;
+  linearIndices.push_back(1);
+  linearIndices.push_back(2);
 
-  std::pair<GaussianConditional::shared_ptr, GaussianFactorGraph> result = linearFactorGraph.eliminate(linearIndices, EliminateCholesky);
+
+  GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
+
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
-    expectedMarginals.add(LinearContainerFactor(factor, ordering, newValues));
-
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
+    expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
   }
 
+
   FastList<Key> keysToMarginalize;
   keysToMarginalize.push_back(1);
   keysToMarginalize.push_back(2);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
index 0b91644e2..c02dbea18 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
@@ -115,8 +115,8 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
   newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
@@ -132,8 +132,8 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
   newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
@@ -166,8 +166,8 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
   newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
@@ -183,8 +183,8 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
   newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
@@ -200,12 +200,6 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
   CHECK(assert_equal(expected3, actual3));
 }
 
-/* ************************************************************************* */
-TEST( ConcurrentIncrementalSmootherDL, getOrdering )
-{
-  // TODO: Think about how to check ordering...
-}
-
 /* ************************************************************************* */
 TEST( ConcurrentIncrementalSmootherDL, getDelta )
 {
@@ -229,8 +223,8 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
   newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -249,8 +243,8 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors3;
-  newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues3;
   newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@@ -316,8 +310,8 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
   newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -336,8 +330,8 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors3;
-  newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues3;
   newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@@ -403,11 +397,9 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_1 )
   Values smootherValues, filterSeparatorValues;
   filterSeparatorValues.insert(1, Pose3().compose(poseError));
   filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
-  filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
+
+  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
 
   // Create expected values: the smoother output will be empty for this case
   NonlinearFactorGraph expectedSmootherSummarization;
@@ -467,15 +459,13 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 )
   // Create a separator and cached smoother factors *from* the filter
   NonlinearFactorGraph smootherFactors, filterSumarization;
   Values smootherValues, filterSeparatorValues;
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
+
   filterSeparatorValues.insert(1, Pose3().compose(poseError));
   filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
-  filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues,), filterSeparatorValues));
+  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
@@ -548,16 +538,14 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
   // Create a separator and cached smoother factors *from* the filter
   NonlinearFactorGraph smootherFactors, filterSumarization;
   Values smootherValues, filterSeparatorValues;
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
+
   filterSeparatorValues.insert(1, Pose3().compose(poseError));
   filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
-  filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
-  smootherFactors.add(PriorFactor<Pose3>(4, poseInitial, noisePrior));
+  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
+  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
@@ -574,7 +562,6 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
 
   // Check
   CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
-  CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));
 
 
   // Update the smoother
@@ -602,7 +589,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
 
   expectedSmootherSummarization.resize(0);
   BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
-    expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues));
+    expectedSmootherSummarization.push_back(LinearContainerFactor(factor, ordering, allValues));
   }
 
   CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
index bdca9238c..443273479 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
@@ -23,10 +23,9 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LinearContainerFactor.h>
-#include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Symbol.h>
-#include <gtsam/nonlinear/Key.h>
+#include <gtsam/inference/Key.h>
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -115,8 +114,8 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
   newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
@@ -132,8 +131,8 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
   newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
@@ -166,8 +165,8 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors1;
-  newFactors1.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors1.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues1;
   newValues1.insert(1, Pose3());
   newValues1.insert(2, newValues1.at<Pose3>(1).compose(poseOdometry));
@@ -183,8 +182,8 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors2.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors2.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(3, newValues1.at<Pose3>(2).compose(poseOdometry));
   newValues2.insert(4, newValues2.at<Pose3>(3).compose(poseOdometry));
@@ -200,12 +199,6 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
   CHECK(assert_equal(expected3, actual3));
 }
 
-/* ************************************************************************* */
-TEST( ConcurrentIncrementalSmootherGN, getOrdering )
-{
-  // TODO: Think about how to check ordering...
-}
-
 /* ************************************************************************* */
 TEST( ConcurrentIncrementalSmootherGN, getDelta )
 {
@@ -229,8 +222,8 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
   newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -249,8 +242,8 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors3;
-  newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues3;
   newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@@ -316,8 +309,8 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple )
 
   // Add some factors to the smoother
   NonlinearFactorGraph newFactors2;
-  newFactors2.add(PriorFactor<Pose3>(1, poseInitial, noisePrior));
-  newFactors2.add(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
+  newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
+  newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
   Values newValues2;
   newValues2.insert(1, Pose3().compose(poseError));
   newValues2.insert(2, newValues2.at<Pose3>(1).compose(poseOdometry).compose(poseError));
@@ -336,8 +329,8 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple )
 
   // Add some more factors to the smoother
   NonlinearFactorGraph newFactors3;
-  newFactors3.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  newFactors3.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  newFactors3.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   Values newValues3;
   newValues3.insert(3, newValues2.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   newValues3.insert(4, newValues3.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@@ -406,8 +399,8 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_1 )
   Ordering ordering;
   ordering.push_back(1);
   ordering.push_back(2);
-  filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
 
   // Create expected values: the smoother output will be empty for this case
   NonlinearFactorGraph expectedSmootherSummarization;
@@ -467,15 +460,13 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 )
   // Create a separator and cached smoother factors *from* the filter
   NonlinearFactorGraph smootherFactors, filterSumarization;
   Values smootherValues, filterSeparatorValues;
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
+
   filterSeparatorValues.insert(1, Pose3().compose(poseError));
   filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
-  filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
+  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
@@ -548,16 +539,14 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
   // Create a separator and cached smoother factors *from* the filter
   NonlinearFactorGraph smootherFactors, filterSumarization;
   Values smootherValues, filterSeparatorValues;
-  Ordering ordering;
-  ordering.push_back(1);
-  ordering.push_back(2);
+
   filterSeparatorValues.insert(1, Pose3().compose(poseError));
   filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));
-  filterSumarization.add(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  filterSumarization.add(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues, ordering), ordering, filterSeparatorValues));
-  smootherFactors.add(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
-  smootherFactors.add(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
-  smootherFactors.add(PriorFactor<Pose3>(4, poseInitial, noisePrior));
+  filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
+  filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
+  smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
+  smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
+  smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
   smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
   smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
 
@@ -586,9 +575,9 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
   // Check the optimized value of smoother state
   NonlinearFactorGraph allFactors = smootherFactors;
   Values allValues = smoother.getLinearizationPoint();
-  ordering = smoother.getOrdering();  // I'm really hoping this is an acceptable ordering...
+//  ordering = smoother.getOrdering();  // I'm really hoping this is an acceptable ordering...
 
-  GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues, ordering);
+  GaussianFactorGraph::shared_ptr LinFactorGraph = allFactors.linearize(allValues);
 //  GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
 //  GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
 
@@ -602,7 +591,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
 
   expectedSmootherSummarization.resize(0);
   BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, result.second) {
-    expectedSmootherSummarization.add(LinearContainerFactor(factor, ordering, allValues));
+    expectedSmootherSummarization.push_back(LinearContainerFactor(factor, ordering, allValues));
   }
 
   CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));