diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index aae97de37..da1510d29 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -28,81 +28,96 @@
 using namespace std;
 using namespace wrap;
 
+static const uint64_t ptr_constructor_key =
+	(uint64_t('G') << 56) |
+	(uint64_t('T') << 48) |
+	(uint64_t('S') << 40) |
+	(uint64_t('A') << 32) |
+	(uint64_t('M') << 24) |
+	(uint64_t('p') << 16) |
+	(uint64_t('t') << 8) |
+	(uint64_t('r'));
+
 /* ************************************************************************* */
-void Class::matlab_proxy(const string& classFile) const {
+void Class::matlab_proxy(const string& classFile, const string& wrapperName, FileWriter& wrapperFile, vector<string>& functionNames) const {
   // open destination classFile
-  FileWriter file(classFile, verbose_, "%");
+  FileWriter proxyFile(classFile, verbose_, "%");
 
   // get the name of actual matlab object
-  string matlabName = qualifiedName();
+  string matlabName = qualifiedName(), cppName = qualifiedName("::");
 
   // emit class proxy code
   // we want our class to inherit the handle class for memory purposes
-  file.oss << "classdef " << matlabName << " < handle" << endl;
-  file.oss << "  properties" << endl;
-  file.oss << "    self = 0" << endl;
-  file.oss << "  end" << endl;
-  file.oss << "  methods" << endl;
-  // constructor
-  file.oss << "    function obj = " << matlabName << "(varargin)" << endl;
-  //i is constructor id
-  int id = 0;
+  proxyFile.oss << "classdef " << matlabName << " < handle" << endl;
+  proxyFile.oss << "  properties" << endl;
+  proxyFile.oss << "    self = 0" << endl;
+  proxyFile.oss << "  end" << endl;
+  proxyFile.oss << "  methods" << endl;
+
+  // Constructor
+  proxyFile.oss << "    function obj = " << matlabName << "(varargin)" << endl;
+  // Special pointer constructor
+	{
+		const int id = functionNames.size();
+		proxyFile.oss << "      if nargin == 2 && isa(varargin{1}, 'uint64') && ";
+		proxyFile.oss << "varargin{1} == uint64(" << ptr_constructor_key << ")\n";
+		proxyFile.oss << "        obj.self = varargin{2};\n";
+	}
+	// Regular constructors
   BOOST_FOREACH(ArgumentList a, constructor.args_list)
   {
-    constructor.matlab_proxy_fragment(file,matlabName, id, a);
-    id++;
+		const int id = functionNames.size();
+    constructor.proxy_fragment(proxyFile, wrapperName, matlabName, id, a);
+		const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
+			cppName, matlabName, id, using_namespaces, includes, a);
+		wrapperFile.oss << "\n";
+    functionNames.push_back(wrapFunctionName);
   }
-  //Static constructor collect call
-  file.oss << "      if nargin ==14, new_" << matlabName << "(varargin{1},0); end" << endl;
-  file.oss << "      if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl;
-  file.oss << "    end" << endl;
-  // deconstructor
-  file.oss << "    function delete(obj)" << endl;
-  file.oss << "      if obj.self ~= 0" << endl;
-  //TODO: Add verbosity flag
-  //file.oss << "        fprintf(1,'MATLAB class deleting %x',obj.self);" << endl;
-  file.oss << "        new_" << matlabName << "(obj.self);" << endl;
-  file.oss << "        obj.self = 0;" << endl;
-  file.oss << "      end" << endl;
-  file.oss << "    end" << endl;
-  file.oss << "    function display(obj), obj.print(''); end" << endl;
-  file.oss << "    function disp(obj), obj.display; end" << endl;
-  file.oss << "  end" << endl;
-  file.oss << "end" << endl;
+  proxyFile.oss << "      else\n";
+	proxyFile.oss << "        error('" << matlabName << " constructor failed');" << endl;
+	proxyFile.oss << "      end\n";
+  proxyFile.oss << "    end\n\n";
 
-  // close file
-  file.emit(true);
+	// Deconstructor
+	{
+		const int id = functionNames.size();
+		deconstructor.proxy_fragment(proxyFile, wrapperName, matlabName, id);
+		proxyFile.oss << "\n";
+		const string functionName = deconstructor.wrapper_fragment(wrapperFile,
+			cppName, matlabName, id, using_namespaces, includes);
+		wrapperFile.oss << "\n";
+		functionNames.push_back(functionName);
+	}
+  proxyFile.oss << "    function display(obj), obj.print(''); end\n\n";
+  proxyFile.oss << "    function disp(obj), obj.display; end\n\n";
+
+	// Methods
+	BOOST_FOREACH(Method m, methods) {
+		const int id = functionNames.size();
+		m.proxy_fragment(proxyFile, wrapperName, id);
+		proxyFile.oss << "\n";
+		const string wrapFunctionName = m.wrapper_fragment(wrapperFile,
+			cppName, matlabName, id, using_namespaces);
+		wrapperFile.oss << "\n";
+		functionNames.push_back(wrapFunctionName);
+	}
+
+	proxyFile.oss << "  end" << endl;
+	proxyFile.oss << "end" << endl;
+
+  // Close file
+  proxyFile.emit(true);
 }
 
 /* ************************************************************************* */
