diff --git a/cpp/NonlinearFactorGraph-inl.h b/cpp/NonlinearFactorGraph-inl.h
index 06a8bc7de..b7ddb4d67 100644
--- a/cpp/NonlinearFactorGraph-inl.h
+++ b/cpp/NonlinearFactorGraph-inl.h
@@ -28,27 +28,26 @@ namespace gtsam {
 	}
 	/* ************************************************************************* */
 	template<class Config>
-	GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
-			const Config& config) const {
-		// TODO speed up the function either by returning a pointer or by
-		// returning the linearisation as a second argument and returning
-		// the reference
+	boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_(
+			const Config& config) const{
 
 		// create an empty linear FG
-		GaussianFactorGraph linearFG;
+		boost::shared_ptr<GaussianFactorGraph>  linearFG(new GaussianFactorGraph);
 
 		typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
-				const_iterator;
+		const_iterator;
 		// linearize all factors
 		for (const_iterator factor = this->factors_.begin(); factor
-				< this->factors_.end(); factor++) {
+		< this->factors_.end(); factor++) {
 			boost::shared_ptr<GaussianFactor> lf = (*factor)->linearize(config);
-			linearFG.push_back(lf);
+			linearFG->push_back(lf);
 		}
-
 		return linearFG;
 	}
-
-/* ************************************************************************* */
-
+	/* ************************************************************************* */
+	template<class Config>
+	GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
+			const Config& config) const {
+		return *linearize_(config);
+	}
 }
diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h
index 42e383167..86ffb4bf0 100644
--- a/cpp/NonlinearFactorGraph.h
+++ b/cpp/NonlinearFactorGraph.h
@@ -41,6 +41,11 @@ namespace gtsam {
 		 */
 		GaussianFactorGraph linearize(const Config& config) const;
 
+	    /**
+	     * shared pointer versions for MATLAB
+	     */
+	    boost::shared_ptr<GaussianFactorGraph> linearize_(const Config& config) const;
+
 	};
 
 } // namespace
diff --git a/cpp/Ordering.h b/cpp/Ordering.h
index bf5ce35c0..bb61a5125 100644
--- a/cpp/Ordering.h
+++ b/cpp/Ordering.h
@@ -53,6 +53,7 @@ namespace gtsam {
 		 * @return bool
 		 */
 		bool equals(const Ordering &ord, double tol=0) const;
+
 	};
 
 }
diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp
index 4e054d7d4..c1ff3e4ef 100644
--- a/cpp/Pose2Graph.cpp
+++ b/cpp/Pose2Graph.cpp
@@ -12,7 +12,7 @@ namespace gtsam {
 
 // explicit instantiation so all the code is there and we can link with it
 template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
-template class NonlinearFactorGraph<Pose2Factor> ;
+template class NonlinearFactorGraph<Pose2Config> ;
 //template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
 
 void Pose2Graph::print(const std::string& s) const {
diff --git a/cpp/gtsam.h b/cpp/gtsam.h
index 821a8cec0..e5e8f2aa4 100644
--- a/cpp/gtsam.h
+++ b/cpp/gtsam.h
@@ -2,6 +2,7 @@ class Ordering {
   Ordering();
   void push_back(string s);
   void print(string s) const;
+  void unique ();
 };
 
 class SymbolicFactor{
@@ -193,6 +194,8 @@ class Pose2Graph{
 	Pose2Graph();
 	void print(string s) const;
 	bool equals(const Pose2Graph& p, double tol) const;
-	GaussianFactorGraph linearize(const Pose2Config& config) const;
+	GaussianFactorGraph* linearize_(const Pose2Config& config) const;
 	void push_back(Pose2Factor* factor);
 };
+
+
diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp
index 64034b5f6..d77154a10 100644
--- a/cpp/testGaussianFactorGraph.cpp
+++ b/cpp/testGaussianFactorGraph.cpp
@@ -31,6 +31,7 @@ double tol=1e-4;
 TEST( GaussianFactorGraph, equals ){
 
   GaussianFactorGraph fg = createGaussianFactorGraph();
+  fg.print("fg");
   GaussianFactorGraph fg2 = createGaussianFactorGraph();
   CHECK(fg.equals(fg2));
 }