diff --git a/cpp/GaussianISAM2.cpp b/cpp/GaussianISAM2.cpp
index 64d147237..9cb77c186 100644
--- a/cpp/GaussianISAM2.cpp
+++ b/cpp/GaussianISAM2.cpp
@@ -11,7 +11,7 @@ using namespace gtsam;
 
 // Explicitly instantiate so we don't have to include everywhere
 #include "ISAM2-inl.h"
-template class ISAM2<GaussianConditional, VectorConfig>;
+//template class ISAM2<GaussianConditional, VectorConfig>;
 
 namespace gtsam {
 
diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h
index 24a828002..81deef8cd 100644
--- a/cpp/ISAM2-inl.h
+++ b/cpp/ISAM2-inl.h
@@ -30,10 +30,10 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class Conditional, class Config>
-	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactorsXXX, Cliques& orphans) {
+	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactorsXXX, const Config& config, Cliques& orphans) {
 
-		Config xxx;
-		FactorGraph<GaussianFactor> newFactors; //todo = newFactorsXXX.linearize(xxx);
+		config_ = config; // todo
+		FactorGraph<GaussianFactor> newFactors = newFactorsXXX.linearize(config); // todo: just for testing
 
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<GaussianFactor> factors;
@@ -74,9 +74,9 @@ namespace gtsam {
 	}
 
 	template<class Conditional, class Config>
-	void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors) {
+	void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors, const Config& config) {
 		Cliques orphans;
-		this->update_internal(newFactors, orphans);
+		this->update_internal(newFactors, config, orphans);
 	}
 
 /* ************************************************************************* */
diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h
index 6f2a6ac3c..6d9f6785a 100644
--- a/cpp/ISAM2.h
+++ b/cpp/ISAM2.h
@@ -26,6 +26,8 @@ namespace gtsam {
 	template<class Conditional, class Config>
 	class ISAM2: public BayesTree<Conditional> {
 
+		// for keeping all original nonlinear data
+		Config config_;
 		NonlinearFactorGraph<Config> nonlinearFactors_;
 
 	public:
@@ -47,8 +49,8 @@ namespace gtsam {
 		/**
 		 * ISAM2. (update_internal provides access to list of orphans for drawing purposes)
 		 */
-		void update_internal(const NonlinearFactorGraph<Config>& newFactors, Cliques& orphans);
-		void update(const NonlinearFactorGraph<Config>& newFactors);
+		void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
+		void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
 
 	}; // ISAM2
 
diff --git a/cpp/testGaussianISAM2.cpp b/cpp/testGaussianISAM2.cpp
index c63dd0f6e..db90d0c65 100644
--- a/cpp/testGaussianISAM2.cpp
+++ b/cpp/testGaussianISAM2.cpp
@@ -24,7 +24,7 @@ using namespace gtsam;
 
 double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
 		0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
-#if 0
+
 /* ************************************************************************* */
 TEST( ISAM2, ISAM2_smoother )
 {
@@ -38,7 +38,7 @@ TEST( ISAM2, ISAM2_smoother )
 	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<VectorConfig> > factor, smoother) {
 		ExampleNonlinearFactorGraph factorGraph;
 		factorGraph.push_back(factor);
-		actual.update(factorGraph);
+		actual.update(factorGraph, poses);
 	}
 
 	// Create expected Bayes Tree by solving smoother with "natural" ordering
@@ -75,7 +75,7 @@ TEST( ISAM2, ISAM2_smoother2 )
 	// run ISAM2 with remaining factors
 	ExampleNonlinearFactorGraph factors2;
 	for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
-	actual.update(factors2);
+	actual.update(factors2, poses);
 
 	// Create expected Bayes Tree by solving smoother with "natural" ordering
 	Ordering ordering;
@@ -302,7 +302,7 @@ TEST( BayesTree, balanced_smoother_joint )
 	GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
 	CHECK(assert_equal(expected4,actual4,1e-4));
 }
-#endif
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */