diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index 59e5da88e..a7e977bf3 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -261,33 +261,37 @@ static vector<ArgumentList> expandArgumentListsTemplate(
 }
 
 /* ************************************************************************* */
+// TODO, Method, StaticMethod, and GlobalFunction should have common base ?
 template<class METHOD>
-map<string, METHOD> expandMethodTemplate(const map<string, METHOD>& methods,
-    const string& templateArg, const Qualified& qualifiedType,
-    const Qualified& expandedClass) {
+METHOD expandMethodTemplate(const METHOD& method, const string& templateArg,
+    const Qualified& qualifiedType, const Qualified& expandedClass) {
+  // Create new instance
+  METHOD instMethod = method;
+  // substitute template in arguments
+  instMethod.argLists = expandArgumentListsTemplate(method.argLists,
+      templateArg, qualifiedType, expandedClass);
+  // do the same for return types
+  instMethod.returnVals.clear();
+  BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
+    ReturnValue instRetVal = retVal.substituteTemplate(templateArg,
+        qualifiedType, expandedClass);
+    instMethod.returnVals.push_back(instRetVal);
+  }
+  // return new method
+  return instMethod;
+}
+
+/* ************************************************************************* */
+template<class METHOD>
+static map<string, METHOD> expandMethodTemplate(
+    const map<string, METHOD>& methods, const string& templateArg,
+    const Qualified& qualifiedType, const Qualified& expandedClass) {
   map<string, METHOD> result;
-  typedef pair<const string, METHOD> Name_Method;
-  BOOST_FOREACH(const Name_Method& name_method, methods) {
-    const METHOD& method = name_method.second;
-    METHOD instMethod = method;
-    instMethod.argLists = expandArgumentListsTemplate(method.argLists,
-        templateArg, qualifiedType, expandedClass);
-    instMethod.returnVals.clear();
-    BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
-      ReturnValue instRetVal = retVal;
-      if (retVal.type1.name == templateArg) {
-        instRetVal.type1.rename(qualifiedType);
-      } else if (retVal.type1.name == "This") {
-        instRetVal.type1.rename(expandedClass);
-      }
-      if (retVal.type2.name == templateArg) {
-        instRetVal.type2.rename(qualifiedType);
-      } else if (retVal.type2.name == "This") {
-        instRetVal.type2.rename(expandedClass);
-      }
-      instMethod.returnVals.push_back(instRetVal);
-    }
-    result.insert(make_pair(name_method.first, instMethod));
+  typedef pair<const string, METHOD> NamedMethod;
+  BOOST_FOREACH(NamedMethod namedMethod, methods) {
+    namedMethod.second = expandMethodTemplate(namedMethod.second, templateArg,
+        qualifiedType, expandedClass);
+    result.insert(namedMethod);
   }
   return result;
 }
@@ -328,7 +332,24 @@ vector<Class> Class::expandTemplate(const string& templateArg,
 void Class::addMethod(bool verbose, bool is_const, const string& name,
     const ArgumentList& args, const ReturnValue& retVal,
     const string& templateArgName, const vector<Qualified>& templateArgValues) {
-  methods[name].addOverload(verbose, is_const, name, args, retVal);
+  // Check if templated
+  if (!templateArgName.empty() && templateArgValues.size() > 0) {
+    // Create method to expand
+    Method method;
+    method.addOverload(verbose, is_const, name, args, retVal);
+    // For all values of the template argument, create a new method
+    BOOST_FOREACH(const Qualified& instName, templateArgValues) {
+      Method expanded = //
+          expandMethodTemplate(method, templateArgName, instName, *this);
+      expanded.name += instName.name;
+      if (exists(expanded.name))
+        throw runtime_error(
+            "Class::addMethod: Overloading and templates are mutex, for now");
+      methods[expanded.name] = expanded;
+    }
+  } else
+    // just add overload
+    methods[name].addOverload(verbose, is_const, name, args, retVal);
 }
 
 /* ************************************************************************* */
diff --git a/wrap/Qualified.h b/wrap/Qualified.h
index b92d6dace..f38587fbb 100644
--- a/wrap/Qualified.h
+++ b/wrap/Qualified.h
@@ -29,7 +29,11 @@ namespace wrap {
 struct Qualified {
 
   std::vector<std::string> namespaces; ///< Stack of namespaces
-  std::string name;                    ///< type name
+  std::string name; ///< type name
+
+  Qualified(const std::string& name_ = "") :
+      name(name_) {
+  }
 
   bool empty() const {
     return namespaces.empty() && name.empty();
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 5287410e0..ea2cb1489 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -58,6 +58,23 @@ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
         << "> Shared" << name << ";" << endl;
 }
 
+/* ************************************************************************* */
+ReturnValue ReturnValue::substituteTemplate(const string& templateArg,
+    const Qualified& qualifiedType, const Qualified& expandedClass) const {
+  ReturnValue instRetVal = *this;
+  if (type1.name == templateArg) {
+    instRetVal.type1.rename(qualifiedType);
+  } else if (type1.name == "This") {
+    instRetVal.type1.rename(expandedClass);
+  }
+  if (type2.name == templateArg) {
+    instRetVal.type2.rename(qualifiedType);
+  } else if (type2.name == "This") {
+    instRetVal.type2.rename(expandedClass);
+  }
+  return instRetVal;
+}
+
 /* ************************************************************************* */
 string ReturnValue::return_type(bool add_ptr) const {
   if (isPair)
@@ -78,8 +95,10 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
     // For a pair, store the returned pair so we do not evaluate the function twice
     wrapperFile.oss << "  " << return_type(true) << " pairResult = " << result
         << ";\n";
-    type1.wrap_result("  out[0]", "pairResult.first", wrapperFile, typeAttributes);
-    type2.wrap_result("  out[1]", "pairResult.second", wrapperFile, typeAttributes);
+    type1.wrap_result("  out[0]", "pairResult.first", wrapperFile,
+        typeAttributes);
+    type2.wrap_result("  out[1]", "pairResult.second", wrapperFile,
+        typeAttributes);
   } else { // Not a pair
     type1.wrap_result("  out[0]", result, wrapperFile, typeAttributes);
   }
diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h
index 1a0caf8f3..6e6e149de 100644
--- a/wrap/ReturnValue.h
+++ b/wrap/ReturnValue.h
@@ -85,6 +85,10 @@ struct ReturnValue {
       isPair(false), type1(type) {
   }
 
+  /// Substitute template argument
+  ReturnValue substituteTemplate(const std::string& templateArg,
+      const Qualified& qualifiedType, const Qualified& expandedClass) const;
+
   std::string return_type(bool add_ptr) const;
 
   std::string matlab_returnType() const;
@@ -95,6 +99,7 @@ struct ReturnValue {
   void wrapTypeUnwrap(FileWriter& wrapperFile) const;
 
   void emit_matlab(FileWriter& proxyFile) const;
+
 };
 
 template<class T>
diff --git a/wrap/tests/expected2/MyFactorPosePoint2.m b/wrap/tests/expected2/MyFactorPosePoint2.m
index 08a72ddba..166e1514d 100644
--- a/wrap/tests/expected2/MyFactorPosePoint2.m
+++ b/wrap/tests/expected2/MyFactorPosePoint2.m
@@ -12,9 +12,9 @@ classdef MyFactorPosePoint2 < handle
     function obj = MyFactorPosePoint2(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(67, my_ptr);
+        geometry_wrapper(69, my_ptr);
       elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
-        my_ptr = geometry_wrapper(68, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = geometry_wrapper(70, varargin{1}, varargin{2}, varargin{3}, varargin{4});
       else
         error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
       end
@@ -22,7 +22,7 @@ classdef MyFactorPosePoint2 < handle
     end
 
     function delete(obj)
-      geometry_wrapper(69, obj.ptr_MyFactorPosePoint2);
+      geometry_wrapper(71, obj.ptr_MyFactorPosePoint2);
     end
 
     function display(obj), obj.print(''); end
diff --git a/wrap/tests/expected2/MyTemplatePoint2.m b/wrap/tests/expected2/MyTemplatePoint2.m
index 272ab9ebd..9553f1413 100644
--- a/wrap/tests/expected2/MyTemplatePoint2.m
+++ b/wrap/tests/expected2/MyTemplatePoint2.m
@@ -12,7 +12,8 @@
 %return_T(Point2 value) : returns Point2
 %return_Tptr(Point2 value) : returns Point2
 %return_ptrs(Point2 p1, Point2 p2) : returns pair< Point2, Point2 >
-%templatedMethod(Test t) : returns void
+%templatedMethodPoint2(Point2 t) : returns void
+%templatedMethodPoint3(Point3 t) : returns void
 %
 classdef MyTemplatePoint2 < MyBase
   properties
@@ -106,13 +107,23 @@ classdef MyTemplatePoint2 < MyBase
       end
     end
 
-    function varargout = templatedMethod(this, varargin)
-      % TEMPLATEDMETHOD usage: templatedMethod(Test t) : returns void
+    function varargout = templatedMethodPoint2(this, varargin)
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'Test')
+      if length(varargin) == 1 && isa(varargin{1},'Point2')
         geometry_wrapper(54, this, varargin{:});
       else
-        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
+        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodPoint2');
+      end
+    end
+
+    function varargout = templatedMethodPoint3(this, varargin)
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'Point3')
+        geometry_wrapper(55, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodPoint3');
       end
     end
 
diff --git a/wrap/tests/expected2/MyTemplatePoint3.m b/wrap/tests/expected2/MyTemplatePoint3.m
index ab66bd30c..1cd4c1250 100644
--- a/wrap/tests/expected2/MyTemplatePoint3.m
+++ b/wrap/tests/expected2/MyTemplatePoint3.m
@@ -12,7 +12,8 @@
 %return_T(Point3 value) : returns Point3
 %return_Tptr(Point3 value) : returns Point3
 %return_ptrs(Point3 p1, Point3 p2) : returns pair< Point3, Point3 >
-%templatedMethod(Test t) : returns void
+%templatedMethodPoint2(Point2 t) : returns void
+%templatedMethodPoint3(Point3 t) : returns void
 %
 classdef MyTemplatePoint3 < MyBase
   properties
@@ -24,11 +25,11 @@ classdef MyTemplatePoint3 < MyBase
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = geometry_wrapper(56, varargin{2});
+          my_ptr = geometry_wrapper(57, varargin{2});
         end
-        base_ptr = geometry_wrapper(55, my_ptr);
+        base_ptr = geometry_wrapper(56, my_ptr);
       elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(57);
+        [ my_ptr, base_ptr ] = geometry_wrapper(58);
       else
         error('Arguments do not match any overload of MyTemplatePoint3 constructor');
       end
@@ -37,7 +38,7 @@ classdef MyTemplatePoint3 < MyBase
     end
 
     function delete(obj)
-      geometry_wrapper(58, obj.ptr_MyTemplatePoint3);
+      geometry_wrapper(59, obj.ptr_MyTemplatePoint3);
     end
 
     function display(obj), obj.print(''); end
@@ -48,7 +49,7 @@ classdef MyTemplatePoint3 < MyBase
       % ACCEPT_T usage: accept_T(Point3 value) : returns void
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 1 && isa(varargin{1},'Point3')
-        geometry_wrapper(59, this, varargin{:});
+        geometry_wrapper(60, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.accept_T');
       end
@@ -58,7 +59,7 @@ classdef MyTemplatePoint3 < MyBase
       % ACCEPT_TPTR usage: accept_Tptr(Point3 value) : returns void
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 1 && isa(varargin{1},'Point3')
-        geometry_wrapper(60, this, varargin{:});
+        geometry_wrapper(61, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.accept_Tptr');
       end
@@ -67,20 +68,20 @@ classdef MyTemplatePoint3 < MyBase
     function varargout = create_MixedPtrs(this, varargin)
       % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Point3, Point3 >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(61, this, varargin{:});
+      [ varargout{1} varargout{2} ] = geometry_wrapper(62, this, varargin{:});
     end
 
     function varargout = create_ptrs(this, varargin)
       % CREATE_PTRS usage: create_ptrs() : returns pair< Point3, Point3 >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(62, this, varargin{:});
+      [ varargout{1} varargout{2} ] = geometry_wrapper(63, this, varargin{:});
     end
 
     function varargout = return_T(this, varargin)
       % RETURN_T usage: return_T(Point3 value) : returns Point3
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 1 && isa(varargin{1},'Point3')
-        varargout{1} = geometry_wrapper(63, this, varargin{:});
+        varargout{1} = geometry_wrapper(64, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.return_T');
       end
@@ -90,7 +91,7 @@ classdef MyTemplatePoint3 < MyBase
       % RETURN_TPTR usage: return_Tptr(Point3 value) : returns Point3
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 1 && isa(varargin{1},'Point3')
-        varargout{1} = geometry_wrapper(64, this, varargin{:});
+        varargout{1} = geometry_wrapper(65, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.return_Tptr');
       end
@@ -100,19 +101,29 @@ classdef MyTemplatePoint3 < MyBase
       % RETURN_PTRS usage: return_ptrs(Point3 p1, Point3 p2) : returns pair< Point3, Point3 >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 2 && isa(varargin{1},'Point3') && isa(varargin{2},'Point3')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(65, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.return_ptrs');
       end
     end
 
-    function varargout = templatedMethod(this, varargin)
-      % TEMPLATEDMETHOD usage: templatedMethod(Test t) : returns void
+    function varargout = templatedMethodPoint2(this, varargin)
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1 && isa(varargin{1},'Test')
-        geometry_wrapper(66, this, varargin{:});
+      if length(varargin) == 1 && isa(varargin{1},'Point2')
+        geometry_wrapper(67, this, varargin{:});
       else
-        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
+        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethodPoint2');
+      end
+    end
+
+    function varargout = templatedMethodPoint3(this, varargin)
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'Point3')
+        geometry_wrapper(68, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethodPoint3');
       end
     end
 
diff --git a/wrap/tests/expected2/aGlobalFunction.m b/wrap/tests/expected2/aGlobalFunction.m
index 2e87a30b0..5cf9aafa1 100644
--- a/wrap/tests/expected2/aGlobalFunction.m
+++ b/wrap/tests/expected2/aGlobalFunction.m
@@ -1,6 +1,6 @@
 function varargout = aGlobalFunction(varargin)
       if length(varargin) == 0
-        varargout{1} = geometry_wrapper(70, varargin{:});
+        varargout{1} = geometry_wrapper(72, varargin{:});
       else
         error('Arguments do not match any overload of function aGlobalFunction');
       end
diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp
index 00f64c708..47790d816 100644
--- a/wrap/tests/expected2/geometry_wrapper.cpp
+++ b/wrap/tests/expected2/geometry_wrapper.cpp
@@ -664,16 +664,25 @@ void MyTemplatePoint2_return_ptrs_53(int nargout, mxArray *out[], int nargin, co
   out[1] = wrap_shared_ptr(pairResult.second,"Point2", false);
 }
 
-void MyTemplatePoint2_templatedMethod_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_templatedMethodPoint2_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint2> Shared;
-  checkArguments("templatedMethod",nargout,nargin-1,1);
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
   Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  obj->templatedMethod(t);
+  Point2& t = *unwrap_shared_ptr< Point2 >(in[1], "ptr_Point2");
+  obj->templatedMethodPoint2(t);
 }
 
-void MyTemplatePoint3_collectorInsertAndMakeBase_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_templatedMethodPoint3_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
+  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  Point3& t = *unwrap_shared_ptr< Point3 >(in[1], "ptr_Point3");
+  obj->templatedMethodPoint3(t);
+}
+
+void MyTemplatePoint3_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -686,7 +695,7 @@ void MyTemplatePoint3_collectorInsertAndMakeBase_55(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
-void MyTemplatePoint3_upcastFromVoid_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void MyTemplatePoint3_upcastFromVoid_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
@@ -695,7 +704,7 @@ void MyTemplatePoint3_upcastFromVoid_56(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void MyTemplatePoint3_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -710,7 +719,7 @@ void MyTemplatePoint3_constructor_57(int nargout, mxArray *out[], int nargin, co
   *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
 }
 
-void MyTemplatePoint3_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_deconstructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   checkArguments("delete_MyTemplatePoint3",nargout,nargin,1);
@@ -723,7 +732,7 @@ void MyTemplatePoint3_deconstructor_58(int nargout, mxArray *out[], int nargin,
   }
 }
 
-void MyTemplatePoint3_accept_T_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_accept_T_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   checkArguments("accept_T",nargout,nargin-1,1);
@@ -732,7 +741,7 @@ void MyTemplatePoint3_accept_T_59(int nargout, mxArray *out[], int nargin, const
   obj->accept_T(value);
 }
 
-void MyTemplatePoint3_accept_Tptr_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_accept_Tptr_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   checkArguments("accept_Tptr",nargout,nargin-1,1);
@@ -741,7 +750,7 @@ void MyTemplatePoint3_accept_Tptr_60(int nargout, mxArray *out[], int nargin, co
   obj->accept_Tptr(value);
 }
 
-void MyTemplatePoint3_create_MixedPtrs_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_create_MixedPtrs_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> SharedPoint3;
   typedef boost::shared_ptr<Point3> SharedPoint3;
@@ -753,7 +762,7 @@ void MyTemplatePoint3_create_MixedPtrs_61(int nargout, mxArray *out[], int nargi
   out[1] = wrap_shared_ptr(pairResult.second,"Point3", false);
 }
 
-void MyTemplatePoint3_create_ptrs_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_create_ptrs_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> SharedPoint3;
   typedef boost::shared_ptr<Point3> SharedPoint3;
@@ -765,7 +774,7 @@ void MyTemplatePoint3_create_ptrs_62(int nargout, mxArray *out[], int nargin, co
   out[1] = wrap_shared_ptr(pairResult.second,"Point3", false);
 }
 
-void MyTemplatePoint3_return_T_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_return_T_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> SharedPoint3;
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -775,7 +784,7 @@ void MyTemplatePoint3_return_T_63(int nargout, mxArray *out[], int nargin, const
   out[0] = wrap_shared_ptr(SharedPoint3(new Point3(obj->return_T(value))),"Point3", false);
 }
 
-void MyTemplatePoint3_return_Tptr_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_return_Tptr_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> SharedPoint3;
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -785,7 +794,7 @@ void MyTemplatePoint3_return_Tptr_64(int nargout, mxArray *out[], int nargin, co
   out[0] = wrap_shared_ptr(obj->return_Tptr(value),"Point3", false);
 }
 
-void MyTemplatePoint3_return_ptrs_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_return_ptrs_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> SharedPoint3;
   typedef boost::shared_ptr<Point3> SharedPoint3;
@@ -799,16 +808,25 @@ void MyTemplatePoint3_return_ptrs_65(int nargout, mxArray *out[], int nargin, co
   out[1] = wrap_shared_ptr(pairResult.second,"Point3", false);
 }
 
-void MyTemplatePoint3_templatedMethod_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_templatedMethodPoint2_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
-  checkArguments("templatedMethod",nargout,nargin-1,1);
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
   Shared obj = unwrap_shared_ptr<MyTemplatePoint3>(in[0], "ptr_MyTemplatePoint3");
-  Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  obj->templatedMethod(t);
+  Point2& t = *unwrap_shared_ptr< Point2 >(in[1], "ptr_Point2");
+  obj->templatedMethodPoint2(t);
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_templatedMethodPoint3_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplatePoint3> Shared;
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
+  Shared obj = unwrap_shared_ptr<MyTemplatePoint3>(in[0], "ptr_MyTemplatePoint3");
+  Point3& t = *unwrap_shared_ptr< Point3 >(in[1], "ptr_Point3");
+  obj->templatedMethodPoint3(t);
+}
+
+void MyFactorPosePoint2_collectorInsertAndMakeBase_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
@@ -817,7 +835,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_67(int nargout, mxArray *out[
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
@@ -832,7 +850,7 @@ void MyFactorPosePoint2_constructor_68(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
   checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
@@ -845,18 +863,18 @@ void MyFactorPosePoint2_deconstructor_69(int nargout, mxArray *out[], int nargin
   }
 }
 
-void aGlobalFunction_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void aGlobalFunction_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("aGlobalFunction",nargout,nargin,0);
   out[0] = wrap< Vector >(aGlobalFunction());
 }
-void overloadedGlobalFunction_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,1);
   int a = unwrap< int >(in[0]);
   out[0] = wrap< Vector >(overloadedGlobalFunction(a));
 }
-void overloadedGlobalFunction_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,2);
   int a = unwrap< int >(in[0]);
@@ -1038,61 +1056,67 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplatePoint2_return_ptrs_53(nargout, out, nargin-1, in+1);
       break;
     case 54:
-      MyTemplatePoint2_templatedMethod_54(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_templatedMethodPoint2_54(nargout, out, nargin-1, in+1);
       break;
     case 55:
-      MyTemplatePoint3_collectorInsertAndMakeBase_55(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_templatedMethodPoint3_55(nargout, out, nargin-1, in+1);
       break;
     case 56:
-      MyTemplatePoint3_upcastFromVoid_56(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1);
       break;
     case 57:
-      MyTemplatePoint3_constructor_57(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_upcastFromVoid_57(nargout, out, nargin-1, in+1);
       break;
     case 58:
-      MyTemplatePoint3_deconstructor_58(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_constructor_58(nargout, out, nargin-1, in+1);
       break;
     case 59:
-      MyTemplatePoint3_accept_T_59(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_deconstructor_59(nargout, out, nargin-1, in+1);
       break;
     case 60:
-      MyTemplatePoint3_accept_Tptr_60(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_accept_T_60(nargout, out, nargin-1, in+1);
       break;
     case 61:
-      MyTemplatePoint3_create_MixedPtrs_61(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_accept_Tptr_61(nargout, out, nargin-1, in+1);
       break;
     case 62:
-      MyTemplatePoint3_create_ptrs_62(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_create_MixedPtrs_62(nargout, out, nargin-1, in+1);
       break;
     case 63:
-      MyTemplatePoint3_return_T_63(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_create_ptrs_63(nargout, out, nargin-1, in+1);
       break;
     case 64:
-      MyTemplatePoint3_return_Tptr_64(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_return_T_64(nargout, out, nargin-1, in+1);
       break;
     case 65:
-      MyTemplatePoint3_return_ptrs_65(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_return_Tptr_65(nargout, out, nargin-1, in+1);
       break;
     case 66:
-      MyTemplatePoint3_templatedMethod_66(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_return_ptrs_66(nargout, out, nargin-1, in+1);
       break;
     case 67:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_67(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_templatedMethodPoint2_67(nargout, out, nargin-1, in+1);
       break;
     case 68:
-      MyFactorPosePoint2_constructor_68(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_templatedMethodPoint3_68(nargout, out, nargin-1, in+1);
       break;
     case 69:
-      MyFactorPosePoint2_deconstructor_69(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_collectorInsertAndMakeBase_69(nargout, out, nargin-1, in+1);
       break;
     case 70:
-      aGlobalFunction_70(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_constructor_70(nargout, out, nargin-1, in+1);
       break;
     case 71:
-      overloadedGlobalFunction_71(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_deconstructor_71(nargout, out, nargin-1, in+1);
       break;
     case 72:
-      overloadedGlobalFunction_72(nargout, out, nargin-1, in+1);
+      aGlobalFunction_72(nargout, out, nargin-1, in+1);
+      break;
+    case 73:
+      overloadedGlobalFunction_73(nargout, out, nargin-1, in+1);
+      break;
+    case 74:
+      overloadedGlobalFunction_74(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {
diff --git a/wrap/tests/expected2/overloadedGlobalFunction.m b/wrap/tests/expected2/overloadedGlobalFunction.m
index b242dbac0..24758ed6e 100644
--- a/wrap/tests/expected2/overloadedGlobalFunction.m
+++ b/wrap/tests/expected2/overloadedGlobalFunction.m
@@ -1,8 +1,8 @@
 function varargout = overloadedGlobalFunction(varargin)
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = geometry_wrapper(71, varargin{:});
+        varargout{1} = geometry_wrapper(73, varargin{:});
       elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
-        varargout{1} = geometry_wrapper(72, varargin{:});
+        varargout{1} = geometry_wrapper(74, varargin{:});
       else
         error('Arguments do not match any overload of function overloadedGlobalFunction');
       end
diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h
index 9bc56f1ed..8c5be7a3c 100644
--- a/wrap/tests/geometry.h
+++ b/wrap/tests/geometry.h
@@ -102,7 +102,7 @@ virtual class MyTemplate : MyBase {
   MyTemplate();
 
   template<ARG = {Point2, Point3}>
-  void templatedMethod(const Test& t);
+  void templatedMethod(const ARG& t);
 
   // Stress test templates and pointer combinations
   void accept_T(const T& value) const;
diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp
index 5ab0e00b8..775181bcc 100644
--- a/wrap/tests/testClass.cpp
+++ b/wrap/tests/testClass.cpp
@@ -30,28 +30,52 @@ TEST( Class, Constructor ) {
 }
 
 /* ************************************************************************* */
-// addMethodOverloads
-TEST( Class, addMethod ) {
+// test method overloading
+TEST( Class, OverloadingMethod ) {
   Class cls;
   const string name = "method1";
   EXPECT(!cls.exists(name));
 
-  bool verbose=true, is_const=true;
+  bool verbose = true, is_const = true;
   ArgumentList args;
   const ReturnValue retVal;
   const string templateArgName;
   vector<Qualified> templateArgValues;
   cls.addMethod(verbose, is_const, name, args, retVal, templateArgName,
       templateArgValues);
-  EXPECT_LONGS_EQUAL(1,cls.nrMethods());
+  EXPECT_LONGS_EQUAL(1, cls.nrMethods());
   EXPECT(cls.exists(name));
   Method& method = cls.method(name);
-  EXPECT_LONGS_EQUAL(1,method.returnVals.size());
+  EXPECT_LONGS_EQUAL(1, method.returnVals.size());
 
   cls.addMethod(verbose, is_const, name, args, retVal, templateArgName,
       templateArgValues);
-  EXPECT_LONGS_EQUAL(1,cls.nrMethods());
-  EXPECT_LONGS_EQUAL(2,method.returnVals.size());
+  EXPECT_LONGS_EQUAL(1, cls.nrMethods());
+  EXPECT_LONGS_EQUAL(2, method.returnVals.size());
+}
+
+/* ************************************************************************* */
+// test templated methods
+TEST( Class, TemplatedMethods ) {
+  Class cls;
+  const string name = "method";
+  EXPECT(!cls.exists(name));
+
+  bool verbose = true, is_const = true;
+  ArgumentList args;
+  Argument arg;
+  arg.type.name = "T";
+  args.push_back(arg);
+  const ReturnValue retVal(ReturnType("T"));
+  const string templateArgName("T");
+  vector<Qualified> templateArgValues;
+  templateArgValues.push_back(Qualified("Point2"));
+  templateArgValues.push_back(Qualified("Point3"));
+  cls.addMethod(verbose, is_const, name, args, retVal, templateArgName,
+      templateArgValues);
+  EXPECT_LONGS_EQUAL(2, cls.nrMethods());
+  EXPECT(cls.exists(name+"Point2"));
+  EXPECT(cls.exists(name+"Point3"));
 }
 
 /* ************************************************************************* */