diff --git a/gtsam-broken.h b/gtsam-broken.h
index bbb6245a3..e7f950f5b 100644
--- a/gtsam-broken.h
+++ b/gtsam-broken.h
@@ -2,13 +2,76 @@
 // Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class
 // We also have to solve the shared pointer mess to avoid duplicate methods
 
+class GaussianFactor {
+	GaussianFactor(string key1,
+			Matrix A1,
+			Vector b_in,
+			const SharedDiagonal& model);
+	GaussianFactor(string key1,
+			Matrix A1,
+			string key2,
+			Matrix A2,
+			Vector b_in,
+			const SharedDiagonal& model);
+	GaussianFactor(string key1,
+			Matrix A1,
+			string key2,
+			Matrix A2,
+			string key3,
+			Matrix A3,
+			Vector b_in,
+			const SharedDiagonal& model);
+	void print(string s) const;
+	bool equals(const GaussianFactor& lf, double tol) const;
+	bool empty() const;
+	Vector get_b() const;
+	Matrix get_A(string key) const;
+	double error(const VectorValues& c) const;
+	bool involves(string key) const;
+	pair<Matrix,Vector> matrix(const Ordering& ordering) const;
+	pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
+};
+
+class GaussianConditional {
+	GaussianConditional(string key,
+			Vector d,
+			Matrix R,
+			Vector sigmas);
+	GaussianConditional(string key,
+			Vector d,
+			Matrix R,
+			string name1,
+			Matrix S,
+			Vector sigmas);
+	GaussianConditional(string key,
+			Vector d,
+			Matrix R,
+			string name1,
+			Matrix S,
+			string name2,
+			Matrix T,
+			Vector sigmas);
+	void add(string key, Matrix S);
+};
+
+class GaussianFactorGraph {
+	GaussianConditional* eliminateOne(string key);
+	GaussianBayesNet* eliminate_(const Ordering& ordering);
+	VectorValues* optimize_(const Ordering& ordering);
+	pair<Matrix,Vector> matrix(const Ordering& ordering) const;
+	Matrix sparse(const Ordering& ordering) const;
+	VectorValues* steepestDescent_(const VectorValues& x0) const;
+	VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
+};
+
+
 class Pose2Values{
 	Pose2Values();
 	Pose2 get(string key) const;
 	void insert(string name, const Pose2& val);
-    void print(string s) const;
-    void clear();
-    int size();
+	void print(string s) const;
+	void clear();
+	int size();
 };
 
 class Pose2Factor {
@@ -27,4 +90,60 @@ class Pose2Graph{
 	void push_back(Pose2Factor* factor);
 };
 
+class Ordering{
+	Ordering(string key);
+	Ordering subtract(const Ordering& keys) const;
+	void unique ();
+	void reverse ();
+};
+
+
+class SymbolicFactor{
+	SymbolicFactor(const Ordering& keys);
+	void print(string s) const;
+};
+
+
+class VectorValues {
+	void insert(string name, Vector val);
+	Vector get(string name) const;
+	bool contains(string name) const;
+};
+
+class Simulated2DPosePrior {
+	GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Simulated2DOrientedPosePrior {
+	GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
+};
+
+class Simulated2DPointPrior {
+	GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Simulated2DOdometry {
+	GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Simulated2DOrientedOdometry {
+	GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
+};
+
+class Simulated2DMeasurement {
+	GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Pose2SLAMOptimizer {
+	Pose2SLAMOptimizer(string dataset_name);
+	void print(string s) const;
+	void update(Vector x) const;
+	Vector optimize() const;
+	double error() const;
+	Matrix a1() const;
+	Matrix a2() const;
+	Vector b1() const;
+	Vector b2() const;
+};
+
 
diff --git a/gtsam.h b/gtsam.h
index 3bf84cafb..620b70f85 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -9,60 +9,19 @@ class SharedDiagonal {
 
 class Ordering {
   Ordering();
-  Ordering(string key);
-  Ordering subtract(const Ordering& keys) const;
   void print(string s) const;
   bool equals(const Ordering& ord, double tol) const;
   void push_back(string s);
-  void unique ();
-  void reverse ();
 };
 
-class SymbolicFactor{
-	SymbolicFactor(const Ordering& keys);
-	void print(string s) const;
-};
 
 class VectorValues {
   VectorValues();
   void print(string s) const;
   bool equals(const VectorValues& expected, double tol) const;
-  void insert(string name, Vector val);
-  Vector get(string name) const;
-  bool contains(string name) const;
   size_t size() const;
 };
 
-class GaussianFactor {
-  GaussianFactor(string key1,
-	       Matrix A1,
-	       Vector b_in,
-	       const SharedDiagonal& model);
-  GaussianFactor(string key1,
-	       Matrix A1,
-	       string key2,
-	       Matrix A2,
-	       Vector b_in,
-	       const SharedDiagonal& model);
-  GaussianFactor(string key1,
-	       Matrix A1,
-	       string key2,
-	       Matrix A2,
-	       string key3,
-	       Matrix A3,
-	       Vector b_in,
-	       const SharedDiagonal& model);
-  void print(string s) const;
-  bool equals(const GaussianFactor& lf, double tol) const;
-  bool empty() const;
-  Vector get_b() const;
-  Matrix get_A(string key) const;
-  double error(const VectorValues& c) const;
-  bool involves(string key) const;
-  pair<Matrix,Vector> matrix(const Ordering& ordering) const;
-  pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
-};
-
 class GaussianFactorSet {
   GaussianFactorSet();
   void push_back(GaussianFactor* factor);
@@ -70,27 +29,8 @@ class GaussianFactorSet {
 
 class GaussianConditional {
   GaussianConditional();
-  GaussianConditional(string key,
-  		    Vector d,
-		      Matrix R,
-		      Vector sigmas);
-  GaussianConditional(string key,
-  		    Vector d,
-		      Matrix R,
-		      string name1,
-		      Matrix S,
-		      Vector sigmas);
-  GaussianConditional(string key,
-  		    Vector d,
-		      Matrix R,
-		      string name1,
-		      Matrix S,
-		      string name2,
-		      Matrix T,
-		      Vector sigmas);
   void print(string s) const;
   bool equals(const GaussianConditional &cg, double tol) const;
-  void add(string key, Matrix S);
   Vector solve(const VectorValues& x);
 };
 
@@ -112,14 +52,6 @@ class GaussianFactorGraph {
   double error(const VectorValues& c) const;
   double probPrime(const VectorValues& c) const;
   void combine(const GaussianFactorGraph& lfg);
-
-  GaussianConditional* eliminateOne(string key);
-  GaussianBayesNet* eliminate_(const Ordering& ordering);
-  VectorValues* optimize_(const Ordering& ordering);
-  pair<Matrix,Vector> matrix(const Ordering& ordering) const;
-  Matrix sparse(const Ordering& ordering) const;
-  VectorValues* steepestDescent_(const VectorValues& x0) const;
-  VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
 };
 
 class Point2 {
@@ -184,54 +116,36 @@ class Simulated2DOrientedValues {
 class Simulated2DPosePrior {
 	Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
   void print(string s) const;
-	GaussianFactor* linearize(const Simulated2DValues& config) const;
   double error(const Simulated2DValues& c) const;
 };
 
 class Simulated2DOrientedPosePrior {
 	Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
   void print(string s) const;
-	GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
   double error(const Simulated2DOrientedValues& c) const;
 };
 
 class Simulated2DPointPrior {
 	Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
   void print(string s) const;
-	GaussianFactor* linearize(const Simulated2DValues& config) const;
   double error(const Simulated2DValues& c) const;
 };
 
 class Simulated2DOdometry {
   Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
   void print(string s) const;
-	GaussianFactor* linearize(const Simulated2DValues& config) const;
   double error(const Simulated2DValues& c) const;
 };
 
 class Simulated2DOrientedOdometry {
 	Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
   void print(string s) const;
-	GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
   double error(const Simulated2DOrientedValues& c) const;
 };
 
 class Simulated2DMeasurement {
   Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
   void print(string s) const;
-	GaussianFactor* linearize(const Simulated2DValues& config) const;
   double error(const Simulated2DValues& c) const;
 };
 
-class Pose2SLAMOptimizer {
-	Pose2SLAMOptimizer(string dataset_name);
-	void print(string s) const;
-	void update(Vector x) const;
-	Vector optimize() const;
-	double error() const;
-	Matrix a1() const;
-	Matrix a2() const;
-	Vector b1() const;
-	Vector b2() const;
-};
-
diff --git a/wrap/Makefile.am b/wrap/Makefile.am
index 657ee7008..5d1812627 100644
--- a/wrap/Makefile.am
+++ b/wrap/Makefile.am
@@ -9,7 +9,7 @@ interfacePath = $(top_srcdir)
 moduleName = gtsam
 toolboxpath = ../toolbox
 nameSpace = "gtsam"
-mexFlags = "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -L${exec_prefix}/lib -lgtsam"
+mexFlags = "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam"
 all:
 	./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags}