diff --git a/gtsam.h b/gtsam.h
index 90cbe7bdf..46ea37241 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -14,16 +14,19 @@
  *     - void
  *     - Any class with which be copied with boost::make_shared()
  *     - boost::shared_ptr of any object type
- *   Methods cannot return
- *     - const references of anything
- *   Limitations on methods
- *     - Parsing does not support overloading
- *     - There can only be one method (static or otherwise) with a given name
+<*   Constructors
+ *     - Overloads are supported
+ *     - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
+ *   Methods
  *     - Constness has no effect
- *   Methods must start with a lowercase letter
- *   Static methods must start with a letter (upper or lowercase) and use the "static" keyword
- *     - The function generated by wrap will start with an uppercase letter, regardless
- *   Arguments to functions any of
+ *     - Specify by-value (not reference) return types, even if C++ method returns reference
+ *     - Must start with a lowercase letter
+ *     - Overloads are supported
+ *   Static methods
+ *     - Must start with a letter (upper or lowercase) and use the "static" keyword
+ *     - The first letter will be made uppercase in the generated MATLAB interface
+ *     - Overloads are supported
+=*   Arguments to functions any of
  *   	 - Eigen types:       Matrix, Vector
  *   	 - Eigen types and classes as an optionally const reference
  *     - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char
@@ -47,15 +50,24 @@
  *   	 - All namespaces must have angle brackets: <path>
  *   	 - To override, add a full include statement just before the class statement
  *   	 - An override include can be added for a namespace by placing it just before the namespace statement
- *   Overriding type dependency checks
+ *   	 - Both classes and namespace accept exactly one namespace
+ *   Using classes defined in other modules
  *     - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error