-//TODO: Consolidate into single file
-void Class::matlab_constructors(const string& toolboxPath) const {
-  /*BOOST_FOREACH(Constructor c, constructors) {
-    args_list.push_back(c.args);
-  }*/
-
-  BOOST_FOREACH(ArgumentList a, constructor.args_list) {
-    constructor.matlab_mfile(toolboxPath, qualifiedName(), a);
-  }
-    constructor.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), 
-                     using_namespaces, includes);
-}
-
-/* ************************************************************************* */
-void Class::matlab_methods(const string& classPath) const {
-	string matlabName = qualifiedName(), cppName = qualifiedName("::");
-  BOOST_FOREACH(Method m, methods) {
-    m.matlab_mfile  (classPath);
-    m.matlab_wrapper(classPath, name, cppName, matlabName, using_namespaces, includes);
-  }
-}
-
-/* ************************************************************************* */
-void Class::matlab_static_methods(const string& toolboxPath) const {
+void Class::matlab_static_methods(const string& toolboxPath, const string& wrapperName,
+																	FileWriter& wrapperFile, vector<string>& functionNames) const {
 	string matlabName = qualifiedName(), cppName = qualifiedName("::");
   BOOST_FOREACH(const StaticMethod& m, static_methods) {
-    m.matlab_mfile  (toolboxPath, qualifiedName());
-    m.matlab_wrapper(toolboxPath, name, matlabName, cppName, using_namespaces, includes);
+		const int id = functionNames.size();
+		m.proxy_fragment(toolboxPath, matlabName, wrapperName, id);
+    const string wrapFunction = m.wrapper_fragment(wrapperFile, matlabName, cppName, id, using_namespaces);
+		functionNames.push_back(wrapFunction);
   }
 }
 
diff --git a/wrap/Class.h b/wrap/Class.h
index fb9906716..bef7467c3 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -19,6 +19,7 @@
 #pragma once
 
 #include <string>
+#include <boost/bimap.hpp>
 
 #include "Constructor.h"
 #include "Deconstructor.h"
@@ -37,16 +38,17 @@ struct Class {
   std::vector<Method> methods;              ///< Class methods
   std::vector<StaticMethod> static_methods; ///< Static methods
   std::vector<std::string> namespaces;      ///< Stack of namespaces
-  std::vector<std::string> using_namespaces; ///< default namespaces
+  std::vector<std::string> using_namespaces;///< default namespaces
   std::vector<std::string> includes;        ///< header include overrides
-  Constructor constructor;    ///< Class constructors
+  Constructor constructor;                  ///< Class constructors
+	Deconstructor deconstructor;              ///< Deconstructor to deallocate C++ object
   bool verbose_;                            ///< verbose flag
 
   // And finally MATLAB code is emitted, methods below called by Module::matlab_code
-  void matlab_proxy(const std::string& classFile) const;          ///< emit proxy class
-  void matlab_constructors(const std::string& toolboxPath) const;   ///< emit constructor wrappers
-  void matlab_methods(const std::string& classPath) const;   ///< emit method wrappers
-  void matlab_static_methods(const std::string& classPath) const;   ///< emit static method wrappers
+  void matlab_proxy(const std::string& classFile, const std::string& wrapperName,
+		FileWriter& wrapperFile, std::vector<std::string>& functionNames) const;          ///< emit proxy class
+  void matlab_static_methods(const std::string& toolboxPath, const std::string& wrapperName,
+		FileWriter& wrapperFile, std::vector<std::string>& functionNames) const;   ///< emit static method wrappers
   void matlab_make_fragment(FileWriter& file,
 			    const std::string& toolboxPath,
 			    const std::string& mexFlags) const;   ///< emit make fragment for global make script
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
index 978516b18..43f35fb86 100644
--- a/wrap/Constructor.cpp
+++ b/wrap/Constructor.cpp
@@ -20,6 +20,7 @@
 #include <algorithm>
 
 #include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include "utilities.h"
 #include "Constructor.h"
@@ -35,11 +36,11 @@ string Constructor::matlab_wrapper_name(const string& className) const {
 }
 
 /* ************************************************************************* */
-void Constructor::matlab_proxy_fragment(FileWriter& file, 
+void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName,
         const string& className, const int id, const ArgumentList args) const {
 	size_t nrArgs = args.size();
 	// check for number of arguments...
-  file.oss << "      if (nargin == " << nrArgs;
+  file.oss << "      elseif nargin == " << nrArgs;
   if (nrArgs>0) file.oss << " && ";
 	// ...and their types
   bool first = true;
@@ -49,117 +50,48 @@ void Constructor::matlab_proxy_fragment(FileWriter& file,
     first=false;
   }
   // emit code for calling constructor
-  file.oss << "), obj.self = " << matlab_wrapper_name(className) << "(" << "0," << id;
+  file.oss << "\n        obj.self = " << wrapperName << "(" << id;
   // emit constructor arguments
   for(size_t i=0;i<nrArgs;i++) {
     file.oss << ",";
     file.oss << "varargin{" << i+1 << "}";
   }
-  file.oss << "); end" << endl;
+  file.oss << ");\n";
 }
 
 /* ************************************************************************* */
-void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName, const ArgumentList args) const {
-
-  string matlabName = matlab_wrapper_name(qualifiedMatlabName);
-
-  // open destination m-file
-  string wrapperFile = toolboxPath + "/" + matlabName + ".m";
-  FileWriter file(wrapperFile, verbose_, "%");
-
-  // generate code
-  file.oss << "function result = " << matlabName << "(obj";
-  if (args.size()) file.oss << "," << args.names();
-  file.oss << ")" << endl;
-  file.oss << "  error('need to compile " << matlabName << ".cpp');" << endl;
-  file.oss << "end" << endl;
-
-  // close file
-  file.emit(true);
-}
-
-/* ************************************************************************* */
-void Constructor::matlab_wrapper(const string& toolboxPath,
+string Constructor::wrapper_fragment(FileWriter& file,
 				 const string& cppClassName,
 				 const string& matlabClassName,
+				 int id,
 				 const vector<string>& using_namespaces, 
-				 const vector<string>& includes) const {
-  string matlabName = matlab_wrapper_name(matlabClassName);
-
-  // open destination wrapperFile
-  string wrapperFile = toolboxPath + "/" + matlabName + ".cpp";
-  FileWriter file(wrapperFile, verbose_, "//");
-
-  // generate code
-  generateIncludes(file, name, includes);
-  generateUsingNamespace(file, using_namespaces);
+				 const vector<string>& includes,
+				 const ArgumentList& al) const {
+  const string matlabName = matlab_wrapper_name(matlabClassName);
+	const string wrapFunctionName = matlabClassName + "_constructor_" + boost::lexical_cast<string>(id);
 
+  file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
+  file.oss << "{\n";
+	file.oss << "  mexAtExit(&_deleteAllObjects);\n";
+	generateUsingNamespace(file, using_namespaces);
   //Typedef boost::shared_ptr
-  file.oss << "typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
-  file.oss << endl;
+	file.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;\n";
+	file.oss << "\n";
 
-  //Generate collector
-  file.oss << "static std::set<Shared*> collector;" << endl;
-  file.oss << endl;
+	//Check to see if there will be any arguments and remove {} for consiseness
+	if(al.size() > 0)
+		al.matlab_unwrap(file); // unwrap arguments
+	file.oss << "  Shared *self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl;
+	file.oss << "  collector_" << matlabClassName << ".insert(self);\n";
 
-  //Generate cleanup function
-  file.oss << "void cleanup(void) {" << endl;
-  file.oss << "  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {\n";
-  file.oss << "    delete *iter;\n";
-  file.oss << "    collector.erase(iter++);\n";
-  file.oss << "  }\n";
-  file.oss << "}" << endl;
-
-  file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
-  file.oss << "{" << endl;
-  //Cleanup function callback
-  file.oss << "  mexAtExit(cleanup);" << endl;
-  file.oss << endl;
-  file.oss << "  const mxArray* input = in[0];" << endl;
-  file.oss << "  Shared* self = *(Shared**) mxGetData(input);" << endl;
-  file.oss << endl;
-  file.oss << "  if(self) {" << endl;
-  file.oss << "    if(nargin > 1) {" << endl;
-  file.oss << "      collector.insert(self);" << endl;
-  if(verbose_)
-    file.oss << "      std::cout << \"Collected\" << collector.size() << std::endl;" << endl;
-  file.oss << "    }" << endl;
-  file.oss << "    else if(collector.erase(self))" << endl;
-  file.oss << "      delete self;" << endl;
-  file.oss << "  } else {" << endl;
-  file.oss << "    int nc = unwrap<int>(in[1]);" << endl << endl;
-
-  int i = 0;
-  BOOST_FOREACH(ArgumentList al, args_list)
-  {
-      //Check to see if there will be any arguments and remove {} for consiseness
-      if(al.size())
-      {
-        file.oss << "    if(nc == " << i <<") {" << endl;
-        al.matlab_unwrap(file, 2); // unwrap arguments, start at 1
-        file.oss << "      self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl;
-        file.oss << "    }" << endl;
-      }
-      else
-      {
-        file.oss << "    if(nc == " << i <<")" << endl;
-        file.oss << "      self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl;
-      }
-    i++;
-  }
-  
-  //file.oss << "    self = construct(nc, in);" << endl;
-  file.oss << "    collector.insert(self);" << endl;
-  if(verbose_)
-    file.oss << "    std::cout << \"constructed \" << self << \", size=\" << collector.size() << std::endl;" << endl;
-  file.oss << "    out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl;
-  file.oss << "    *reinterpret_cast<Shared**> (mxGetPr(out[0])) = self;" << endl;
-  file.oss << "  }" << endl;
+	if(verbose_)
+    file.oss << "  std::cout << \"constructed \" << self << \" << std::endl;" << endl;
+  file.oss << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl;
+  file.oss << "  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;" << endl;
 
   file.oss << "}" << endl;
 
-  // close file
-  file.emit(true);
+	return wrapFunctionName;
 }
 
 /* ************************************************************************* */
diff --git a/wrap/Constructor.h b/wrap/Constructor.h
index 467ff3b22..4a9e31f9b 100644
--- a/wrap/Constructor.h
+++ b/wrap/Constructor.h
@@ -48,21 +48,18 @@ struct Constructor {
 	 * Create fragment to select constructor in proxy class, e.g.,
 	 * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
 	 */
-	void matlab_proxy_fragment(FileWriter& file, 
-	        const std::string& className, const int i,
+	void proxy_fragment(FileWriter& file, const std::string& wrapperName,
+	        const std::string& className, const int id,
 	        const ArgumentList args) const;
 
-	/// m-file
-	void matlab_mfile(const std::string& toolboxPath,
-			const std::string& qualifiedMatlabName,
-			const ArgumentList args) const;
-
 	/// cpp wrapper
-	void matlab_wrapper(const std::string& toolboxPath,
+	std::string wrapper_fragment(FileWriter& file,
 			 const std::string& cppClassName,
 			 const std::string& matlabClassName,
+			 int id,
 			 const std::vector<std::string>& using_namespaces,
-			 const std::vector<std::string>& includes) const;
+			 const std::vector<std::string>& includes,
+			 const ArgumentList& al) const;
 
 	/// constructor function
 	void generate_construct(FileWriter& file, const std::string& cppClassName,
diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp
index 93ed724f5..335488cf5 100644
--- a/wrap/Deconstructor.cpp
+++ b/wrap/Deconstructor.cpp
@@ -19,6 +19,7 @@
 #include <fstream>
 
 #include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include "utilities.h"
 #include "Deconstructor.h"
@@ -33,51 +34,42 @@ string Deconstructor::matlab_wrapper_name(const string& className) const {
 }
 
 /* ************************************************************************* */
-void Deconstructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) const {
+void Deconstructor::proxy_fragment(FileWriter& file,
+		const std::string& wrapperName,
+		const std::string& qualifiedMatlabName, int id) const {
 
-  string matlabName = matlab_wrapper_name(qualifiedMatlabName);
-
-  // open destination m-file
-  string wrapperFile = toolboxPath + "/" + matlabName + ".m";
-  FileWriter file(wrapperFile, verbose_, "%");
-
-  // generate code
-  file.oss << "function result = " << matlabName << "(obj";
-  if (args.size()) file.oss << "," << args.names();
-  file.oss << ")" << endl;
-  file.oss << "  error('need to compile " << matlabName << ".cpp');" << endl;
-  file.oss << "end" << endl;
-
-  // close file
-  file.emit(true);
+	file.oss << "    function delete(obj)\n";
+	file.oss << "      " << wrapperName << "(" << id << ", obj.self);\n";
+	file.oss << "    end\n";
 }
 
 /* ************************************************************************* */
-void Deconstructor::matlab_wrapper(const string& toolboxPath,
+string Deconstructor::wrapper_fragment(FileWriter& file,
 				 const string& cppClassName,
 				 const string& matlabClassName,
+				 int id,
 				 const vector<string>& using_namespaces, const vector<string>& includes) const {
-  string matlabName = matlab_wrapper_name(matlabClassName);
+  
+	const string matlabName = matlab_wrapper_name(matlabClassName);
 
-  // open destination wrapperFile
-  string wrapperFile = toolboxPath + "/" + matlabName + ".cpp";
-  FileWriter file(wrapperFile, verbose_, "//");
-
-  // generate code
-  //
-  generateIncludes(file, name, includes);
-  cout << "Generate includes " << name << endl;
-  generateUsingNamespace(file, using_namespaces);
+	const string wrapFunctionName = matlabClassName + "_deconstructor_" + boost::lexical_cast<string>(id);
     
-  file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
+  file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
   file.oss << "{" << endl;
+  generateUsingNamespace(file, using_namespaces);
+  file.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
   //Deconstructor takes 1 arg, the mxArray obj
   file.oss << "  checkArguments(\"" << matlabName << "\",nargout,nargin," << "1" << ");" << endl;
-  file.oss << "  delete_shared_ptr< " << cppClassName << " >(in[0],\"" << matlabClassName << "\");" << endl;
+	file.oss << "  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));\n";
+	file.oss << "  Collector_" << matlabClassName << "::iterator item;\n";
+	file.oss << "  item = collector_" << matlabClassName << ".find(self);\n";
+	file.oss << "  if(item != collector_" << matlabClassName << ".end()) {\n";
+  file.oss << "    delete self;\n";
+	file.oss << "    collector_" << matlabClassName << ".erase(item);\n";
+	file.oss << "  }\n";
   file.oss << "}" << endl;
 
-  // close file
-  file.emit(true);
+	return wrapFunctionName;
 }
 
 /* ************************************************************************* */
diff --git a/wrap/Deconstructor.h b/wrap/Deconstructor.h
index a5af4e327..363f986e6 100644
--- a/wrap/Deconstructor.h
+++ b/wrap/Deconstructor.h
@@ -46,13 +46,15 @@ struct Deconstructor {
 	std::string matlab_wrapper_name(const std::string& className) const;
 
 	/// m-file
-	void matlab_mfile(const std::string& toolboxPath,
-			const std::string& qualifiedMatlabName) const;
+	void proxy_fragment(FileWriter& file,
+		const std::string& wrapperName,
+		const std::string& qualifiedMatlabName, int id) const;
 
 	/// cpp wrapper
-	void matlab_wrapper(const std::string& toolboxPath,
+	std::string wrapper_fragment(FileWriter& file,
 			 const std::string& cppClassName,
 			 const std::string& matlabClassName,
+			 int id,
 			 const std::vector<std::string>& using_namespaces,
 			 const std::vector<std::string>& includes) const;
 };
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index 60b02918f..bcca86d40 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -18,6 +18,7 @@
 #include <fstream>
 
 #include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include "Method.h"
 #include "utilities.h"
@@ -26,57 +27,49 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void Method::matlab_mfile(const string& classPath) const {
+void Method::proxy_fragment(FileWriter& file, const std::string& wrapperName, const int id) const {
 
-  // open destination m-file
-  string wrapperFile = classPath + "/" + name + ".m";
-  FileWriter file(wrapperFile, verbose_, "%");
-
-  // generate code
-  string returnType = returnVal.matlab_returnType();
-  file.oss << "% " << returnType << " = obj." << name << "(" << args.names() << ")" << endl;
-  file.oss << "function " << returnType << " = " << name << "(obj";
-  if (args.size()) file.oss << "," << args.names();
-  file.oss << ")" << endl;
-  file.oss << "  error('need to compile " << name << ".cpp');" << endl;
-  file.oss << "end" << endl;
-
-  // close file
-  file.emit(false);
+	string output;
+	if(returnVal.isPair)
+		output = "[ r1 r2 ] = ";
+	else if(returnVal.category1 == ReturnValue::VOID)
+		output = "";
+	else
+		output = "r = ";
+	file.oss << "    function " << output << name << "(varargin)\n";
+	file.oss << "      " << output << wrapperName << "(" << id << ", varargin{:});\n";
+	file.oss << "    end\n";
 }
 
 /* ************************************************************************* */
-void Method::matlab_wrapper(const string& classPath, 
-			    const string& className,
+string Method::wrapper_fragment(FileWriter& file, 
 			    const string& cppClassName,
 			    const string& matlabClassName,
-			    const vector<string>& using_namespaces, const std::vector<std::string>& includes) const {
-  // open destination wrapperFile
-  string wrapperFile = classPath + "/" + name + ".cpp";
-  FileWriter file(wrapperFile, verbose_, "//");
+					int id,
+			    const vector<string>& using_namespaces) const {
 
   // generate code
 
-  // header
-  generateIncludes(file, className, includes);
-  generateUsingNamespace(file, using_namespaces);
+	const string wrapFunctionName = matlabClassName + "_" + name + "_" + boost::lexical_cast<string>(id);
+
+	// call
+	file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
+	// start
+	file.oss << "{\n";
+	generateUsingNamespace(file, using_namespaces);
 
   if(returnVal.isPair)
   {
       if(returnVal.category1 == ReturnValue::CLASS)
-        file.oss << "typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
+        file.oss << "  typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
       if(returnVal.category2 == ReturnValue::CLASS)
-        file.oss << "typedef boost::shared_ptr<"  << returnVal.qualifiedType2("::")  << "> Shared" <<  returnVal.type2 << ";"<< endl;
+        file.oss << "  typedef boost::shared_ptr<"  << returnVal.qualifiedType2("::")  << "> Shared" <<  returnVal.type2 << ";"<< endl;
   }
   else
       if(returnVal.category1 == ReturnValue::CLASS)
-        file.oss << "typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
+        file.oss << "  typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
 
-  file.oss << "typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
-  // call
-  file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  // start
-  file.oss << "{\n";
+  file.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
 
   // check arguments
   // extra argument obj -> nargin-1 is passed !
@@ -103,8 +96,7 @@ void Method::matlab_wrapper(const string& classPath,
   // finish
   file.oss << "}\n";
 
-  // close file
-  file.emit(true);
+	return wrapFunctionName;
 }
 
 /* ************************************************************************* */
diff --git a/wrap/Method.h b/wrap/Method.h
index bcfac82e3..60b0c1282 100644
--- a/wrap/Method.h
+++ b/wrap/Method.h
@@ -42,13 +42,12 @@ struct Method {
 	// MATLAB code generation
 	// classPath is class directory, e.g., ../matlab/@Point2
 
-	void matlab_mfile(const std::string& classPath) const; ///< m-file
-	void matlab_wrapper(const std::string& classPath,
-			const std::string& className,
+	void proxy_fragment(FileWriter& file, const std::string& wrapperName, const int id) const;
+	std::string wrapper_fragment(FileWriter& file,
 	    const std::string& cppClassName,
 	    const std::string& matlabClassname,
-	    const std::vector<std::string>& using_namespaces,
-	    const std::vector<std::string>& includes) const; ///< cpp wrapper
+			int id,
+	    const std::vector<std::string>& using_namespaces) const; ///< cpp wrapper
 };
 
 } // \namespace wrap
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 13adacc4c..2f1b1f252 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -56,7 +56,7 @@ Module::Module(const string& interfacePath,
   ArgumentList args0, args;
   vector<string> arg_dup; ///keep track of duplicates
   Constructor constructor0(enable_verbose), constructor(enable_verbose);
-  //Deconstructor deconstructor0(enable_verbose), deconstructor(enable_verbose);
+  Deconstructor deconstructor0(enable_verbose), deconstructor(enable_verbose);
   Method method0(enable_verbose), method(enable_verbose);
   StaticMethod static_method0(enable_verbose), static_method(enable_verbose);
   Class cls0(enable_verbose),cls(enable_verbose);
@@ -193,10 +193,10 @@ Module::Module(const string& interfacePath,
   		[assign_a(cls.namespaces, namespaces)]
   		 [assign_a(cls.using_namespaces, using_namespace_current)]
   		[append_a(cls.includes, namespace_includes)]
-        //[assign_a(deconstructor.name,cls.name)]
-        //[assign_a(cls.d, deconstructor)]
+        [assign_a(deconstructor.name,cls.name)]
+        [assign_a(cls.deconstructor, deconstructor)]
   		[push_back_a(classes,cls)]
-        //[assign_a(deconstructor,deconstructor0)]
+        [assign_a(deconstructor,deconstructor0)]
         [assign_a(constructor, constructor0)]
   		[assign_a(cls,cls0)];
 
@@ -299,6 +299,16 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
     string makeFileName = toolboxPath + "/Makefile";
     FileWriter makeModuleMakefile(makeFileName, verbose, "#");
 
+		// create the unified .cpp switch file
+		const string wrapperName = name + "_wrapper";
+		string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
+		FileWriter wrapperFile(wrapperFileName, verbose, "//");
+		vector<string> functionNames; // Function names stored by index for switch
+		wrapperFile.oss << "#include <wrap/matlab.h>\n";
+		wrapperFile.oss << "#include <map>\n";
+		wrapperFile.oss << "#include <boost/foreach.hpp>\n";
+		wrapperFile.oss << "\n";
+
     makeModuleMfile.oss << "echo on" << endl << endl;
     makeModuleMfile.oss << "toolboxpath = mfilename('fullpath');" << endl;
     makeModuleMfile.oss << "delims = find(toolboxpath == '/' | toolboxpath == '\\');" << endl;
@@ -333,15 +343,42 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
     }
     makeModuleMakefile.oss << "\n\n";
 
+		// Generate all includes
+		BOOST_FOREACH(Class cls, classes) {
+			generateIncludes(wrapperFile, cls.name, cls.includes);
+		}
+		wrapperFile.oss << "\n";
+
+		// Generate all collectors
+		BOOST_FOREACH(Class cls, classes) {
+			const string matlabName = cls.qualifiedName(), cppName = cls.qualifiedName("::");
+			wrapperFile.oss << "typedef std::set<boost::shared_ptr<" << cppName << ">*> "
+				<< "Collector_" << matlabName << ";\n";
+			wrapperFile.oss << "static Collector_" << matlabName <<
+				" collector_" << matlabName << ";\n";
+		}
+
+		// generate mexAtExit cleanup function
+		wrapperFile.oss << "void _deleteAllObjects()\n";
+		wrapperFile.oss << "{\n";
+		BOOST_FOREACH(Class cls, classes) {
+			const string matlabName = cls.qualifiedName();
+			const string cppName = cls.qualifiedName("::");
+			const string collectorType = "Collector_" + matlabName;
+			const string collectorName = "collector_" + matlabName;
+			wrapperFile.oss << "  for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n";
+			wrapperFile.oss << "      iter != " << collectorName << ".end(); ) {\n";
+			wrapperFile.oss << "    delete *iter;\n";
+			wrapperFile.oss << "    " << collectorName << ".erase(iter++);\n";
+			wrapperFile.oss << "  }\n";
+		}
+		wrapperFile.oss << "}\n";
+
     // generate proxy classes and wrappers
     BOOST_FOREACH(Class cls, classes) {
-      // create directory if needed
-      string classPath = toolboxPath + "/@" + cls.qualifiedName();
-      fs::create_directories(classPath);
-
       // create proxy class
-      string classFile = classPath + "/" + cls.qualifiedName() + ".m";
-      cls.matlab_proxy(classFile);
+      string classFile = toolboxPath + "/" + cls.qualifiedName() + ".m";
+      cls.matlab_proxy(classFile, wrapperName, wrapperFile, functionNames);
 
       // verify all of the function arguments
       //TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
@@ -353,12 +390,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
       verifyReturnTypes<Method>(validTypes, cls.methods);
 
       // create constructor and method wrappers
-      cls.matlab_constructors(toolboxPath);
-      cls.matlab_static_methods(toolboxPath);
-      cls.matlab_methods(classPath);
-
-      // create deconstructor
-      //cls.matlab_deconstructor(toolboxPath);
+      cls.matlab_static_methods(toolboxPath, wrapperName, wrapperFile, functionNames);
 
       // add lines to make m-file
       makeModuleMfile.oss << "%% " << cls.qualifiedName() << endl;
@@ -384,6 +416,30 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
     // finish Makefile
     makeModuleMakefile.oss << "\n" << endl;
     makeModuleMakefile.emit(true);
+
+		// finish wrapper file
+		finish_wrapper(wrapperFile, functionNames);
+
+		wrapperFile.emit(true);
   }
 
+/* ************************************************************************* */
+	void Module::finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const {
+		file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
+		file.oss << "{\n";
+		file.oss << "  mstream mout;\n"; // Send stdout to MATLAB console, see matlab.h
+		file.oss << "  std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n";
+		file.oss << "  int id = unwrap<int>(in[0]);\n\n";
+		file.oss << "  switch(id) {\n";
+		for(size_t id = 0; id < functionNames.size(); ++id) {
+			file.oss << "  case " << id << ":\n";
+			file.oss << "    " << functionNames[id] << "(nargout, out, nargin-1, in+1);\n";
+			file.oss << "    break;\n";
+		}
+		file.oss << "  }\n";
+		file.oss << "\n";
+		file.oss << "  std::cout.rdbuf(outbuf);\n"; // Restore cout, see matlab.h
+		file.oss << "}\n";
+	}
+
 /* ************************************************************************* */
diff --git a/wrap/Module.h b/wrap/Module.h
index 239aa20b0..8df6abbba 100644
--- a/wrap/Module.h
+++ b/wrap/Module.h
@@ -46,6 +46,8 @@ struct Module {
 		   const std::string& mexExt,
 		   const std::string& headerPath,
 		   const std::string& mexFlags) const;
+
+	void finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const;
 };
 
 } // \namespace wrap
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 087a1c6ef..2c1231c6b 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -49,16 +49,20 @@ string ReturnValue::qualifiedType2(const string& delim) const {
 void ReturnValue::wrap_result(FileWriter& file) const {
 	string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1();
 	string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2();
+	const string collectorName1 = "collector_" + matlabType1;
+	const string collectorName2 = "collector_" + matlabType2;
 
   if (isPair) {
   	// first return value in pair
     if (isPtr1) {// if we already have a pointer
       file.oss << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(result.first);" << endl;
-      file.oss << "  out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n";
+			file.oss << "  " << collectorName1 << ".insert(ret);\n";
+      file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n";
     }
     else if (category1 == ReturnValue::CLASS) { // if we are going to make one
-        file.oss << "  Shared" <<  type1 << "* ret = new Shared" <<  type1 << "(new " << cppType1 << "(result.first));\n";
-    	file.oss << "  out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n";
+      file.oss << "  Shared" <<  type1 << "* ret = new Shared" <<  type1 << "(new " << cppType1 << "(result.first));\n";
+			file.oss << "  " << collectorName1 << ".insert(ret);\n";
+    	file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n";
     }
     else // if basis type
       file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n";
@@ -66,22 +70,26 @@ void ReturnValue::wrap_result(FileWriter& file) const {
     // second return value in pair
     if (isPtr2) {// if we already have a pointer
       file.oss << "  Shared" << type2 <<"* ret = new Shared" << type2 << "(result.second);" << endl;
-      file.oss << "  out[1] = wrap_collect_shared_ptr(ret,\"" << matlabType2 << "\");\n";
+			file.oss << "  " << collectorName2 << ".insert(ret);\n";
+      file.oss << "  out[1] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n";
     }
     else if (category2 == ReturnValue::CLASS) { // if we are going to make one
-        file.oss << "  Shared" <<  type2 << "* ret = new Shared" <<  type2 << "(new " << cppType2 << "(result.first));\n";
-    	file.oss << "  out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType2 << "\");\n";
+      file.oss << "  Shared" <<  type2 << "* ret = new Shared" <<  type2 << "(new " << cppType2 << "(result.first));\n";
+			file.oss << "  " << collectorName2 << ".insert(ret);\n";
+    	file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n";
     }
     else
       file.oss << "  out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
   }
   else if (isPtr1){
     file.oss << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(result);" << endl;
-    file.oss << "  out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n";
+		file.oss << "  " << collectorName1 << ".insert(ret);\n";
+    file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n";
   }
   else if (category1 == ReturnValue::CLASS){
     file.oss << "  Shared" <<  type1 << "* ret = new Shared" <<  type1 << "(new " << cppType1 << "(result));\n";
-    file.oss << "  out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n";
+		file.oss << "  " << collectorName1 << ".insert(ret);\n";
+		file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n";
   }
   else if (matlabType1!="void")
     file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp
index a4fb3fbe8..8cdf0e597 100644
--- a/wrap/StaticMethod.cpp
+++ b/wrap/StaticMethod.cpp
@@ -19,6 +19,7 @@
 #include <fstream>
 
 #include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include "StaticMethod.h"
 #include "utilities.h"
@@ -27,55 +28,51 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) const {
+void StaticMethod::proxy_fragment(const string& toolboxPath, const string& matlabClassName, const std::string& wrapperName, const int id) const {
 
-  // open destination m-file
-	string full_name = className + "_" + name;
-  string wrapperFile = toolboxPath + "/" + full_name + ".m";
-  FileWriter file(wrapperFile, verbose, "%");
+	const string full_name = matlabClassName + "_" + name;
+	FileWriter file(toolboxPath + "/" + full_name + ".m", verbose, "%");
 
-  // generate code
-  string returnType = returnVal.matlab_returnType();
-  file.oss << "function " << returnType << " = " << full_name << "(";
-  if (args.size()) file.oss << args.names();
-  file.oss << ")" << endl;
-  file.oss << "% usage: x = " << full_name << "(" << args.names() << ")" << endl;
-  file.oss << "  error('need to compile " << full_name << ".cpp');" << endl;
-  file.oss << "end" << endl;
+	string output;
+	if(returnVal.isPair)
+		output = "[ r1 r2 ] = ";
+	else if(returnVal.category1 == ReturnValue::VOID)
+		output = "";
+	else
+		output = "r = ";
+	file.oss << "function " << output << full_name << "(varargin)\n";
+	file.oss << "  " << output << wrapperName << "(" << id << ", varargin{:});\n";
+	file.oss << "end\n";
 
-  // close file
-  file.emit(true);
+	file.emit(true);
 }
 
 /* ************************************************************************* */
-void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className,
+string StaticMethod::wrapper_fragment(FileWriter& file,
 		const string& matlabClassName, const string& cppClassName,
-		const vector<string>& using_namespaces,
-		const vector<string>& includes) const {
-  // open destination wrapperFile
-	string full_name = matlabClassName + "_" + name;
-  string wrapperFile = toolboxPath + "/" + full_name + ".cpp";
-  FileWriter file(wrapperFile, verbose, "//");
+		int id,	const vector<string>& using_namespaces) const {
+	
+	const string full_name = matlabClassName + "_" + name;
+	const string wrapFunctionName = matlabClassName + "_" + name + "_" + boost::lexical_cast<string>(id);
 
-  // generate code
-
-  // header
-  generateIncludes(file, className, includes);
-  generateUsingNamespace(file, using_namespaces);
+	// call
+	file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
+	// start
+	file.oss << "{\n";
+	generateUsingNamespace(file, using_namespaces);
 
   if(returnVal.isPair)
   {
-    file.oss << "typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
-    file.oss << "typedef boost::shared_ptr<"  << returnVal.qualifiedType2("::")  << "> Shared" <<  returnVal.type2 << ";"<< endl;
+      if(returnVal.category1 == ReturnValue::CLASS)
+        file.oss << "  typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
+      if(returnVal.category2 == ReturnValue::CLASS)
+        file.oss << "  typedef boost::shared_ptr<"  << returnVal.qualifiedType2("::")  << "> Shared" <<  returnVal.type2 << ";"<< endl;
   }
   else
-    file.oss << "typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
+      if(returnVal.category1 == ReturnValue::CLASS)
+        file.oss << "  typedef boost::shared_ptr<"  << returnVal.qualifiedType1("::")  << "> Shared" <<  returnVal.type1 << ";"<< endl;
 
-  file.oss << "typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
-  // call
-  file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  // start
-  file.oss << "{\n";
+  file.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
 
   // check arguments
   // NOTE: for static functions, there is no object passed
@@ -98,8 +95,7 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& class
   // finish
   file.oss << "}\n";
 
-  // close file
-  file.emit(true);
+	return wrapFunctionName;
 }
 
 /* ************************************************************************* */
diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h
index f9cc14af7..3baa58585 100644
--- a/wrap/StaticMethod.h
+++ b/wrap/StaticMethod.h
@@ -44,12 +44,13 @@ struct StaticMethod {
 	// NOTE: static functions are not inside the class, and
 	// are created with [ClassName]_[FunctionName]() format
 
-	void matlab_mfile(const std::string& toolboxPath, const std::string& className) const; ///< m-file
-	void matlab_wrapper(const std::string& toolboxPath,
-			const std::string& className, const std::string& matlabClassName,
+	void proxy_fragment(const std::string& toolboxPath, const std::string& matlabClassName,
+		const std::string& wrapperName, const int id) const; ///< m-file
+	std::string wrapper_fragment(FileWriter& file,
+			const std::string& matlabClassName,
 			const std::string& cppClassName,
-			const std::vector<std::string>& using_namespaces,
-			const std::vector<std::string>& includes) const; ///< cpp wrapper
+			int id,
+			const std::vector<std::string>& using_namespaces) const; ///< cpp wrapper
 };
 
 } // \namespace wrap
diff --git a/wrap/matlab.h b/wrap/matlab.h
index 31e29c806..dc13eafad 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -1,372 +1,387 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file matlab.h
- * @brief header file to be included in MATLAB wrappers
- * @date 2008
- * @author Frank Dellaert
- *
- * wrapping and unwrapping is done using specialized templates, see
- * http://www.cplusplus.com/doc/tutorial/templates.html
- */
-
-#include <gtsam/base/Vector.h>
-#include <gtsam/base/Matrix.h>
-
-using gtsam::Vector;
-using gtsam::Matrix;
-
-extern "C" {
-#include <mex.h>
-}
-
-#include <boost/shared_ptr.hpp>
-#include <boost/make_shared.hpp>
-#include <boost/foreach.hpp>
-
-#include <list>
-#include <string>
-#include <sstream>
-#include <typeinfo>
-#include <set>
-
-using namespace std;
-using namespace boost; // not usual, but for conciseness of generated code
-
-// start GTSAM Specifics /////////////////////////////////////////////////
-// to enable Matrix and Vector constructor for SharedGaussian:
-#define GTSAM_MAGIC_GAUSSIAN
-// end GTSAM Specifics /////////////////////////////////////////////////
-
-#if defined(__LP64__) || defined(_WIN64)
-// 64-bit
-#define mxUINT32OR64_CLASS mxUINT64_CLASS
-#else
-#define mxUINT32OR64_CLASS mxUINT32_CLASS
-#endif
-
-//*****************************************************************************
-// Utilities
-//*****************************************************************************
-
-void error(const char* str) {
-  mexErrMsgIdAndTxt("wrap:error", str);
-}
-
-mxArray *scalar(mxClassID classid) {
-  mwSize dims[1]; dims[0]=1;
-  return mxCreateNumericArray(1, dims, classid, mxREAL);
-}
-
-void checkScalar(const mxArray* array, const char* str) {
-	int m = mxGetM(array), n = mxGetN(array);
-	if (m!=1 || n!=1)
-		mexErrMsgIdAndTxt("wrap: not a scalar in ", str);
-}
-
-//*****************************************************************************
-// Check arguments
-//*****************************************************************************
-
-void checkArguments(const string& name, int nargout, int nargin, int expected) {
-  stringstream err; 
-  err << name << " expects " << expected << " arguments, not " << nargin;
-  if (nargin!=expected)
-    error(err.str().c_str());
-}
-
-//*****************************************************************************
-// wrapping C++ basis types in MATLAB arrays
-//*****************************************************************************
-
-// default wrapping throws an error: only basis types are allowed in wrap
-template <typename Class>
-mxArray* wrap(Class& value) {
-  error("wrap internal error: attempted wrap of invalid type");
-}
-
-// specialization to string
-// wraps into a character array
-template<>
-mxArray* wrap<string>(string& value) {
-  return mxCreateString(value.c_str());
-}
-
-// specialization to char
-template<>
-mxArray* wrap<char>(char& value) {
-  mxArray *result = scalar(mxUINT32OR64_CLASS);
-  *(char*)mxGetData(result) = value;
-  return result;
-}
-
-// specialization to unsigned char
-template<>
-mxArray* wrap<unsigned char>(unsigned char& value) {
-  mxArray *result = scalar(mxUINT32OR64_CLASS);
-  *(unsigned char*)mxGetData(result) = value;
-  return result;
-}
-
-// specialization to bool
-template<>
-mxArray* wrap<bool>(bool& value) {
-  mxArray *result = scalar(mxUINT32OR64_CLASS);
-  *(bool*)mxGetData(result) = value;
-  return result;
-}
-
-// specialization to size_t
-template<>
-mxArray* wrap<size_t>(size_t& value) {
-  mxArray *result = scalar(mxUINT32OR64_CLASS);
-  *(size_t*)mxGetData(result) = value;
-  return result;
-}
-
-// specialization to int
-template<>
-mxArray* wrap<int>(int& value) {
-  mxArray *result = scalar(mxUINT32OR64_CLASS);
-  *(int*)mxGetData(result) = value;
-  return result;
-}
-
-// specialization to double -> just double
-template<>
-mxArray* wrap<double>(double& value) {
-  return mxCreateDoubleScalar(value);
-}
-
-// wrap a const Eigen vector into a double vector
-mxArray* wrap_Vector(const gtsam::Vector& v) {
-  int m = v.size();
-  mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL);
-  double *data = mxGetPr(result);
-  for (int i=0;i<m;i++) data[i]=v(i);
-  return result;
-}
-
-// specialization to Eigen vector -> double vector
-template<>
-mxArray* wrap<gtsam::Vector >(gtsam::Vector& v) {
-  return wrap_Vector(v);
-}
-
-// const version
-template<>
-mxArray* wrap<const gtsam::Vector >(const gtsam::Vector& v) {
-  return wrap_Vector(v);
-}
-
-// wrap a const Eigen MATRIX into a double matrix
-mxArray* wrap_Matrix(const gtsam::Matrix& A) {
-  int m = A.rows(), n = A.cols();
-#ifdef DEBUG_WRAP
-  mexPrintf("wrap_Matrix called with A = \n", m,n);
-  gtsam::print(A);
-#endif
-  mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL);
-  double *data = mxGetPr(result);
-  // converts from column-major to row-major
-  for (int j=0;j<n;j++) for (int i=0;i<m;i++,data++) *data = A(i,j);
-	return result;
-}
-
-// specialization to Eigen MATRIX -> double matrix
-template<>
-mxArray* wrap<gtsam::Matrix >(gtsam::Matrix& A) {
-  return wrap_Matrix(A);
-}
-
-// const version
-template<>
-mxArray* wrap<const gtsam::Matrix >(const gtsam::Matrix& A) {
-  return wrap_Matrix(A);
-}
-
-//*****************************************************************************
-// unwrapping MATLAB arrays into C++ basis types
-//*****************************************************************************
-
-// default unwrapping throws an error
-// as wrap only supports passing a reference or one of the basic types
-template <typename T>
-T unwrap(const mxArray* array) {
-  error("wrap internal error: attempted unwrap of invalid type");
-}
-
-// specialization to string
-// expects a character array
-// Warning: relies on mxChar==char
-template<>
-string unwrap<string>(const mxArray* array) {
-  char *data = mxArrayToString(array);
-  if (data==NULL) error("unwrap<string>: not a character array");
-  string str(data);
-  mxFree(data);
-  return str;
-}
-
-// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit
-template <typename T>
-T myGetScalar(const mxArray* array) {
-	switch (mxGetClassID(array)) {
-		case mxINT64_CLASS:
-			return (T) *(int64_t*) mxGetData(array);
-		case mxUINT64_CLASS:
-			return (T) *(uint64_t*) mxGetData(array);
-		default:
-			// hope for the best!
-			return (T) mxGetScalar(array);
-	}
-}
-
-// specialization to bool
-template<>
-bool unwrap<bool>(const mxArray* array) {
-	checkScalar(array,"unwrap<bool>");
-  return myGetScalar<bool>(array);
-}
-
-// specialization to char
-template<>
-char unwrap<char>(const mxArray* array) {
-	checkScalar(array,"unwrap<char>");
-  return myGetScalar<char>(array);
-}
-
-// specialization to unsigned char
-template<>
-unsigned char unwrap<unsigned char>(const mxArray* array) {
-	checkScalar(array,"unwrap<unsigned char>");
-  return myGetScalar<unsigned char>(array);
-}
-
-// specialization to int
-template<>
-int unwrap<int>(const mxArray* array) {
-	checkScalar(array,"unwrap<int>");
-  return myGetScalar<int>(array);
-}
-
-// specialization to size_t
-template<>
-size_t unwrap<size_t>(const mxArray* array) {
-	checkScalar(array, "unwrap<size_t>");
-  return myGetScalar<size_t>(array);
-}
-
-// specialization to double
-template<>
-double unwrap<double>(const mxArray* array) {
-	checkScalar(array,"unwrap<double>");
-  return myGetScalar<double>(array);
-}
-
-// specialization to Eigen vector
-template<>
-gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) {
-  int m = mxGetM(array), n = mxGetN(array);
-  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
-#ifdef DEBUG_WRAP
-  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
-#endif
-  double* data = (double*)mxGetData(array);
-  gtsam::Vector v(m);
-  for (int i=0;i<m;i++,data++) v(i) = *data;
-#ifdef DEBUG_WRAP
-	gtsam::print(v);
-#endif
-  return v;
-}
-
-// specialization to Eigen matrix
-template<>
-gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
-  if (mxIsDouble(array)==false) error("unwrap<matrix>: not a matrix");
-  int m = mxGetM(array), n = mxGetN(array);
-#ifdef DEBUG_WRAP
-  mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n);
-#endif
-  double* data = (double*)mxGetData(array);
-  gtsam::Matrix A(m,n);
-  // converts from row-major to column-major
-  for (int j=0;j<n;j++) for (int i=0;i<m;i++,data++) A(i,j) = *data;
-#ifdef DEBUG_WRAP
-	gtsam::print(A);
-#endif
-  return A;
-}
-
-/*
- [create_object] creates a MATLAB proxy class object with a mexhandle
- in the self property. Matlab does not allow the creation of matlab
- objects from within mex files, hence we resort to an ugly trick: we
- invoke the proxy class constructor by calling MATLAB, and pass 13
- dummy arguments to let the constructor know we want an object without
- the self property initialized. We then assign the mexhandle to self.
-*/
-mxArray* create_object(const char *classname, mxArray* h) {
-  mxArray *result;
-  mxArray* dummy[13] = {h,h,h,h,h, h,h,h,h,h, h,h,h};
-  mexCallMATLAB(1,&result,13,dummy,classname);
-  mxSetProperty(result, 0, "self", h);
-  return result;
-}
-
-/*
- * Similar to create object, this also collects the shared_ptr in addition
- * to creating the dummy object. Mainly used for static constructor methods
- * which don't have direct access to the function.
- */
-mxArray* create_collect_object(const char *classname, mxArray* h){
-  mxArray *result;
-  //First arg is a flag to collect
-  mxArray* dummy[14] = {h,h,h,h,h, h,h,h,h,h, h,h,h,h};
-  mexCallMATLAB(1,&result,14,dummy,classname);
-  mxSetProperty(result, 0, "self", h);
-  return result;
-}
-
-/*
- When the user calls a method that returns a shared pointer, we create
- an ObjectHandle from the shared_pointer and return it as a proxy
- class to matlab.
-*/
-template <typename Class>
-mxArray* wrap_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) {
-  mxArray* mxh = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh)) = shared_ptr;
-  //return mxh;
-  return create_object(classname, mxh);
-}
-
-template <typename Class>
-mxArray* wrap_collect_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) {
-  mxArray* mxh = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
-  *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh)) = shared_ptr;
-  //return mxh;
-  return create_collect_object(classname, mxh);
-}
-
-template <typename Class>
-boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) {
-
-  mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh)
-    || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error(
-    "Parameter is not an Shared type.");
-
-  boost::shared_ptr<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
-  return *spp;
-}
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file matlab.h
+ * @brief header file to be included in MATLAB wrappers
+ * @date 2008
+ * @author Frank Dellaert
+ *
+ * wrapping and unwrapping is done using specialized templates, see
+ * http://www.cplusplus.com/doc/tutorial/templates.html
+ */
+
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/Matrix.h>
+
+using gtsam::Vector;
+using gtsam::Matrix;
+
+extern "C" {
+#include <mex.h>
+}
+
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <boost/foreach.hpp>
+
+#include <list>
+#include <string>
+#include <sstream>
+#include <typeinfo>
+#include <set>
+#include <streambuf>
+
+using namespace std;
+using namespace boost; // not usual, but for conciseness of generated code
+
+// start GTSAM Specifics /////////////////////////////////////////////////
+// to enable Matrix and Vector constructor for SharedGaussian:
+#define GTSAM_MAGIC_GAUSSIAN
+// end GTSAM Specifics /////////////////////////////////////////////////
+
+#if defined(__LP64__) || defined(_WIN64)
+// 64-bit
+#define mxUINT32OR64_CLASS mxUINT64_CLASS
+#else
+#define mxUINT32OR64_CLASS mxUINT32_CLASS
+#endif
+
+// "Unique" key to signal calling the matlab object constructor with a raw pointer
+// Also present in Class.cpp
+static const uint64_t ptr_constructor_key =
+	(uint64_t('G') << 56) |
+	(uint64_t('T') << 48) |
+	(uint64_t('S') << 40) |
+	(uint64_t('A') << 32) |
+	(uint64_t('M') << 24) |
+	(uint64_t('p') << 16) |
+	(uint64_t('t') << 8) |
+	(uint64_t('r'));
+
+//*****************************************************************************
+// Utilities
+//*****************************************************************************
+
+void error(const char* str) {
+  mexErrMsgIdAndTxt("wrap:error", str);
+}
+
+mxArray *scalar(mxClassID classid) {
+  mwSize dims[1]; dims[0]=1;
+  return mxCreateNumericArray(1, dims, classid, mxREAL);
+}
+
+void checkScalar(const mxArray* array, const char* str) {
+	int m = mxGetM(array), n = mxGetN(array);
+	if (m!=1 || n!=1)
+		mexErrMsgIdAndTxt("wrap: not a scalar in ", str);
+}
+
+// Replacement streambuf for cout that writes to the MATLAB console
+// Thanks to http://stackoverflow.com/a/249008
+class mstream : public std::streambuf {
+protected:
+	virtual std::streamsize xsputn(const char *s, std::streamsize n) {
+		mexPrintf("%.*s",n,s);
+		return n;
+	}
+	virtual int overflow(int c = EOF) {
+		if (c != EOF) {
+			mexPrintf("%.1s",&c);
+		}
+		return 1;
+	}
+};
+
+//*****************************************************************************
+// Check arguments
+//*****************************************************************************
+
+void checkArguments(const string& name, int nargout, int nargin, int expected) {
+  stringstream err; 
+  err << name << " expects " << expected << " arguments, not " << nargin;
+  if (nargin!=expected)
+    error(err.str().c_str());
+}
+
+//*****************************************************************************
+// wrapping C++ basis types in MATLAB arrays
+//*****************************************************************************
+
+// default wrapping throws an error: only basis types are allowed in wrap
+template <typename Class>
+mxArray* wrap(Class& value) {
+  error("wrap internal error: attempted wrap of invalid type");
+}
+
+// specialization to string
+// wraps into a character array
+template<>
+mxArray* wrap<string>(string& value) {
+  return mxCreateString(value.c_str());
+}
+
+// specialization to char
+template<>
+mxArray* wrap<char>(char& value) {
+  mxArray *result = scalar(mxUINT32OR64_CLASS);
+  *(char*)mxGetData(result) = value;
+  return result;
+}
+
+// specialization to unsigned char
+template<>
+mxArray* wrap<unsigned char>(unsigned char& value) {
+  mxArray *result = scalar(mxUINT32OR64_CLASS);
+  *(unsigned char*)mxGetData(result) = value;
+  return result;
+}
+
+// specialization to bool
+template<>
+mxArray* wrap<bool>(bool& value) {
+  mxArray *result = scalar(mxUINT32OR64_CLASS);
+  *(bool*)mxGetData(result) = value;
+  return result;
+}
+
+// specialization to size_t
+template<>
+mxArray* wrap<size_t>(size_t& value) {
+  mxArray *result = scalar(mxUINT32OR64_CLASS);
+  *(size_t*)mxGetData(result) = value;
+  return result;
+}
+
+// specialization to int
+template<>
+mxArray* wrap<int>(int& value) {
+  mxArray *result = scalar(mxUINT32OR64_CLASS);
+  *(int*)mxGetData(result) = value;
+  return result;
+}
+
+// specialization to double -> just double
+template<>
+mxArray* wrap<double>(double& value) {
+  return mxCreateDoubleScalar(value);
+}
+
+// wrap a const Eigen vector into a double vector
+mxArray* wrap_Vector(const gtsam::Vector& v) {
+  int m = v.size();
+  mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL);
+  double *data = mxGetPr(result);
+  for (int i=0;i<m;i++) data[i]=v(i);
+  return result;
+}
+
+// specialization to Eigen vector -> double vector
+template<>
+mxArray* wrap<gtsam::Vector >(gtsam::Vector& v) {
+  return wrap_Vector(v);
+}
+
+// const version
+template<>
+mxArray* wrap<const gtsam::Vector >(const gtsam::Vector& v) {
+  return wrap_Vector(v);
+}
+
+// wrap a const Eigen MATRIX into a double matrix
+mxArray* wrap_Matrix(const gtsam::Matrix& A) {
+  int m = A.rows(), n = A.cols();
+#ifdef DEBUG_WRAP
+  mexPrintf("wrap_Matrix called with A = \n", m,n);
+  gtsam::print(A);
+#endif
+  mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL);
+  double *data = mxGetPr(result);
+  // converts from column-major to row-major
+  for (int j=0;j<n;j++) for (int i=0;i<m;i++,data++) *data = A(i,j);
+	return result;
+}
+
+// specialization to Eigen MATRIX -> double matrix
+template<>
+mxArray* wrap<gtsam::Matrix >(gtsam::Matrix& A) {
+  return wrap_Matrix(A);
+}
+
+// const version
+template<>
+mxArray* wrap<const gtsam::Matrix >(const gtsam::Matrix& A) {
+  return wrap_Matrix(A);
+}
+
+//*****************************************************************************
+// unwrapping MATLAB arrays into C++ basis types
+//*****************************************************************************
+
+// default unwrapping throws an error
+// as wrap only supports passing a reference or one of the basic types
+template <typename T>
+T unwrap(const mxArray* array) {
+  error("wrap internal error: attempted unwrap of invalid type");
+}
+
+// specialization to string
+// expects a character array
+// Warning: relies on mxChar==char
+template<>
+string unwrap<string>(const mxArray* array) {
+  char *data = mxArrayToString(array);
+  if (data==NULL) error("unwrap<string>: not a character array");
+  string str(data);
+  mxFree(data);
+  return str;
+}
+
+// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit
+template <typename T>
+T myGetScalar(const mxArray* array) {
+	switch (mxGetClassID(array)) {
+		case mxINT64_CLASS:
+			return (T) *(int64_t*) mxGetData(array);
+		case mxUINT64_CLASS:
+			return (T) *(uint64_t*) mxGetData(array);
+		default:
+			// hope for the best!
+			return (T) mxGetScalar(array);
+	}
+}
+
+// specialization to bool
+template<>
+bool unwrap<bool>(const mxArray* array) {
+	checkScalar(array,"unwrap<bool>");
+  return myGetScalar<bool>(array);
+}
+
+// specialization to char
+template<>
+char unwrap<char>(const mxArray* array) {
+	checkScalar(array,"unwrap<char>");
+  return myGetScalar<char>(array);
+}
+
+// specialization to unsigned char
+template<>
+unsigned char unwrap<unsigned char>(const mxArray* array) {
+	checkScalar(array,"unwrap<unsigned char>");
+  return myGetScalar<unsigned char>(array);
+}
+
+// specialization to int
+template<>
+int unwrap<int>(const mxArray* array) {
+	checkScalar(array,"unwrap<int>");
+  return myGetScalar<int>(array);
+}
+
+// specialization to size_t
+template<>
+size_t unwrap<size_t>(const mxArray* array) {
+	checkScalar(array, "unwrap<size_t>");
+  return myGetScalar<size_t>(array);
+}
+
+// specialization to double
+template<>
+double unwrap<double>(const mxArray* array) {
+	checkScalar(array,"unwrap<double>");
+  return myGetScalar<double>(array);
+}
+
+// specialization to Eigen vector
+template<>
+gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) {
+  int m = mxGetM(array), n = mxGetN(array);
+  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
+#ifdef DEBUG_WRAP
+  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
+#endif
+  double* data = (double*)mxGetData(array);
+  gtsam::Vector v(m);
+  for (int i=0;i<m;i++,data++) v(i) = *data;
+#ifdef DEBUG_WRAP
+	gtsam::print(v);
+#endif
+  return v;
+}
+
+// specialization to Eigen matrix
+template<>
+gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
+  if (mxIsDouble(array)==false) error("unwrap<matrix>: not a matrix");
+  int m = mxGetM(array), n = mxGetN(array);
+#ifdef DEBUG_WRAP
+  mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n);
+#endif
+  double* data = (double*)mxGetData(array);
+  gtsam::Matrix A(m,n);
+  // converts from row-major to column-major
+  for (int j=0;j<n;j++) for (int i=0;i<m;i++,data++) A(i,j) = *data;
+#ifdef DEBUG_WRAP
+	gtsam::print(A);
+#endif
+  return A;
+}
+
+/*
+ [create_object] creates a MATLAB proxy class object with a mexhandle
+ in the self property. Matlab does not allow the creation of matlab
+ objects from within mex files, hence we resort to an ugly trick: we
+ invoke the proxy class constructor by calling MATLAB, and pass 13
+ dummy arguments to let the constructor know we want an object without
+ the self property initialized. We then assign the mexhandle to self.
+*/
+mxArray* create_object(const char *classname, void *pointer) {
+  mxArray *result;
+	mxArray *input[2];
+	// First input argument is pointer constructor key
+	input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL);
+	*reinterpret_cast<uint64_t*>(mxGetData(input[0])) = ptr_constructor_key;
+	// Second input argument is the pointer
+  input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+	*reinterpret_cast<void**>(mxGetData(input[1])) = pointer;
+	// Call special pointer constructor, which sets 'self'
+  mexCallMATLAB(1,&result,2,input,classname);
+  // Deallocate our memory
+	mxDestroyArray(input[0]);
+	mxDestroyArray(input[1]);
+  return result;
+}
+
+/*
+ When the user calls a method that returns a shared pointer, we create
+ an ObjectHandle from the shared_pointer and return it as a proxy
+ class to matlab.
+*/
+template <typename Class>
+mxArray* wrap_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) {
+	// Create actual class object from out pointer
+  mxArray* result = create_object(classname, shared_ptr);
+	return result;
+}
+
+template <typename Class>
+boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) {
+
+  mxArray* mxh = mxGetProperty(obj,0,"self");
+  if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh)
+    || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error(
+    "Parameter is not an Shared type.");
+
+  boost::shared_ptr<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
+  return *spp;
+}
diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp
index 9d56ec5cd..31b65bdcc 100644
--- a/wrap/utilities.cpp
+++ b/wrap/utilities.cpp
@@ -123,7 +123,6 @@ void generateUsingNamespace(FileWriter& file, const vector<string>& using_namesp
 /* ************************************************************************* */
 void generateIncludes(FileWriter& file, const string& class_name,
 		const vector<string>& includes) {
-	file.oss << "#include <wrap/matlab.h>" << endl;
 	bool added_include = false;
 	BOOST_FOREACH(const string& s, includes) {
 		if (!s.empty()) {