diff --git a/.cproject b/.cproject
index c593a801b..7828f8165 100644
--- a/.cproject
+++ b/.cproject
@@ -2325,4 +2325,5 @@
 			</target>
 		</buildTargets>
 	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
 </cproject>
diff --git a/gtsam.h b/gtsam.h
index 68ed6201d..941e39af7 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -497,7 +497,7 @@ class SimpleCamera {
 //*************************************************************************
 // inference
 //*************************************************************************
-class Permutation {
+/*class Permutation {
 	// Standard Constructors and Named Constructors
 	Permutation();
 	Permutation(size_t nVars);
@@ -511,12 +511,12 @@ class Permutation {
 	bool equals(const gtsam::Permutation& rhs, double tol) const;
 
 	// Standard interface
-	size_t at(size_t variable) const;
+	//size_t at(size_t variable) const;
 	size_t size() const;
 	bool empty() const;
 	void resize(size_t newSize);
-	gtsam::Permutation* permute(const gtsam::Permutation& permutation) const;
-	gtsam::Permutation* inverse() const;
+	//gtsam::Permutation permute(const gtsam::Permutation& permutation) const;
+	//gtsam::Permutation inverse() const;
 };
 
 class IndexFactor {
@@ -541,24 +541,24 @@ class IndexFactor {
 
 class IndexConditional {
   // Standard Constructors and Named Constructors
-  IndexConditional();
-  IndexConditional(size_t key);
-  IndexConditional(size_t key, size_t parent);
-  IndexConditional(size_t key, size_t parent1, size_t parent2);
-  IndexConditional(size_t key, size_t parent1, size_t parent2, size_t parent3);
+  //IndexConditional();
+  //IndexConditional(size_t key);
+  //IndexConditional(size_t key, size_t parent);
+  //IndexConditional(size_t key, size_t parent1, size_t parent2);
+  //IndexConditional(size_t key, size_t parent1, size_t parent2, size_t parent3);
   // FIXME: Must wrap std::vector<KeyType> for this to work
   //IndexFactor(size_t key, const std::vector<KeyType>& parents);
   //IndexConditional(const std::vector<Index>& keys, size_t nrFrontals);
   //template<class KEYS> static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals);
 
   // Testable
-  void print(string s) const;
-  bool equals(const gtsam::IndexConditional& other, double tol) const;
+  //void print(string s) const;
+  //bool equals(const gtsam::IndexConditional& other, double tol) const;
 
   // Standard interface
-  size_t nrFrontals() const;
-  size_t nrParents() const;
-  gtsam::IndexFactor* toFactor() const;
+  //size_t nrFrontals() const;
+  //size_t nrParents() const;
+  //gtsam::IndexFactor* toFactor() const;
 };
 
 #include <gtsam/inference/SymbolicFactorGraph.h>
@@ -672,7 +672,7 @@ class VariableIndex {
   size_t nFactors() const;
   size_t nEntries() const;
   void permute(const gtsam::Permutation& permutation);
-};
+};*/
 
 //*************************************************************************
 // linear
@@ -681,11 +681,12 @@ class VariableIndex {
 #include <gtsam/linear/NoiseModel.h>
 namespace noiseModel {
 class Base {
+	void print(string s) const;
 };
 
 class Gaussian {
-	static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
-	static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
+	//static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
+	//static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
 //	Matrix R() const;		// FIXME: cannot parse!!!
 	void print(string s) const;
 };
@@ -712,7 +713,7 @@ class Unit {
 }///\namespace noiseModel
 
 
-class Sampler {
+/*class Sampler {
 	Sampler(gtsam::noiseModel::Diagonal* model, int seed);
 	Sampler(Vector sigmas, int seed);
 	Sampler(int seed);
@@ -723,7 +724,7 @@ class Sampler {
 
 	Vector sample();
 	Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
-};
+};*/
 
 class VectorValues {
 	VectorValues();
@@ -897,11 +898,11 @@ class Ordering {
 	// Standard interface
 	size_t nVars() const;
   size_t size() const;
-  size_t at(size_t key) const;
+  size_t at(size_t key);
   bool exists(size_t key) const;
   void insert(size_t key, size_t order);
   void push_back(size_t key);
-  void permuteWithInverse(const gtsam::Permutation& inversePermutation);
+  //void permuteWithInverse(const gtsam::Permutation& inversePermutation);
   // FIXME: Wrap InvertedMap as well
   //InvertedMap invert() const;
 };
@@ -958,9 +959,9 @@ class Marginals {
 
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 class LevenbergMarquardtParams {
-  LevenbergMarquardtParams();
+  /*LevenbergMarquardtParams();
   LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose);
-  void print(string s) const;
+  void print(string s) const;*/
   double getlambdaInitial() const ;
   double getlambdaFactor() const ;
   double getlambdaUpperBound() const;
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index c67828822..9d39b5674 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -262,7 +262,7 @@ namespace gtsam {
 			 * A diagonal noise model created by specifying a Vector of sigmas, i.e.
 			 * standard devations, the diagonal of the square root covariance matrix.
 			 */
-			static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
+			static shared_ptr Sigmas(const Vector& sigmas, bool smart = false);
 
 			/**
 			 * A diagonal noise model created by specifying a Vector of variances, i.e.
diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m
index f203c858e..8b2439c78 100644
--- a/matlab/examples/OdometryExample.m
+++ b/matlab/examples/OdometryExample.m
@@ -29,6 +29,7 @@ odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x
 graph.addRelativePose(1, 2, odometry, odometryNoise);
 graph.addRelativePose(2, 3, odometry, odometryNoise);
 
+pause
 %% print
 graph.print(sprintf('\nFactor graph:\n'));
 
@@ -56,4 +57,4 @@ for i=1:result.size()
 end
 axis([-0.6 4.8 -1 1])
 axis equal
-view(2)
\ No newline at end of file
+view(2)
diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
index f51b86864..e2073ac8c 100644
--- a/wrap/Argument.cpp
+++ b/wrap/Argument.cpp
@@ -12,6 +12,7 @@
 /**
  * @file Argument.ccp
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #include <iostream>
@@ -53,14 +54,14 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
 		file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< ";
 	else if (is_ref)
 		// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
-		file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";
+		file.oss << "    " << cppType << "& " << name << " = *unwrap_shared_ptr< ";
 	else
 		// Not a pointer or a reference: emit an "unwrap" call
 		// unwrap is specified in matlab.h as a series of template specializations
 		// that know how to unpack the expected MATLAB object
 		// example: double tol = unwrap< double >(in[2]);
 		// example: Vector v = unwrap< Vector >(in[1]);
-		file.oss << cppType << " " << name << " = unwrap< ";
+		file.oss << "    " << cppType << " " << name << " = unwrap< ";
 
 	file.oss << cppType << " >(" << matlabName;
   if (is_ptr || is_ref) file.oss << ", \"" << matlabType << "\"";
@@ -126,3 +127,4 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const {
 }
 
 /* ************************************************************************* */
+
diff --git a/wrap/Argument.h b/wrap/Argument.h
index 000e3079a..b4c3fe4a7 100644
--- a/wrap/Argument.h
+++ b/wrap/Argument.h
@@ -13,6 +13,7 @@
  * @file Argument.h
  * @brief arguments to constructors and methods
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #pragma once
@@ -65,6 +66,7 @@ struct ArgumentList: public std::vector<Argument> {
 	 * @param start initial index for input array, set to 1 for method
 	 */
 	void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++
+
 };
 
 } // \namespace wrap
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index 6f6f91b0a..7280d5488 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -10,8 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Class.ccp
+ * @file Class.cpp
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #include <vector>
@@ -22,6 +23,7 @@
 
 #include "Class.h"
 #include "utilities.h"
+#include "Argument.h"
 
 using namespace std;
 using namespace wrap;
@@ -43,13 +45,24 @@ void Class::matlab_proxy(const string& classFile) const {
   file.oss << "  methods" << endl;
   // constructor
   file.oss << "    function obj = " << matlabName << "(varargin)" << endl;
-  BOOST_FOREACH(Constructor c, constructors)
-    c.matlab_proxy_fragment(file,matlabName);
-  file.oss << "      if nargin ~= 13 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl;
+  //i is constructor id
+  int id = 0;
+  BOOST_FOREACH(ArgumentList a, constructor.args_list)
+  {
+    constructor.matlab_proxy_fragment(file,matlabName, id, a);
+    id++;
+  }
+  //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 << "       delete_" << matlabName << "(obj);" << endl;
+  file.oss << "      if obj.self ~= 0" << endl;
+  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;
@@ -61,18 +74,19 @@ void Class::matlab_proxy(const string& classFile) const {
 }
 
 /* ************************************************************************* */
+//TODO: Consolidate into single file
 void Class::matlab_constructors(const string& toolboxPath) const {
-  BOOST_FOREACH(Constructor c, constructors) {
-    c.matlab_mfile  (toolboxPath, qualifiedName());
-    c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes);
+  /*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_deconstructor(const string& toolboxPath) const {
-    d.matlab_mfile  (toolboxPath, qualifiedName());
-    d.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes);
-}
 /* ************************************************************************* */
 void Class::matlab_methods(const string& classPath) const {
 	string matlabName = qualifiedName(), cppName = qualifiedName("::");
@@ -97,9 +111,7 @@ void Class::matlab_make_fragment(FileWriter& file,
 				 const string& mexFlags) const {
   string mex = "mex " + mexFlags + " ";
   string matlabClassName = qualifiedName();
-  BOOST_FOREACH(Constructor c, constructors)
-    file.oss << mex << c.matlab_wrapper_name(matlabClassName) << ".cpp" << endl;
-  file.oss << mex << d.matlab_wrapper_name(matlabClassName) << ".cpp" << endl;
+  file.oss << mex << constructor.matlab_wrapper_name(matlabClassName) << ".cpp" << endl;
   BOOST_FOREACH(StaticMethod sm, static_methods)
     file.oss << mex << matlabClassName + "_" + sm.name << ".cpp" << endl;
   file.oss << endl << "cd @" << matlabClassName << endl;
@@ -123,15 +135,12 @@ void Class::makefile_fragment(FileWriter& file) const {
 //
 //	Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING)
 
-	string matlabName = qualifiedName();
+  string matlabName = qualifiedName();
 
-	// collect names
-	vector<string> file_names;
-  BOOST_FOREACH(Constructor c, constructors) {
-  	string file_base = c.matlab_wrapper_name(matlabName);
-  	file_names.push_back(file_base);
-  }
-  file_names.push_back(d.matlab_wrapper_name(matlabName));
+  // collect names
+  vector<string> file_names;
+  string file_base = constructor.matlab_wrapper_name(matlabName);
+  file_names.push_back(file_base);
   BOOST_FOREACH(StaticMethod c, static_methods) {
   	string file_base = matlabName + "_" + c.name;
   	file_names.push_back(file_base);
diff --git a/wrap/Class.h b/wrap/Class.h
index 875b3203e..fb9906716 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -13,6 +13,7 @@
  * @file Class.h
  * @brief describe the C++ class that is being wrapped
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #pragma once
@@ -33,19 +34,17 @@ struct Class {
 
 	// Then the instance variables are set directly by the Module constructor
   std::string name;                         ///< Class name
-  std::vector<Constructor> constructors;    ///< Class constructors
   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> includes;        ///< header include overrides
-  Deconstructor d;
+  Constructor constructor;    ///< Class constructors
   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_deconstructor(const std::string& toolboxPath) const;
   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_make_fragment(FileWriter& file,
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
index fda7db1b4..0ee459389 100644
--- a/wrap/Constructor.cpp
+++ b/wrap/Constructor.cpp
@@ -12,10 +12,12 @@
 /**
  * @file Constructor.ccp
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #include <iostream>
 #include <fstream>
+#include <algorithm>
 
 #include <boost/foreach.hpp>
 
@@ -25,14 +27,16 @@
 using namespace std;
 using namespace wrap;
 
+
 /* ************************************************************************* */
 string Constructor::matlab_wrapper_name(const string& className) const {
-  string str = "new_" + className + "_" + args.signature();
+  string str = "new_" + className + "_";
   return str;
 }
 
 /* ************************************************************************* */
-void Constructor::matlab_proxy_fragment(FileWriter& file, const string& className) const {
+void Constructor::matlab_proxy_fragment(FileWriter& file, 
+        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;
@@ -45,19 +49,17 @@ void Constructor::matlab_proxy_fragment(FileWriter& file, const string& classNam
     first=false;
   }
   // emit code for calling constructor
-  file.oss << "), obj.self = " << matlab_wrapper_name(className) << "(";
+  file.oss << "), obj.self = " << matlab_wrapper_name(className) << "(" << "0," << id;
   // emit constructor arguments
-  first = true;
   for(size_t i=0;i<nrArgs;i++) {
-    if (!first) file.oss << ",";
+    file.oss << ",";
     file.oss << "varargin{" << i+1 << "}";
-    first=false;
   }
   file.oss << "); end" << endl;
 }
 
 /* ************************************************************************* */
-void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) const {
+void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName, const ArgumentList args) const {
 
   string matlabName = matlab_wrapper_name(qualifiedMatlabName);
 
@@ -80,7 +82,8 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifie
 void Constructor::matlab_wrapper(const string& toolboxPath,
 				 const string& cppClassName,
 				 const string& matlabClassName,
-				 const vector<string>& using_namespaces, const vector<string>& includes) const {
+				 const vector<string>& using_namespaces, 
+				 const vector<string>& includes) const {
   string matlabName = matlab_wrapper_name(matlabClassName);
 
   // open destination wrapperFile
@@ -91,12 +94,66 @@ void Constructor::matlab_wrapper(const string& toolboxPath,
   generateIncludes(file, name, includes);
   generateUsingNamespace(file, using_namespaces);
 
+  //Typedef boost::shared_ptr
+  file.oss << "typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
+  file.oss << endl;
+
+  //Generate collector
+  file.oss << "static std::set<Shared*> collector;" << endl;
+  file.oss << endl;
+
+  //Generate the destructor function
+  file.oss << "struct Destruct" << endl;
+  file.oss << "{" << endl;
+  file.oss << "  void operator() (Shared* p)" << endl;
+  file.oss << "  {" << endl;
+  file.oss << "    delete p;" << endl;
+  file.oss << "  }" << endl;
+  file.oss << "};" << endl;
+  file.oss << endl;
+
+  //Generate cleanup function
+  file.oss << "void cleanup(void) {" << endl;
+  file.oss << "  mexPrintf(\"MEX-file new_gtsamDummy_ is terminating\");" << endl;
+  file.oss << "  std::for_each( collector.begin(), collector.end(), Destruct() );" << endl;
+  file.oss << "}" << endl;
+
   file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
   file.oss << "{" << endl;
-  file.oss << "  checkArguments(\"" << matlabName << "\",nargout,nargin," << args.size() << ");" << endl;
-  args.matlab_unwrap(file); // unwrap arguments
-  file.oss << "  " << cppClassName << "* self = new " << cppClassName << "(" << args.names() << ");" << endl; // need qualified name, delim: "::"
-  file.oss << "  out[0] = wrap_constructed(self,\"" << matlabClassName << "\");" << endl; // need matlab qualified name
+  //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;
+  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;
+  file.oss << "    mexPrintf(\"MEX-file constructor\");" << endl << endl;;
+
+  int i = 0;
+  BOOST_FOREACH(ArgumentList al, args_list)
+  {
+    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;
+    i++;
+  }
+  
+  //file.oss << "    self = construct(nc, in);" << endl;
+  file.oss << "    collector.insert(self);" << endl;
+  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;
+
   file.oss << "}" << endl;
 
   // close file
diff --git a/wrap/Constructor.h b/wrap/Constructor.h
index 4aba5b984..a766905b6 100644
--- a/wrap/Constructor.h
+++ b/wrap/Constructor.h
@@ -33,7 +33,8 @@ struct Constructor {
 	}
 
 	// Then the instance variables are set directly by the Module constructor
-	ArgumentList args;
+	// TODO:Vector of argument lists?
+    std::vector<ArgumentList> args_list;
 	std::string name;
 	bool verbose_;
 
@@ -48,11 +49,14 @@ 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;
+	void matlab_proxy_fragment(FileWriter& file, 
+	        const std::string& className, const int i,
+	        const ArgumentList args) const;
 
 	/// m-file
 	void matlab_mfile(const std::string& toolboxPath,
-			const std::string& qualifiedMatlabName) const;
+			const std::string& qualifiedMatlabName,
+			const ArgumentList args) const;
 
 	/// cpp wrapper
 	void matlab_wrapper(const std::string& toolboxPath,
@@ -60,7 +64,12 @@ struct Constructor {
 			 const std::string& matlabClassName,
 			 const std::vector<std::string>& using_namespaces,
 			 const std::vector<std::string>& includes) const;
+
+	/// constructor function
+	void generate_construct(FileWriter& file, const std::string& cppClassName,
+	        std::vector<ArgumentList>& args_list) const;
+	
 };
 
-} // \namespace wrap
 
+} // \namespace wrap
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index 7686ecd2d..ccd36fe50 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -61,6 +61,15 @@ void Method::matlab_wrapper(const string& classPath,
   generateIncludes(file, className, includes);
   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;
+  }
+  else
+    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
@@ -73,9 +82,9 @@ void Method::matlab_wrapper(const string& classPath,
 
   // get class pointer
   // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
-  file.oss << "  boost::shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName
-      << " >(in[0],\"" << matlabClassName << "\");" << endl;
-
+  file.oss << "  mxArray* mxh = mxGetProperty(in[0],0,\"self\");" << endl;
+  file.oss << "  Shared* self = *reinterpret_cast<Shared**> (mxGetPr(mxh));" << endl; 
+  file.oss << "  Shared obj = *self;" << endl;
   // unwrap arguments, see Argument.cpp
   args.matlab_unwrap(file,1);
 
@@ -84,7 +93,7 @@ void Method::matlab_wrapper(const string& classPath,
   file.oss << "  ";
   if (returnVal.type1!="void")
     file.oss << returnVal.return_type(true,ReturnValue::pair) << " result = ";
-  file.oss << "self->" << name << "(" << args.names() << ");\n";
+  file.oss << "obj->" << name << "(" << args.names() << ");\n";
 
   // wrap result
   // example: out[0]=wrap<bool>(result);
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index e0771351a..972bacbee 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -26,8 +26,10 @@
 #include <boost/spirit/include/classic_clear_actor.hpp>
 #include <boost/foreach.hpp>
 #include <boost/filesystem.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include <iostream>
+#include <algorithm>
 
 using namespace std;
 using namespace wrap;
@@ -52,8 +54,9 @@ Module::Module(const string& interfacePath,
 	ReturnValue retVal0, retVal;
   Argument arg0, arg;
   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);
@@ -118,11 +121,12 @@ Module::Module(const string& interfacePath,
 
   Rule constructor_p = 
     (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p)
-    [assign_a(constructor.args,args)]
-    [assign_a(constructor.name,cls.name)]
-    [assign_a(args,args0)]
-    [push_back_a(cls.constructors, constructor)]
-    [assign_a(constructor,constructor0)];
+    [push_back_a(constructor.args_list, args)]
+    [assign_a(args,args0)];
+    //[assign_a(constructor.args,args)]
+    //[assign_a(constructor.name,cls.name)]
+    //[push_back_a(cls.constructors, constructor)]
+    //[assign_a(constructor,constructor0)];
 
   Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::");
 
@@ -184,13 +188,16 @@ Module::Module(const string& interfacePath,
       >> '{'
   		>> *(functions_p | comments_p)
   		>> str_p("};"))
+        [assign_a(constructor.name, cls.name)]
+        [assign_a(cls.constructor, constructor)]
   		[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.d, deconstructor)]
   		[push_back_a(classes,cls)]
-        [assign_a(deconstructor,deconstructor0)]
+        //[assign_a(deconstructor,deconstructor0)]
+        [assign_a(constructor, constructor0)]
   		[assign_a(cls,cls0)];
 
 	Rule namespace_def_p =
@@ -336,7 +343,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
       cls.matlab_proxy(classFile);
 
       // verify all of the function arguments
-      verifyArguments<Constructor>(validTypes, cls.constructors);
+      //TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
       verifyArguments<StaticMethod>(validTypes, cls.static_methods);
       verifyArguments<Method>(validTypes, cls.methods);
 
@@ -350,7 +357,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
       cls.matlab_methods(classPath);
 
       // create deconstructor
-      cls.matlab_deconstructor(toolboxPath);
+      //cls.matlab_deconstructor(toolboxPath);
 
       // add lines to make m-file
       makeModuleMfile.oss << "%% " << cls.qualifiedName() << endl;
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index ace1159ec..087a1c6ef 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -3,6 +3,7 @@
  *
  * @date Dec 1, 2011
  * @author Alex Cunningham
+ * @author Andrew Melim
  */
 
 #include <boost/foreach.hpp>
@@ -17,11 +18,11 @@ using namespace wrap;
 string ReturnValue::return_type(bool add_ptr, pairing p) const {
   if (p==pair && isPair) {
     string str = "pair< " +
-    		maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::")) + ", " +
-      maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::")) + " >";
+    		maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::"), type1) + ", " +
+      maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::"), type2) + " >";
     return str;
   } else
-    return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"));
+    return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1);
 }
 
 /* ************************************************************************* */
@@ -44,31 +45,44 @@ string ReturnValue::qualifiedType2(const string& delim) const {
 }
 
 /* ************************************************************************* */
+//TODO:Fix this
 void ReturnValue::wrap_result(FileWriter& file) const {
 	string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1();
 	string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2();
 
   if (isPair) {
   	// first return value in pair
-    if (isPtr1) // if we already have a pointer
-      file.oss << "  out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n";
-    else if (category1 == ReturnValue::CLASS) // if we are going to make one
-    	file.oss << "  out[0] = wrap_shared_ptr(boost::make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n";
+    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";
+    }
+    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";
+    }
     else // if basis type
       file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n";
 
     // second return value in pair
-    if (isPtr2) // if we already have a pointer
-      file.oss << "  out[1] = wrap_shared_ptr(result.second,\"" << matlabType2 << "\");\n";
-    else if (category2 == ReturnValue::CLASS) // if we are going to make one
-    	file.oss << "  out[1] = wrap_shared_ptr(boost::make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n";
+    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";
+    }
+    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";
+    }
     else
       file.oss << "  out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
   }
-  else if (isPtr1)
-    file.oss << "  out[0] = wrap_shared_ptr(result,\"" << matlabType1 << "\");\n";
-  else if (category1 == ReturnValue::CLASS)
-  	file.oss << "  out[0] = wrap_shared_ptr(boost::make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\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";
+  }
+  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";
+  }
   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 7bdba650b..a4fb3fbe8 100644
--- a/wrap/StaticMethod.cpp
+++ b/wrap/StaticMethod.cpp
@@ -10,8 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Method.ccp
+ * @file StaticMethod.ccp
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #include <iostream>
@@ -62,6 +63,15 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& class
   generateIncludes(file, className, includes);
   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;
+  }
+  else
+    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
diff --git a/wrap/matlab.h b/wrap/matlab.h
index 313cfaa9f..99dd0c6b3 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -41,6 +41,8 @@ extern "C" {
 #include <list>
 #include <string>
 #include <sstream>
+#include <typeinfo>
+#include <set>
 
 using namespace std;
 using namespace boost; // not usual, but for conciseness of generated code
@@ -311,6 +313,77 @@ gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
   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.
+*/
+// TODO: think about memory
+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);
+  cout << "Return collect" << endl;
+  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>**> (mxGetPr(mxh)) = shared_ptr;
+  cout << "wrapped:" << mxh << endl << "end wrap" << endl;
+  //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>**> (mxGetPr(mxh)) = shared_ptr;
+  cout << "wrapped:" << mxh << endl << "end wrap" << endl;
+  //return mxh;
+  return create_collect_object(classname, mxh);
+}
+
+template <typename Class>
+boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) {
+    cout << "UNWRAP CALL" << endl;
+
+  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.");
+
+  cout << "unwrapped:" << mxh << endl;
+  boost::shared_ptr<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetPr(mxh));
+  cout << "unwrapped:" << spp << endl;
+  return *spp;
+}
+
+
 //*****************************************************************************
 // Shared Pointer Handle
 // inspired by mexhandle, but using shared_ptr
@@ -329,6 +402,9 @@ public:
 	ObjectHandle(T* ptr) :
 		type(&typeid(T)), t(ptr) {
 		signature = this;
+		mexPrintf("Created Shared Pointer use_count = %li\n", t.use_count());
+		mexPrintf("Created Pointer points to %d\n", t.get());
+		
 	}
 
 	// Constructor for shared pointers
@@ -336,6 +412,8 @@ public:
 	ObjectHandle(boost::shared_ptr<T> shared_ptr) :
 		/*type(&typeid(T)),*/ t(shared_ptr) {
 		signature = this;
+		mexPrintf("Created sp from sp use_count = %li\n", t.use_count());
+		mexPrintf("Created sp from sp points to %d\n", t.get());
 	}
 
 	~ObjectHandle() {
@@ -418,33 +496,7 @@ mxArray* wrap_constructed(Class* pointer, const char *classname) {
   return handle->to_mex_handle();
 }
 
-/*
- [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.
-*/
-// TODO: think about memory
-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;
-}
 
-/*
- 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) {
-  ObjectHandle<Class>* handle = new ObjectHandle<Class>(shared_ptr);
-  return create_object(classname,handle->to_mex_handle());
-}
 
 //*****************************************************************************
 // unwrapping a MATLAB proxy class to a C++ object reference
@@ -457,116 +509,11 @@ mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const char *clas
  the ObjectHandle from the self property, and returns a shared pointer
  to the object.
 */
-template <typename Class>
-boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) {
-	#ifndef UNSAFE_WRAP
-  // Useful code to check argument type
-	// Problem, does not support inheritance
-  bool isClass = mxIsClass(obj, className.c_str());
-  if (!isClass) {
-    mexPrintf("Expected %s, got %s\n", className.c_str(), mxGetClassName(obj));
-    error("Argument has wrong type.");
-  }
-#endif
-  mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxh==NULL) error("unwrap_reference: invalid wrap object");
-  ObjectHandle<Class>* handle = ObjectHandle<Class>::from_mex_handle(mxh);
-  return handle->get_object();
-}
 
-/*
- * Specialized template for noise model. Checking their derived types properly
- */
-// Isotropic
-template <>
-boost::shared_ptr<Isotropic> unwrap_shared_ptr(const mxArray* obj, const string& className) {
-#ifndef UNSAFE_WRAP
-  bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
-  bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
-  if (!isIsotropic && !isUnit) {
-    mexPrintf("Expected gtsamnoiseModelIsotropic or derived classes, got %s\n", mxGetClassName(obj));
-    error("Argument has wrong type.");
-  }
-#endif
-  mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxh==NULL) error("unwrap_reference: invalid wrap object");
-  ObjectHandle<Isotropic>* handle = ObjectHandle<Isotropic>::from_mex_handle(mxh);
-  return handle->get_object();
-}
-
-// Diagonal
-template <>
-boost::shared_ptr<Diagonal> unwrap_shared_ptr(const mxArray* obj, const string& className) {
-#ifndef UNSAFE_WRAP
-	bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal");
-  bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
-  bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
-  if (!isDiagonal && !isIsotropic && !isUnit ) {
-    mexPrintf("Expected gtsamnoiseModelDiagonal or derived classes, got %s\n", mxGetClassName(obj));
-    error("Argument has wrong type.");
-  }
-#endif
-  mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxh==NULL) error("unwrap_reference: invalid wrap object");
-  ObjectHandle<Isotropic>* handle = ObjectHandle<Isotropic>::from_mex_handle(mxh);
-  return handle->get_object();
-}
-
-// Gaussian
-template <>
-boost::shared_ptr<Gaussian> unwrap_shared_ptr(const mxArray* obj, const string& className) {
-#ifndef UNSAFE_WRAP
-	bool isGaussian = mxIsClass(obj, "gtsamnoiseModelGaussian");
-	bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal");
-  bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
-  bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
-  if (!isGaussian && !isDiagonal && !isIsotropic && !isUnit) {
-    mexPrintf("Expected gtsamnoiseModelGaussian or derived classes, got %s\n", mxGetClassName(obj));
-    error("Argument has wrong type.");
-  }
-#endif
-  mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxh==NULL) error("unwrap_reference: invalid wrap object");
-  ObjectHandle<Isotropic>* handle = ObjectHandle<Isotropic>::from_mex_handle(mxh);
-  return handle->get_object();
-}
-
-// Base
-template <>
-boost::shared_ptr<Base> unwrap_shared_ptr(const mxArray* obj, const string& className) {
-#ifndef UNSAFE_WRAP
-	bool isBase = mxIsClass(obj, "gtsamnoiseModelBase");
-	bool isGaussian = mxIsClass(obj, "gtsamnoiseModelGaussian");
-	bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal");
-  bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
-  bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
-  if (!isBase && !isGaussian && !isDiagonal && !isIsotropic && !isUnit) {
-    mexPrintf("Expected gtsamnoiseModelBase or derived classes, got %s\n", mxGetClassName(obj));
-    error("Argument has wrong type.");
-  }
-#endif
-  mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxh==NULL) error("unwrap_reference: invalid wrap object");
-  ObjectHandle<Isotropic>* handle = ObjectHandle<Isotropic>::from_mex_handle(mxh);
-  return handle->get_object();
-}
-
-//end specialized templates
 
 template <typename Class>
 void delete_shared_ptr(const mxArray* obj, const string& className) {
-    //Why is this here?
-#ifndef UNSAFE_WRAP
-  bool isClass = true;//mxIsClass(obj, className.c_str());
-  if (!isClass) {
-    mexPrintf("Expected %s, got %s\n", className.c_str(), mxGetClassName(obj));
-    error("Argument has wrong type.");
-  }
-#endif
   mxArray* mxh = mxGetProperty(obj,0,"self");
-  if (mxh==NULL) error("unwrap_reference: invalid wrap object");
   ObjectHandle<Class>* handle = ObjectHandle<Class>::from_mex_handle(mxh);
   delete handle;
 }
-
-//*****************************************************************************
diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp
index bfd1e77df..00606f930 100644
--- a/wrap/utilities.cpp
+++ b/wrap/utilities.cpp
@@ -12,6 +12,7 @@
 /**
  * @file utilities.ccp
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #include <iostream>
@@ -103,10 +104,12 @@ bool files_equal(const string& expected, const string& actual, bool skipheader)
 }
 
 /* ************************************************************************* */
-string maybe_shared_ptr(bool add, const string& type) {
-  string str = add? "boost::shared_ptr<" : "";
-  str += type;
-  if (add) str += ">";
+string maybe_shared_ptr(bool add, const string& qtype, const string& type) {
+  string str = add? "Shared" : "";
+  if (add) str += type; 
+  else str += qtype;
+
+  //if (add) str += ">";
   return str;
 }
 
@@ -121,6 +124,7 @@ 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;
+	file.oss << "#include <set>" << endl;
 	bool added_include = false;
 	BOOST_FOREACH(const string& s, includes) {
 		if (!s.empty()) {
@@ -134,4 +138,5 @@ void generateIncludes(FileWriter& file, const string& class_name,
 
 /* ************************************************************************* */
 
+
 } // \namespace wrap
diff --git a/wrap/utilities.h b/wrap/utilities.h
index 384ff3749..1eb0e249f 100644
--- a/wrap/utilities.h
+++ b/wrap/utilities.h
@@ -10,8 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file utilities.ccp
+ * @file utilities.h
  * @author Frank Dellaert
+ * @author Andrew Melim
  **/
 
 #pragma once
@@ -84,7 +85,7 @@ bool assert_equal(const std::string& expected, const std::string& actual);
 bool assert_equal(const std::vector<std::string>& expected, const std::vector<std::string>& actual);
 
 // auxiliary function to wrap an argument into a shared_ptr template
-std::string maybe_shared_ptr(bool add, const std::string& type);
+std::string maybe_shared_ptr(bool add, const std::string& qtype, const std::string& type);
 
 /**
  * Creates the "using namespace [name];" declarations