+ *   Virtual inheritance
+ *     - Specify fully-qualified base classes, i.e. "virtual class Derived : module::Base {"
+ *     - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : module::Base {"
+ *     - Forward declarations must also be marked virtual, e.g. "virtual class module::Base;" and
+ *       also "virtual class module::Derived;"
+ *     - Pure virtual (abstract) classes should list no constructors in this interface file
+ *     - Virtual classes must have a clone() function in C++ (though it does not have to be included
+ *       in the MATLAB interface).  clone() will be called whenever an object copy is needed, instead
+ *       of using the copy constructor (which is used for non-virtual objects).
  */
 
 /**
  * Status:
  *  - TODO: global functions
  *  - TODO: default values for arguments
- *  - TODO: signatures for constructors can be ambiguous if two types have the same first letter
  *  - TODO: Handle gtsam::Rot3M conversions to quaternions
  */
 
@@ -65,7 +77,17 @@ namespace gtsam {
 // base
 //*************************************************************************
 
-class LieVector {
+virtual class Value {
+	// No constructors because this is an abstract class
+
+	// Testable
+	void print(string s) const;
+
+	// Manifold
+	size_t dim() const;
+};
+
+virtual class LieVector : gtsam::Value {
 	// Standard constructors
 	LieVector();
 	LieVector(Vector v);
@@ -97,7 +119,7 @@ class LieVector {
 // geometry
 //*************************************************************************
 
-class Point2 {
+virtual class Point2 : gtsam::Value {
   // Standard Constructors
 	Point2();
 	Point2(double x, double y);
@@ -129,7 +151,7 @@ class Point2 {
   Vector vector() const;
 };
 
-class StereoPoint2 {
+virtual class StereoPoint2 : gtsam::Value {
   // Standard Constructors
   StereoPoint2();
   StereoPoint2(double uL, double uR, double v);
@@ -158,7 +180,7 @@ class StereoPoint2 {
   Vector vector() const;
 };
 
-class Point3 {
+virtual class Point3 : gtsam::Value {
   // Standard Constructors
 	Point3();
 	Point3(double x, double y, double z);
@@ -191,7 +213,7 @@ class Point3 {
 	double z() const;
 };
 
-class Rot2 {
+virtual class Rot2 : gtsam::Value {
   // Standard Constructors and Named Constructors
 	Rot2();
 	Rot2(double theta);
@@ -233,7 +255,7 @@ class Rot2 {
   Matrix matrix() const;
 };
 
-class Rot3 {
+virtual class Rot3 : gtsam::Value {
   // Standard Constructors and Named Constructors
 	Rot3();
 	Rot3(Matrix R);
@@ -285,7 +307,7 @@ class Rot3 {
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
 };
 
-class Pose2 {
+virtual class Pose2 : gtsam::Value {
   // Standard Constructor
 	Pose2();
 	Pose2(double x, double y, double theta);
@@ -331,7 +353,7 @@ class Pose2 {
   Matrix matrix() const;
 };
 
-class Pose3 {
+virtual class Pose3 : gtsam::Value {
 	// Standard Constructors
 	Pose3();
 	Pose3(const gtsam::Pose3& pose);
@@ -379,7 +401,7 @@ class Pose3 {
 	double range(const gtsam::Pose3& pose); // FIXME: shadows other range
 };
 
-class Cal3_S2 {
+virtual class Cal3_S2 : gtsam::Value {
   // Standard Constructors
   Cal3_S2();
   Cal3_S2(double fx, double fy, double s, double u0, double v0);
@@ -429,7 +451,7 @@ class Cal3_S2Stereo {
   double baseline() const;
 };
 
-class CalibratedCamera {
+virtual class CalibratedCamera : gtsam::Value {
   // Standard Constructors and Named Constructors
 	CalibratedCamera();
 	CalibratedCamera(const gtsam::Pose3& pose);
@@ -459,7 +481,7 @@ class CalibratedCamera {
   double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
 };
 
-class SimpleCamera {
+virtual class SimpleCamera : gtsam::Value {
   // Standard Constructors and Named Constructors
 	SimpleCamera();
   SimpleCamera(const gtsam::Pose3& pose);
@@ -683,14 +705,14 @@ namespace noiseModel {
 class Base {
 };
 
-class Gaussian {
+class Gaussian : gtsam::noiseModel::Base {
 	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;
 };
 
-class Diagonal {
+class Diagonal : gtsam::noiseModel::Gaussian {
 	static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
 	static gtsam::noiseModel::Diagonal* Variances(Vector variances);
 	static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
@@ -698,14 +720,14 @@ class Diagonal {
 	void print(string s) const;
 };
 
-class Isotropic {
+class Isotropic : gtsam::noiseModel::Gaussian {
 	static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
 	static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
 	static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
 	void print(string s) const;
 };
 
-class Unit {
+class Unit : gtsam::noiseModel::Gaussian {
 	static gtsam::noiseModel::Unit* Create(size_t dim);
 	void print(string s) const;
 };
@@ -944,7 +966,9 @@ class Values {
 	Values();
 	size_t size() const;
 	void print(string s) const;
+	void insert(size_t j, const gtsam::Value& value);
 	bool exists(size_t j) const;
+	gtsam::Value at(size_t j) const;
 };
 
 // Actually a FastList<Key>
diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
index 6c10a29f5..4f6dae9c7 100644
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@@ -16,7 +16,7 @@
  */
 
 #pragma once
-
+#include <boost/make_shared.hpp>
 #include <boost/pool/singleton_pool.hpp>
 #include <gtsam/base/Value.h>
 
@@ -37,7 +37,8 @@ public:
 
   /**
    * Create a duplicate object returned as a pointer to the generic Value interface.
-   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
+   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
+	 * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
    */
   virtual Value* clone_() const {
   	void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
@@ -53,6 +54,13 @@ public:
   	boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
   }
 
+	/**
+	 * Clone this value (normal clone on the heap, delete with 'delete' operator)
+	 */
+	virtual boost::shared_ptr<Value> clone() const {
+		return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
+	}
+
   /// equals implementing generic Value interface
   virtual bool equals_(const Value& p, double tol = 1e-9) const {
     // Cast the base class Value pointer to a derived class pointer
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 394a11e72..8b19a6532 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -101,12 +101,15 @@ namespace gtsam {
   class Value {
   public:
 
-    /** Allocate and construct a clone of this value */
+    /** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */
     virtual Value* clone_() const = 0;
 
     /** Deallocate a raw pointer of this value */
     virtual void deallocate_() const = 0;
 
+		/** Clone this value (normal clone on the heap, delete with 'delete' operator) */
+		virtual boost::shared_ptr<Value> clone() const = 0;
+
     /** Compare this Value with another for equality. */
     virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
 
diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
index 40ad205b6..35f20169b 100644
--- a/wrap/Argument.cpp
+++ b/wrap/Argument.cpp
@@ -64,7 +64,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
 		file.oss << cppType << " " << name << " = unwrap< ";
 
 	file.oss << cppType << " >(" << matlabName;
-  if (is_ptr || is_ref) file.oss << ", \"" << matlabType << "\"";
+  if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabType << "\"";
   file.oss << ");" << endl;
 }
 
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index 84e83a9f5..803017e42 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -1,168 +1,177 @@
-/* ----------------------------------------------------------------------------
-
- * 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 Class.cpp
- * @author Frank Dellaert
- * @author Andrew Melim
- **/
-
-#include <vector>
-#include <iostream>
-#include <fstream>
-#include <stdint.h>
-
-#include <boost/foreach.hpp>
-#include <boost/lexical_cast.hpp>
-
-#include "Class.h"
-#include "utilities.h"
-#include "Argument.h"
-
-using namespace std;
-using namespace wrap;
-
-/* ************************************************************************* */
-void Class::matlab_proxy(const string& classFile, const string& wrapperName, FileWriter& wrapperFile, vector<string>& functionNames) const {
-  // open destination classFile
-  FileWriter proxyFile(classFile, verbose_, "%");
-
-  // get the name of actual matlab object
-  const string matlabName = qualifiedName(), cppName = qualifiedName("::");
-
-  // emit class proxy code
-  // we want our class to inherit the handle class for memory purposes
-  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 constructors - one in MATLAB to create an object and
-	// assign a pointer returned from a C++ function.  In turn this MATLAB
-	// constructor calls a special C++ function that just adds the object to
-	// its collector.  This allows wrapped functions to return objects in
-	// other wrap modules - to add these to their collectors the pointer is
-	// passed from one C++ module into matlab then back into the other C++
-	// module.
-	{
-		int id = functionNames.size();
-		const string functionName = pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, id);
-		functionNames.push_back(functionName);
-	}
-	// Regular constructors
-  BOOST_FOREACH(ArgumentList a, constructor.args_list)
-  {
-		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);
-  }
-  proxyFile.oss << "      else\n";
-	proxyFile.oss << "        error('Arguments do not match any overload of " << matlabName << " constructor');" << endl;
-	proxyFile.oss << "      end\n";
-  proxyFile.oss << "    end\n\n";
-
-	// 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(const Methods::value_type& name_m, methods) {
-		const Method& m = name_m.second;
-		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, functionNames);
-		proxyFile.oss << "\n";
-		wrapperFile.oss << "\n";
-	}
-
-	proxyFile.oss << "  end\n";
-	proxyFile.oss << "\n";
-	proxyFile.oss << "  methods(Static = true)\n";
-
-	// Static methods
-	BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
-		const StaticMethod& m = name_m.second;
-		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, functionNames);
-		proxyFile.oss << "\n";
-		wrapperFile.oss << "\n";
-	}
-
-	proxyFile.oss << "  end" << endl;
-	proxyFile.oss << "end" << endl;
-
-  // Close file
-  proxyFile.emit(true);
-}
-
-/* ************************************************************************* */
-string Class::qualifiedName(const string& delim) const {
-	string result;
-	BOOST_FOREACH(const string& ns, namespaces)
-		result += ns + delim;
-	return result + name;
-}
-
-/* ************************************************************************* */
-string Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, int id) const {
-	
-	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'));
-
-  const string matlabName = qualifiedName(), cppName = qualifiedName("::");
-	const string wrapFunctionName = matlabName + "_constructor_" + boost::lexical_cast<string>(id);
-
-	// MATLAB constructor that assigns pointer to matlab object then calls c++
-	// function to add the object to the collector.
-	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";
-	proxyFile.oss << "        " << wrapperName << "(obj.self);\n";
-
-	// C++ function to add pointer from MATLAB to collector.  The pointer always
-	// comes from a C++ return value; this mechanism allows the object to be added
-	// to a collector in a different wrap module.
-  wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
-  wrapperFile.oss << "{\n";
-	wrapperFile.oss << "  mexAtExit(&_deleteAllObjects);\n";
-	generateUsingNamespace(wrapperFile, using_namespaces);
-  // Typedef boost::shared_ptr
-	wrapperFile.oss << "  typedef boost::shared_ptr<"  << cppName << "> Shared;\n";
-	wrapperFile.oss << "\n";
-	// Get self pointer passed in
-	wrapperFile.oss << "  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
-	// Add to collector
-	wrapperFile.oss << "  collector_" << matlabName << ".insert(self);\n";
-	wrapperFile.oss << "}\n";
-
-	return wrapFunctionName;
-}
-
-/* ************************************************************************* */
+/* ----------------------------------------------------------------------------
+
+ * 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 Class.cpp
+ * @author Frank Dellaert
+ * @author Andrew Melim
+ **/
+
+#include <vector>
+#include <iostream>
+#include <fstream>
+#include <cstdint>
+
+#include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
+
+#include "Class.h"
+#include "utilities.h"
+#include "Argument.h"
+
+using namespace std;
+using namespace wrap;
+
+/* ************************************************************************* */
+void Class::matlab_proxy(const string& classFile, const string& wrapperName,
+												 const ReturnValue::TypeAttributesTable& typeAttributes,
+												 FileWriter& wrapperFile, vector<string>& functionNames) const {
+  // open destination classFile
+  FileWriter proxyFile(classFile, verbose_, "%");
+
+  // get the name of actual matlab object
+  const string matlabName = qualifiedName(), cppName = qualifiedName("::");
+	const string matlabBaseName = wrap::qualifiedName("", qualifiedParent);
+	const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
+
+  // emit class proxy code
+  // we want our class to inherit the handle class for memory purposes
+	const string parent = qualifiedParent.empty() ?
+		"handle" : ::wrap::qualifiedName("", qualifiedParent);
+  proxyFile.oss << "classdef " << matlabName << " < " << parent << endl;
+  proxyFile.oss << "  properties" << endl;
+  proxyFile.oss << "    ptr_" << matlabName << " = 0" << endl;
+  proxyFile.oss << "  end" << endl;
+  proxyFile.oss << "  methods" << endl;
+
+  // Constructor
+  proxyFile.oss << "    function obj = " << matlabName << "(varargin)" << endl;
+  // Special pointer constructors - one in MATLAB to create an object and
+	// assign a pointer returned from a C++ function.  In turn this MATLAB
+	// constructor calls a special C++ function that just adds the object to
+	// its collector.  This allows wrapped functions to return objects in
+	// other wrap modules - to add these to their collectors the pointer is
+	// passed from one C++ module into matlab then back into the other C++
+	// module.
+	{
+		int id = functionNames.size();
+		const string functionName = pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, id);
+		functionNames.push_back(functionName);
+	}
+	// Regular constructors
+  BOOST_FOREACH(ArgumentList a, constructor.args_list)
+  {
+		const int id = functionNames.size();
+		constructor.proxy_fragment(proxyFile, wrapperName, matlabName, matlabBaseName, id, a);
+		const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
+			cppName, matlabName, cppBaseName, id, using_namespaces, a);
+		wrapperFile.oss << "\n";
+    functionNames.push_back(wrapFunctionName);
+  }
+  proxyFile.oss << "      else\n";
+	proxyFile.oss << "        error('Arguments do not match any overload of " << matlabName << " constructor');" << endl;
+	proxyFile.oss << "      end\n";
+	if(!qualifiedParent.empty())
+		proxyFile.oss << "      obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n";
+	proxyFile.oss << "      obj.ptr_" << matlabName << " = my_ptr;\n";
+  proxyFile.oss << "    end\n\n";
+
+	// 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);
+		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(const Methods::value_type& name_m, methods) {
+		const Method& m = name_m.second;
+		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, typeAttributes, functionNames);
+		proxyFile.oss << "\n";
+		wrapperFile.oss << "\n";
+	}
+
+	proxyFile.oss << "  end\n";
+	proxyFile.oss << "\n";
+	proxyFile.oss << "  methods(Static = true)\n";
+
+	// Static methods
+	BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
+		const StaticMethod& m = name_m.second;
+		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, typeAttributes, functionNames);
+		proxyFile.oss << "\n";
+		wrapperFile.oss << "\n";
+	}
+
+	proxyFile.oss << "  end" << endl;
+	proxyFile.oss << "end" << endl;
+
+  // Close file
+  proxyFile.emit(true);
+}
+
+/* ************************************************************************* */
+string Class::qualifiedName(const string& delim) const {
+	return ::wrap::qualifiedName(delim, namespaces, name);
+}
+
+/* ************************************************************************* */
+string Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, int id) const {
+
+  const string matlabName = qualifiedName(), cppName = qualifiedName("::");
+	const string wrapFunctionName = matlabName + "_collectorInsertAndMakeBase_" + boost::lexical_cast<string>(id);
+	const string baseMatlabName = wrap::qualifiedName("", qualifiedParent);
+	const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
+
+	// MATLAB constructor that assigns pointer to matlab object then calls c++
+	// function to add the object to the collector.
+	proxyFile.oss << "      if nargin == 2 && isa(varargin{1}, 'uint64') && ";
+	proxyFile.oss << "varargin{1} == uint64(" << ptr_constructor_key << ")\n";
+	proxyFile.oss << "        my_ptr = varargin{2};\n";
+	if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back
+		proxyFile.oss << "        ";
+	else
+		proxyFile.oss << "        base_ptr = ";
+	proxyFile.oss << wrapperName << "(" << id << ", my_ptr);\n"; // Call collector insert and get base class ptr
+
+	// C++ function to add pointer from MATLAB to collector.  The pointer always
+	// comes from a C++ return value; this mechanism allows the object to be added
+	// to a collector in a different wrap module.  If this class has a base class,
+	// a new pointer to the base class is allocated and returned.
+  wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
+  wrapperFile.oss << "{\n";
+	wrapperFile.oss << "  mexAtExit(&_deleteAllObjects);\n";
+	generateUsingNamespace(wrapperFile, using_namespaces);
+  // Typedef boost::shared_ptr
+	wrapperFile.oss << "  typedef boost::shared_ptr<" << cppName << "> Shared;\n";
+	wrapperFile.oss << "\n";
+	// Get self pointer passed in
+	wrapperFile.oss << "  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
+	// Add to collector
+	wrapperFile.oss << "  collector_" << matlabName << ".insert(self);\n";
+	// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
+	if(!qualifiedParent.empty()) {
+		wrapperFile.oss << "\n";
+		wrapperFile.oss << "  typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n";
+    wrapperFile.oss << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
+		wrapperFile.oss << "  *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);\n";
+	}
+	wrapperFile.oss << "}\n";
+
+	return wrapFunctionName;
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Class.h b/wrap/Class.h
index f1a745d94..5703f8353 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -34,10 +34,12 @@ struct Class {
 	typedef std::map<std::string, StaticMethod> StaticMethods;
 
   /// Constructor creates an empty class
-  Class(bool verbose=true) : verbose_(verbose) {}
+  Class(bool verbose=true) : verbose_(verbose), isVirtual(false) {}
 
 	// Then the instance variables are set directly by the Module constructor
   std::string name;                         ///< Class name
+	bool isVirtual;                           ///< Whether the class is part of a virtual inheritance chain
+	std::vector<std::string> qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack
   Methods methods;                          ///< Class methods
   StaticMethods static_methods;             ///< Static methods
   std::vector<std::string> namespaces;      ///< Stack of namespaces
@@ -48,7 +50,7 @@ struct Class {
   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 std::string& wrapperName,
+  void matlab_proxy(const std::string& classFile, const std::string& wrapperName, const ReturnValue::TypeAttributesTable& typeAttributes,
 		FileWriter& wrapperFile, std::vector<std::string>& functionNames) const;          ///< emit proxy class
   std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter
 
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
index 88fbb748b..aaa7f1c8e 100644
--- a/wrap/Constructor.cpp
+++ b/wrap/Constructor.cpp
@@ -37,7 +37,7 @@ string Constructor::matlab_wrapper_name(const string& className) const {
 
 /* ************************************************************************* */
 void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName,
-        const string& className, const int id, const ArgumentList args) const {
+        const string& matlabName, const string& matlabBaseName, const int id, const ArgumentList args) const {
 	size_t nrArgs = args.size();
 	// check for number of arguments...
   file.oss << "      elseif nargin == " << nrArgs;
@@ -50,10 +50,14 @@ void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperNam
     first=false;
   }
   // emit code for calling constructor
-  file.oss << "\n        obj.self = " << wrapperName << "(" << id;
+	if(matlabBaseName.empty())
+		file.oss << "\n        my_ptr = ";
+	else
+		file.oss << "\n        [ my_ptr, base_ptr ] = ";
+  file.oss << wrapperName << "(" << id;
   // emit constructor arguments
   for(size_t i=0;i<nrArgs;i++) {
-    file.oss << ",";
+    file.oss << ", ";
     file.oss << "varargin{" << i+1 << "}";
   }
   file.oss << ");\n";
@@ -63,9 +67,9 @@ void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperNam
 string Constructor::wrapper_fragment(FileWriter& file,
 				 const string& cppClassName,
 				 const string& matlabClassName,
+				 const string& cppBaseClassName,
 				 int id,
 				 const vector<string>& using_namespaces, 
-				 const vector<string>& includes,
 				 const ArgumentList& al) const {
 
 	const string wrapFunctionName = matlabClassName + "_constructor_" + boost::lexical_cast<string>(id);
@@ -89,6 +93,14 @@ string Constructor::wrapper_fragment(FileWriter& file,
   file.oss << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl;
   file.oss << "  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;" << endl;
 
+	// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
+	if(!cppBaseClassName.empty()) {
+		file.oss << "\n";
+		file.oss << "  typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n";
+    file.oss << "  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
+		file.oss << "  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\n";
+	}
+
   file.oss << "}" << endl;
 
 	return wrapFunctionName;
diff --git a/wrap/Constructor.h b/wrap/Constructor.h
index 4a9e31f9b..46f5c5db9 100644
--- a/wrap/Constructor.h
+++ b/wrap/Constructor.h
@@ -49,16 +49,16 @@ struct Constructor {
 	 * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
 	 */
 	void proxy_fragment(FileWriter& file, const std::string& wrapperName,
-	        const std::string& className, const int id,
+	        const std::string& className, const std::string& matlabBaseName, const int id,
 	        const ArgumentList args) const;
 
 	/// cpp wrapper
 	std::string wrapper_fragment(FileWriter& file,
 			 const std::string& cppClassName,
 			 const std::string& matlabClassName,
+			 const std::string& cppBaseClassName,
 			 int id,
 			 const std::vector<std::string>& using_namespaces,
-			 const std::vector<std::string>& includes,
 			 const ArgumentList& al) const;
 
 	/// constructor function
diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp
index 335488cf5..aa4969af5 100644
--- a/wrap/Deconstructor.cpp
+++ b/wrap/Deconstructor.cpp
@@ -39,7 +39,7 @@ void Deconstructor::proxy_fragment(FileWriter& file,
 		const std::string& qualifiedMatlabName, int id) const {
 
 	file.oss << "    function delete(obj)\n";
-	file.oss << "      " << wrapperName << "(" << id << ", obj.self);\n";
+	file.oss << "      " << wrapperName << "(" << id << ", obj.ptr_" << qualifiedMatlabName << ");\n";
 	file.oss << "    end\n";
 }
 
@@ -48,7 +48,7 @@ string Deconstructor::wrapper_fragment(FileWriter& file,
 				 const string& cppClassName,
 				 const string& matlabClassName,
 				 int id,
-				 const vector<string>& using_namespaces, const vector<string>& includes) const {
+				 const vector<string>& using_namespaces) const {
   
 	const string matlabName = matlab_wrapper_name(matlabClassName);
 
diff --git a/wrap/Deconstructor.h b/wrap/Deconstructor.h
index 363f986e6..035ff74df 100644
--- a/wrap/Deconstructor.h
+++ b/wrap/Deconstructor.h
@@ -55,8 +55,7 @@ struct Deconstructor {
 			 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>& using_namespaces) const;
 };
 
 } // \namespace wrap
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index 88176f675..f44a08a73 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -41,9 +41,10 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF
 																		 const string& matlabClassName,
 																		 const string& wrapperName,
 																		 const vector<string>& using_namespaces,
+																		 const ReturnValue::TypeAttributesTable& typeAttributes,
 																		 vector<string>& functionNames) const {
 
-	proxyFile.oss << "    function varargout = " << name << "(self, varargin)\n";
+	proxyFile.oss << "    function varargout = " << name << "(this, varargin)\n";
 
 	for(size_t overload = 0; overload < argLists.size(); ++overload) {
 		const ArgumentList& args = argLists[overload];
@@ -74,12 +75,12 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF
 			output = "";
 		else
 			output = "varargout{1} = ";
-		proxyFile.oss << "        " << output << wrapperName << "(" << id << ", self, varargin{:});\n";
+		proxyFile.oss << "        " << output << wrapperName << "(" << id << ", this, varargin{:});\n";
 
 		// Output C++ wrapper code
 		
 		const string wrapFunctionName = wrapper_fragment(
-			wrapperFile, cppClassName, matlabClassName, overload, id, using_namespaces);
+			wrapperFile, cppClassName, matlabClassName, overload, id, using_namespaces, typeAttributes);
 
 		// Add to function list
 		functionNames.push_back(wrapFunctionName);
@@ -100,7 +101,8 @@ string Method::wrapper_fragment(FileWriter& file,
 			    const string& matlabClassName,
 					int overload,
 					int id,
-			    const vector<string>& using_namespaces) const {
+			    const vector<string>& using_namespaces,
+					const ReturnValue::TypeAttributesTable& typeAttributes) const {
 
   // generate code
 
@@ -135,20 +137,16 @@ string Method::wrapper_fragment(FileWriter& file,
 
   // get class pointer
   // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
-  file.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"" << cppClassName << "\");" << endl;
+  file.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabClassName << "\");" << endl;
   // unwrap arguments, see Argument.cpp
   args.matlab_unwrap(file,1);
 
-  // call method
-  // example: bool result = self->return_field(t);
-  file.oss << "  ";
+  // call method and wrap result
+  // example: out[0]=wrap<bool>(self->return_field(t));
   if (returnVal.type1!="void")
-    file.oss << returnVal.return_type(true,ReturnValue::pair) << " result = ";
-  file.oss << "obj->" << name << "(" << args.names() << ");\n";
-
-  // wrap result
-  // example: out[0]=wrap<bool>(result);
-  returnVal.wrap_result(file);
+		returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes);
+	else
+		file.oss << "  obj->"+name+"("+args.names()+");\n";
 
   // finish
   file.oss << "}\n";
diff --git a/wrap/Method.h b/wrap/Method.h
index 204e3002d..d9b1f09f1 100644
--- a/wrap/Method.h
+++ b/wrap/Method.h
@@ -50,6 +50,7 @@ struct Method {
 	void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
 		const std::string& cppClassName,	const std::string& matlabClassName,
 		const std::string& wrapperName, const std::vector<std::string>& using_namespaces,
+		const ReturnValue::TypeAttributesTable& typeAttributes,
 		std::vector<std::string>& functionNames) const;
 
 private:
@@ -58,7 +59,8 @@ private:
 	    const std::string& matlabClassname,
 			int overload,
 			int id,
-	    const std::vector<std::string>& using_namespaces) const; ///< cpp wrapper
+	    const std::vector<std::string>& using_namespaces,
+			const ReturnValue::TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
 };
 
 } // \namespace wrap
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 86cc90ef1..006e38d4a 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -24,8 +24,10 @@
 //#define BOOST_SPIRIT_DEBUG
 #include <boost/spirit/include/classic_confix.hpp>
 #include <boost/spirit/include/classic_clear_actor.hpp>
+#include <boost/spirit/include/classic_insert_at_actor.hpp>
 #include <boost/lambda/bind.hpp>
 #include <boost/lambda/lambda.hpp>
+#include <boost/lambda/construct.hpp>
 #include <boost/foreach.hpp>
 #include <boost/filesystem.hpp>
 #include <boost/lexical_cast.hpp>
@@ -65,12 +67,12 @@ Module::Module(const string& interfacePath,
   //Method method0(enable_verbose), method(enable_verbose);
   StaticMethod static_method0(enable_verbose), static_method(enable_verbose);
   Class cls0(enable_verbose),cls(enable_verbose);
+	ForwardDeclaration fwDec0, fwDec;
   vector<string> namespaces, /// current namespace tag
   							 namespace_includes, /// current set of includes
   							 namespaces_return, /// namespace for current return type
   							 using_namespace_current;  /// All namespaces from "using" declarations
   string include_path = "";
-  string class_name = "";
   const string null_str = "";
 
   //----------------------------------------------------------------------------
@@ -93,7 +95,7 @@ Module::Module(const string& interfacePath,
   Rule eigenType_p =
     (str_p("Vector") | "Matrix");
 
-  Rule className_p  = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p)[assign_a(class_name)];
+  Rule className_p  = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p);
 
   Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
 
@@ -114,6 +116,10 @@ Module::Module(const string& interfacePath,
     className_p[assign_a(arg.type)] >>
     (ch_p('*')[assign_a(arg.is_ptr,true)] | ch_p('&')[assign_a(arg.is_ref,true)]);
 
+	Rule classParent_p =
+		*(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >>
+		className_p[push_back_a(cls.qualifiedParent)];
+
   Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
 
   Rule argument_p = 
@@ -196,9 +202,10 @@ Module::Module(const string& interfacePath,
 
   Rule class_p =
   		(!*include_p
+			>> !(str_p("virtual")[assign_a(cls.isVirtual, true)])
   		>> str_p("class")[push_back_a(cls.includes, include_path)][assign_a(include_path, null_str)]
   		>> className_p[assign_a(cls.name)]
-      >> '{'
+			>> ((':' >> classParent_p >> '{') | '{') // By having (parent >> '{' | '{') here instead of (!parent >> '{'), we trigger a parse error on a badly-formed parent spec
   		>> *(functions_p | comments_p)
   		>> str_p("};"))
         [assign_a(constructor.name, cls.name)]
@@ -208,7 +215,7 @@ Module::Module(const string& interfacePath,
   		[append_a(cls.includes, namespace_includes)]
         [assign_a(deconstructor.name,cls.name)]
         [assign_a(cls.deconstructor, deconstructor)]
-  		[push_back_a(classes,cls)]
+			[push_back_a(classes, cls)]
         [assign_a(deconstructor,deconstructor0)]
         [assign_a(constructor, constructor0)]
   		[assign_a(cls,cls0)];
@@ -229,9 +236,12 @@ Module::Module(const string& interfacePath,
 			>> namespace_name_p[push_back_a(using_namespace_current)] >> ch_p(';');
 
 	Rule forward_declaration_p =
-			str_p("class") >>
-					(*(namespace_name_p >> str_p("::")) >> className_p)[push_back_a(forward_declarations)]
-					>> ch_p(';');
+			!(str_p("virtual")[assign_a(fwDec.isVirtual, true)])
+			>> str_p("class")
+			>> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)]
+			>> ch_p(';')
+			[push_back_a(forward_declarations, fwDec)]
+			[assign_a(fwDec, fwDec0)];
 
   Rule module_content_p =	comments_p | using_namespace_p | class_p | forward_declaration_p | namespace_def_p ;
 
@@ -321,7 +331,10 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
 		wrapperFile.oss << "\n";
 
     // Dependency check list
-    vector<string> validTypes = forward_declarations;
+    vector<string> validTypes;
+		BOOST_FOREACH(const ForwardDeclaration& fwDec, forward_declarations) {
+			validTypes.push_back(fwDec.name);
+		}
     validTypes.push_back("void");
     validTypes.push_back("string");
     validTypes.push_back("int");
@@ -333,18 +346,31 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
     validTypes.push_back("Vector");
     validTypes.push_back("Matrix");
 		//Create a list of parsed classes for dependency checking
-    BOOST_FOREACH(Class cls, classes) {
+    BOOST_FOREACH(const Class& cls, classes) {
 			validTypes.push_back(cls.qualifiedName("::"));
     }
+
+		// Create type attributes table
+		ReturnValue::TypeAttributesTable typeAttributes;
+		BOOST_FOREACH(const ForwardDeclaration& fwDec, forward_declarations) {
+			if(!typeAttributes.insert(make_pair(fwDec.name, ReturnValue::TypeAttributes(fwDec.isVirtual))).second)
+				throw DuplicateDefinition("class " + fwDec.name);
+		}
+		BOOST_FOREACH(const Class& cls, classes) {
+			if(!typeAttributes.insert(make_pair(cls.qualifiedName("::"), ReturnValue::TypeAttributes(cls.isVirtual))).second)
+				throw DuplicateDefinition("class " + cls.qualifiedName("::"));
+
+			// Check that class is virtual if it has a parent
+		}
 
 		// Generate all includes
-		BOOST_FOREACH(Class cls, classes) {
+		BOOST_FOREACH(const Class& cls, classes) {
 			generateIncludes(wrapperFile, cls.name, cls.includes);
 		}
 		wrapperFile.oss << "\n";
 
 		// Generate all collectors
-		BOOST_FOREACH(Class cls, classes) {
+		BOOST_FOREACH(const Class& cls, classes) {
 			const string matlabName = cls.qualifiedName(), cppName = cls.qualifiedName("::");
 			wrapperFile.oss << "typedef std::set<boost::shared_ptr<" << cppName << ">*> "
 				<< "Collector_" << matlabName << ";\n";
@@ -355,7 +381,7 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
 		// generate mexAtExit cleanup function
 		wrapperFile.oss << "void _deleteAllObjects()\n";
 		wrapperFile.oss << "{\n";
-		BOOST_FOREACH(Class cls, classes) {
+		BOOST_FOREACH(const Class& cls, classes) {
 			const string matlabName = cls.qualifiedName();
 			const string cppName = cls.qualifiedName("::");
 			const string collectorType = "Collector_" + matlabName;
@@ -369,10 +395,10 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
 		wrapperFile.oss << "}\n";
 
     // generate proxy classes and wrappers
-    BOOST_FOREACH(Class cls, classes) {
+		BOOST_FOREACH(const Class& cls, classes) {
       // create proxy class and wrapper code
       string classFile = toolboxPath + "/" + cls.qualifiedName() + ".m";
-      cls.matlab_proxy(classFile, wrapperName, wrapperFile, functionNames);
+      cls.matlab_proxy(classFile, wrapperName, typeAttributes, wrapperFile, functionNames);
 
       // verify all of the function arguments
       //TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
diff --git a/wrap/Module.h b/wrap/Module.h
index a04a861c4..3cb23be25 100644
--- a/wrap/Module.h
+++ b/wrap/Module.h
@@ -19,6 +19,7 @@
 
 #include <string>
 #include <vector>
+#include <map>
 
 #include "Class.h"
 
@@ -28,11 +29,18 @@ namespace wrap {
  * A module just has a name and a list of classes
  */
 struct Module {
+
+	struct ForwardDeclaration {
+		std::string name;
+		bool isVirtual;
+		ForwardDeclaration() : isVirtual(false) {}
+	};
+
   std::string name;         ///< module name
   std::vector<Class> classes; ///< list of classes
   bool verbose;            ///< verbose flag
 //  std::vector<std::string> using_namespaces; ///< all default namespaces
-  std::vector<std::string> forward_declarations;
+  std::vector<ForwardDeclaration> forward_declarations;
 
   /// constructor that parses interface file
   Module(const std::string& interfacePath,
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 0c9dd5830..77bdeb746 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -46,45 +46,60 @@ string ReturnValue::qualifiedType2(const string& delim) const {
 
 /* ************************************************************************* */
 //TODO:Fix this
-void ReturnValue::wrap_result(FileWriter& file) const {
+void ReturnValue::wrap_result(const string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) 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 << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(result.first);" << endl;
+      file.oss << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ".first);" << endl;
       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";
+			string objCopy;
+			if(typeAttributes.at(cppType1).isVirtual)
+				objCopy = "boost::dynamic_pointer_cast<" + cppType1 + ">(" + result + ".first.clone())";
+			else
+				objCopy = "new " + cppType1 + "(" + result + ".first)";
+      file.oss << "  Shared" <<  type1 << "* ret = new Shared" <<  type1 << "(" << objCopy << ");\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";
+      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 << "  Shared" << type2 <<"* ret = new Shared" << type2 << "(result.second);" << endl;
+      file.oss << "  Shared" << type2 <<"* ret = new Shared" << type2 << "(" << result << ".second);" << endl;
       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";
+			string objCopy;
+			if(typeAttributes.at(cppType1).isVirtual)
+				objCopy = "boost::dynamic_pointer_cast<" + cppType2 + ">(" + result + ".second.clone())";
+			else
+				objCopy = "new " + cppType1 + "(" + result + ".second)";
+      file.oss << "  Shared" <<  type2 << "* ret = new Shared" <<  type2 << "(" << objCopy << ");\n";
     	file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n";
     }
     else
-      file.oss << "  out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
+      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 << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ");" << endl;
     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";
+		string objCopy;
+		if(typeAttributes.at(cppType1).isVirtual)
+			objCopy = "boost::dynamic_pointer_cast<" + cppType1 + ">(" + result + ".clone())";
+		else
+			objCopy = "new " + cppType1 + "(" + result + ")";
+    file.oss << "  Shared" <<  type1 << "* ret = new Shared" <<  type1 << "(" << objCopy << ");\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";
+    file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(" << result << ");\n";
 }
 
 /* ************************************************************************* */
diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h
index 859e00433..7d30578b2 100644
--- a/wrap/ReturnValue.h
+++ b/wrap/ReturnValue.h
@@ -8,6 +8,7 @@
  */
 
 #include <vector>
+#include <map>
 
 #include "FileWriter.h"
 
@@ -17,6 +18,14 @@ namespace wrap {
 
 struct ReturnValue {
 
+	struct TypeAttributes {
+		bool isVirtual;
+		TypeAttributes() : isVirtual(false) {}
+		TypeAttributes(bool isVirtual) : isVirtual(isVirtual) {}
+	};
+
+	typedef std::map<std::string, TypeAttributes> TypeAttributesTable;
+
 	typedef enum {
 		CLASS,
 		EIGEN,
@@ -47,7 +56,7 @@ struct ReturnValue {
 
 	std::string matlab_returnType() const;
 
-	void wrap_result(FileWriter& file) const;
+	void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const;
 
 };
 
diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp
index dd0c606b5..75456b6b7 100644
--- a/wrap/StaticMethod.cpp
+++ b/wrap/StaticMethod.cpp
@@ -42,6 +42,7 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wr
 																		 const string& matlabClassName,
 																		 const string& wrapperName,
 																		 const vector<string>& using_namespaces,
+																		 const ReturnValue::TypeAttributesTable& typeAttributes,
 																		 vector<string>& functionNames) const {
 
   string upperName = name;  upperName[0] = std::toupper(upperName[0], std::locale());
@@ -82,7 +83,7 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wr
 		// Output C++ wrapper code
 		
 		const string wrapFunctionName = wrapper_fragment(
-			wrapperFile, cppClassName, matlabClassName, overload, id, using_namespaces);
+			wrapperFile, cppClassName, matlabClassName, overload, id, using_namespaces, typeAttributes);
 
 		// Add to function list
 		functionNames.push_back(wrapFunctionName);
@@ -103,7 +104,8 @@ string StaticMethod::wrapper_fragment(FileWriter& file,
 			    const string& matlabClassName,
 					int overload,
 					int id,
-			    const vector<string>& using_namespaces) const {
+			    const vector<string>& using_namespaces,
+					const ReturnValue::TypeAttributesTable& typeAttributes) const {
 
   // generate code
 
@@ -140,14 +142,11 @@ string StaticMethod::wrapper_fragment(FileWriter& file,
 
   file.oss << "  ";
 
-  // call method with default type
+  // call method with default type and wrap result
   if (returnVal.type1!="void")
-    file.oss << returnVal.return_type(true,ReturnValue::pair) << " result = ";
-  file.oss << cppClassName  << "::" << name << "(" << args.names() << ");\n";
-
-  // wrap result
-  // example: out[0]=wrap<bool>(result);
-  returnVal.wrap_result(file);
+		returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes);
+	else
+		file.oss << cppClassName+"::"+name+"("+args.names()+");\n";
 
   // finish
   file.oss << "}\n";
diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h
index 5bc599ea5..8a739665d 100644
--- a/wrap/StaticMethod.h
+++ b/wrap/StaticMethod.h
@@ -50,6 +50,7 @@ struct StaticMethod {
 	void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
 		const std::string& cppClassName,	const std::string& matlabClassName,
 		const std::string& wrapperName, const std::vector<std::string>& using_namespaces,
+		const ReturnValue::TypeAttributesTable& typeAttributes,
 		std::vector<std::string>& functionNames) const;
 
 private:
@@ -58,7 +59,8 @@ private:
 	    const std::string& matlabClassname,
 			int overload,
 			int id,
-	    const std::vector<std::string>& using_namespaces) const; ///< cpp wrapper
+	    const std::vector<std::string>& using_namespaces,
+			const ReturnValue::TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
 };
 
 } // \namespace wrap
diff --git a/wrap/matlab.h b/wrap/matlab.h
index 31778cdbd..05f7fffb5 100644
--- a/wrap/matlab.h
+++ b/wrap/matlab.h
@@ -56,7 +56,7 @@ using namespace boost; // not usual, but for conciseness of generated code
 #endif
 
 // "Unique" key to signal calling the matlab object constructor with a raw pointer
-// Also present in Class.cpp
+// Also present in utilities.h
 static const uint64_t ptr_constructor_key =
 	(uint64_t('G') << 56) |
 	(uint64_t('T') << 48) |
@@ -119,7 +119,7 @@ void checkArguments(const string& name, int nargout, int nargin, int expected) {
 
 // default wrapping throws an error: only basis types are allowed in wrap
 template <typename Class>
-mxArray* wrap(Class& value) {
+mxArray* wrap(const Class& value) {
   error("wrap internal error: attempted wrap of invalid type");
 	return 0;
 }
@@ -127,13 +127,13 @@ mxArray* wrap(Class& value) {
 // specialization to string
 // wraps into a character array
 template<>
-mxArray* wrap<string>(string& value) {
+mxArray* wrap<string>(const string& value) {
   return mxCreateString(value.c_str());
 }
 
 // specialization to char
 template<>
-mxArray* wrap<char>(char& value) {
+mxArray* wrap<char>(const char& value) {
   mxArray *result = scalar(mxUINT32OR64_CLASS);
   *(char*)mxGetData(result) = value;
   return result;
@@ -141,7 +141,7 @@ mxArray* wrap<char>(char& value) {
 
 // specialization to unsigned char
 template<>
-mxArray* wrap<unsigned char>(unsigned char& value) {
+mxArray* wrap<unsigned char>(const unsigned char& value) {
   mxArray *result = scalar(mxUINT32OR64_CLASS);
   *(unsigned char*)mxGetData(result) = value;
   return result;
@@ -149,7 +149,7 @@ mxArray* wrap<unsigned char>(unsigned char& value) {
 
 // specialization to bool
 template<>
-mxArray* wrap<bool>(bool& value) {
+mxArray* wrap<bool>(const bool& value) {
   mxArray *result = scalar(mxUINT32OR64_CLASS);
   *(bool*)mxGetData(result) = value;
   return result;
@@ -157,7 +157,7 @@ mxArray* wrap<bool>(bool& value) {
 
 // specialization to size_t
 template<>
-mxArray* wrap<size_t>(size_t& value) {
+mxArray* wrap<size_t>(const size_t& value) {
   mxArray *result = scalar(mxUINT32OR64_CLASS);
   *(size_t*)mxGetData(result) = value;
   return result;
@@ -165,7 +165,7 @@ mxArray* wrap<size_t>(size_t& value) {
 
 // specialization to int
 template<>
-mxArray* wrap<int>(int& value) {
+mxArray* wrap<int>(const int& value) {
   mxArray *result = scalar(mxUINT32OR64_CLASS);
   *(int*)mxGetData(result) = value;
   return result;
@@ -173,7 +173,7 @@ mxArray* wrap<int>(int& value) {
 
 // specialization to double -> just double
 template<>
-mxArray* wrap<double>(double& value) {
+mxArray* wrap<double>(const double& value) {
   return mxCreateDoubleScalar(value);
 }
 
@@ -188,13 +188,7 @@ mxArray* wrap_Vector(const gtsam::Vector& v) {
 
 // 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) {
+mxArray* wrap<gtsam::Vector >(const gtsam::Vector& v) {
   return wrap_Vector(v);
 }
 
@@ -214,13 +208,7 @@ mxArray* wrap_Matrix(const gtsam::Matrix& A) {
 
 // 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) {
+mxArray* wrap<gtsam::Matrix >(const gtsam::Matrix& A) {
   return wrap_Matrix(A);
 }
 
@@ -342,9 +330,14 @@ gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
  [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.
+ invoke the proxy class constructor by calling MATLAB with a special
+ uint64 value ptr_constructor_key and the pointer itself.  MATLAB
+ allocates the object.  Then, the special constructor in our wrap code
+ that is activated when the ptr_constructor_key is passed in passes
+ the pointer back into a C++ function to add the pointer to its
+ collector.  We go through this extra "C++ to MATLAB to C++ step" in
+ order to be able to add to the collector could be in a different wrap
+ module.
 */
 mxArray* create_object(const char *classname, void *pointer) {
   mxArray *result;
@@ -376,9 +369,9 @@ mxArray* wrap_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *cla
 }
 
 template <typename Class>
-boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) {
+boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& propertyName) {
 
-  mxArray* mxh = mxGetProperty(obj,0,"self");
+	mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str());
   if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh)
     || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error(
     "Parameter is not an Shared type.");
diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp
index 31b65bdcc..94117b5d3 100644
--- a/wrap/utilities.cpp
+++ b/wrap/utilities.cpp
@@ -135,6 +135,21 @@ void generateIncludes(FileWriter& file, const string& class_name,
 }
 
 /* ************************************************************************* */
+string qualifiedName(const string& separator, const vector<string>& names, const string& finalName) {
+	string result;
+	if(!names.empty()) {
+		for(size_t i = 0; i < names.size() - 1; ++i)
+			result += (names[i] + separator);
+		if(finalName.empty())
+			result += names.back();
+		else
+			result += (names.back() + separator + finalName);
+	} else if(!finalName.empty()) {
+		result = finalName;
+	}
+	return result;
+}
 
+/* ************************************************************************* */
 
 } // \namespace wrap
diff --git a/wrap/utilities.h b/wrap/utilities.h
index 1eb0e249f..eeda3c68a 100644
--- a/wrap/utilities.h
+++ b/wrap/utilities.h
@@ -21,6 +21,9 @@
 #include <exception>
 #include <fstream>
 #include <sstream>
+#include <cstdint>
+#include <string>
+#include <boost/format.hpp>
 
 #include "FileWriter.h"
 
@@ -28,44 +31,54 @@ namespace wrap {
 
 class CantOpenFile : public std::exception {
  private:
-  std::string filename_;
+	const std::string what_;
  public:
- CantOpenFile(const std::string& filename) : filename_(filename) {}
+ CantOpenFile(const std::string& filename) : what_("Can't open file " + filename) {}
   ~CantOpenFile() throw() {}
-  virtual const char* what() const throw() { 
-    return ("Can't open file " + filename_).c_str(); 
-  }
+	virtual const char* what() const throw() { return what_.c_str(); }
 };
 
 class ParseFailed : public std::exception {
  private:
-  int length_;
+  const std::string what_;
  public:
- ParseFailed(int length) : length_(length) {}
-  ~ParseFailed() throw() {}
-  virtual const char* what() const throw() { 
-    std::stringstream buf;
-    int len = length_+1;
-    buf << "Parse failed at character [" << len << "]";
-    return buf.str().c_str(); 
-  }
+	 ParseFailed(int length) : what_((boost::format("Parse failed at character [%d]")%(length-1)).str()) {}
+	 ~ParseFailed() throw() {}
+	 virtual const char* what() const throw() { return what_.c_str(); }
 };
 
 class DependencyMissing : public std::exception {
 private:
-	std::string dependency_;
-	std::string location_;
+	const std::string what_;
 public:
-	DependencyMissing(const std::string& dep, const std::string& loc) {
-		dependency_ = dep;
-		location_ = loc;
-	}
+	DependencyMissing(const std::string& dep, const std::string& loc) :
+		what_("Missing dependency " + dep + " in " + loc) {}
 	~DependencyMissing() throw() {}
-	virtual const char* what() const throw() {
-		return ("Missing dependency " + dependency_ + " in " + location_).c_str();
-	}
+	virtual const char* what() const throw() { return what_.c_str(); }
 };
 
+class DuplicateDefinition : public std::exception {
+private:
+	const std::string what_;
+public:
+	DuplicateDefinition(const std::string& name) :
+		what_("Duplicate definition of " + name) {}
+	~DuplicateDefinition() throw() {}
+	virtual const char* what() const throw() { return what_.c_str(); }
+};
+	
+/** Special "magic number" passed into MATLAB constructor to indicate creating
+ * a MATLAB object from a shared_ptr allocated in C++
+ */
+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'));
 
 /**
  * read contents of a file into a std::string
@@ -98,4 +111,10 @@ void generateUsingNamespace(FileWriter& file, const std::vector<std::string>& us
 void generateIncludes(FileWriter& file, const std::string& class_name,
 		const std::vector<std::string>& includes);
 
+/**
+ * Return a qualified name, if finalName is empty, only the names vector will
+ * be used (i.e. there won't be a trailing separator on the qualified name).
+ */
+std::string qualifiedName(const std::string& separator, const std::vector<std::string>& names, const std::string& finalName = "");
+
 } // \namespace wrap