diff --git a/wrap/tests/expected-python/geometry_python.cpp b/wrap/tests/expected-python/geometry_python.cpp
index f500f2984..b28e69709 100644
--- a/wrap/tests/expected-python/geometry_python.cpp
+++ b/wrap/tests/expected-python/geometry_python.cpp
@@ -59,6 +59,8 @@ class_<MyTemplatePoint2>("MyTemplatePoint2")
   .def("return_ptrs", &MyTemplatePoint2::return_ptrs);
   .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
   .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
+  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
+  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
 ;
 
 class_<MyTemplatePoint3>("MyTemplatePoint3")
@@ -72,6 +74,8 @@ class_<MyTemplatePoint3>("MyTemplatePoint3")
   .def("return_ptrs", &MyTemplatePoint3::return_ptrs);
   .def("templatedMethod", &MyTemplatePoint3::templatedMethod);
   .def("templatedMethod", &MyTemplatePoint3::templatedMethod);
+  .def("templatedMethod", &MyTemplatePoint3::templatedMethod);
+  .def("templatedMethod", &MyTemplatePoint3::templatedMethod);
 ;
 
 class_<MyFactorPosePoint2>("MyFactorPosePoint2")
diff --git a/wrap/tests/expected2/MyFactorPosePoint2.m b/wrap/tests/expected2/MyFactorPosePoint2.m
index 166e1514d..e55386cc0 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(69, my_ptr);
+        geometry_wrapper(73, 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(70, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = geometry_wrapper(74, 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(71, obj.ptr_MyFactorPosePoint2);
+      geometry_wrapper(75, 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 5f1c69480..9df4d2d1a 100644
--- a/wrap/tests/expected2/MyTemplatePoint2.m
+++ b/wrap/tests/expected2/MyTemplatePoint2.m
@@ -12,8 +12,10 @@
 %return_T(Point2 value) : returns gtsam::Point2
 %return_Tptr(Point2 value) : returns gtsam::Point2
 %return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
+%templatedMethodMatrix(Matrix t) : returns void
 %templatedMethodPoint2(Point2 t) : returns void
 %templatedMethodPoint3(Point3 t) : returns void
+%templatedMethodVector(Vector t) : returns void
 %
 classdef MyTemplatePoint2 < MyBase
   properties
@@ -107,11 +109,21 @@ classdef MyTemplatePoint2 < MyBase
       end
     end
 
+    function varargout = templatedMethodMatrix(this, varargin)
+      % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix 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},'double')
+        geometry_wrapper(54, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
+      end
+    end
+
     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},'gtsam.Point2')
-        geometry_wrapper(54, this, varargin{:});
+        geometry_wrapper(55, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
       end
@@ -121,7 +133,17 @@ classdef MyTemplatePoint2 < MyBase
       % 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},'gtsam.Point3')
-        geometry_wrapper(55, this, varargin{:});
+        geometry_wrapper(56, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
+      end
+    end
+
+    function varargout = templatedMethodVector(this, varargin)
+      % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector 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},'double')
+        geometry_wrapper(57, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
       end
diff --git a/wrap/tests/expected2/MyTemplatePoint3.m b/wrap/tests/expected2/MyTemplatePoint3.m
index 848e224fd..274768d3a 100644
--- a/wrap/tests/expected2/MyTemplatePoint3.m
+++ b/wrap/tests/expected2/MyTemplatePoint3.m
@@ -12,8 +12,10 @@
 %return_T(Point3 value) : returns gtsam::Point3
 %return_Tptr(Point3 value) : returns gtsam::Point3
 %return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
+%templatedMethodMatrix(Matrix t) : returns void
 %templatedMethodPoint2(Point2 t) : returns void
 %templatedMethodPoint3(Point3 t) : returns void
+%templatedMethodVector(Vector t) : returns void
 %
 classdef MyTemplatePoint3 < MyBase
   properties
@@ -25,11 +27,11 @@ classdef MyTemplatePoint3 < MyBase
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = geometry_wrapper(57, varargin{2});
+          my_ptr = geometry_wrapper(59, varargin{2});
         end
-        base_ptr = geometry_wrapper(56, my_ptr);
+        base_ptr = geometry_wrapper(58, my_ptr);
       elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(58);
+        [ my_ptr, base_ptr ] = geometry_wrapper(60);
       else
         error('Arguments do not match any overload of MyTemplatePoint3 constructor');
       end
@@ -38,7 +40,7 @@ classdef MyTemplatePoint3 < MyBase
     end
 
     function delete(obj)
-      geometry_wrapper(59, obj.ptr_MyTemplatePoint3);
+      geometry_wrapper(61, obj.ptr_MyTemplatePoint3);
     end
 
     function display(obj), obj.print(''); end
@@ -49,7 +51,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},'gtsam.Point3')
-        geometry_wrapper(60, this, varargin{:});
+        geometry_wrapper(62, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.accept_T');
       end
@@ -59,7 +61,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},'gtsam.Point3')
-        geometry_wrapper(61, this, varargin{:});
+        geometry_wrapper(63, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.accept_Tptr');
       end
@@ -68,20 +70,20 @@ classdef MyTemplatePoint3 < MyBase
     function varargout = create_MixedPtrs(this, varargin)
       % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::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(64, this, varargin{:});
     end
 
     function varargout = create_ptrs(this, varargin)
       % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      [ varargout{1} varargout{2} ] = geometry_wrapper(63, this, varargin{:});
+      [ varargout{1} varargout{2} ] = geometry_wrapper(65, this, varargin{:});
     end
 
     function varargout = return_T(this, varargin)
       % RETURN_T usage: return_T(Point3 value) : returns gtsam::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},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(64, this, varargin{:});
+        varargout{1} = geometry_wrapper(66, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.return_T');
       end
@@ -91,7 +93,7 @@ classdef MyTemplatePoint3 < MyBase
       % RETURN_TPTR usage: return_Tptr(Point3 value) : returns gtsam::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},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(65, this, varargin{:});
+        varargout{1} = geometry_wrapper(67, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.return_Tptr');
       end
@@ -101,17 +103,27 @@ classdef MyTemplatePoint3 < MyBase
       % RETURN_PTRS usage: return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::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},'gtsam.Point3') && isa(varargin{2},'gtsam.Point3')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(68, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.return_ptrs');
       end
     end
 
+    function varargout = templatedMethodMatrix(this, varargin)
+      % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix 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},'double')
+        geometry_wrapper(69, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
+      end
+    end
+
     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},'gtsam.Point2')
-        geometry_wrapper(67, this, varargin{:});
+        geometry_wrapper(70, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
       end
@@ -121,7 +133,17 @@ classdef MyTemplatePoint3 < MyBase
       % 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},'gtsam.Point3')
-        geometry_wrapper(68, this, varargin{:});
+        geometry_wrapper(71, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
+      end
+    end
+
+    function varargout = templatedMethodVector(this, varargin)
+      % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector 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},'double')
+        geometry_wrapper(72, this, varargin{:});
       else
         error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
       end
diff --git a/wrap/tests/expected2/aGlobalFunction.m b/wrap/tests/expected2/aGlobalFunction.m
index 5cf9aafa1..e000338b6 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(72, varargin{:});
+        varargout{1} = geometry_wrapper(76, 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 ab6ae5aa7..77559f3da 100644
--- a/wrap/tests/expected2/geometry_wrapper.cpp
+++ b/wrap/tests/expected2/geometry_wrapper.cpp
@@ -667,22 +667,40 @@ void MyTemplatePoint2_return_ptrs_53(int nargout, mxArray *out[], int nargin, co
 void MyTemplatePoint2_templatedMethod_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint2> Shared;
-  checkArguments("templatedMethod",nargout,nargin-1,1);
+  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
   Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  obj->templatedMethod<gtsam::Point2>(t);
+  Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
+  obj->templatedMethod<Matrix>(t);
 }
 
 void MyTemplatePoint2_templatedMethod_55(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");
+  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
+  obj->templatedMethod<gtsam::Point2>(t);
+}
+
+void MyTemplatePoint2_templatedMethod_56(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");
   gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
   obj->templatedMethod<gtsam::Point3>(t);
 }
 
-void MyTemplatePoint3_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
+  checkArguments("templatedMethodVector",nargout,nargin-1,1);
+  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector");
+  obj->templatedMethod<Vector>(t);
+}
+
+void MyTemplatePoint3_collectorInsertAndMakeBase_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -695,7 +713,7 @@ void MyTemplatePoint3_collectorInsertAndMakeBase_56(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
-void MyTemplatePoint3_upcastFromVoid_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void MyTemplatePoint3_upcastFromVoid_59(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]));
@@ -704,7 +722,7 @@ void MyTemplatePoint3_upcastFromVoid_57(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void MyTemplatePoint3_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -719,7 +737,7 @@ void MyTemplatePoint3_constructor_58(int nargout, mxArray *out[], int nargin, co
   *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
 }
 
-void MyTemplatePoint3_deconstructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_deconstructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   checkArguments("delete_MyTemplatePoint3",nargout,nargin,1);
@@ -732,7 +750,7 @@ void MyTemplatePoint3_deconstructor_59(int nargout, mxArray *out[], int nargin,
   }
 }
 
-void MyTemplatePoint3_accept_T_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_accept_T_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   checkArguments("accept_T",nargout,nargin-1,1);
@@ -741,7 +759,7 @@ void MyTemplatePoint3_accept_T_60(int nargout, mxArray *out[], int nargin, const
   obj->accept_T(value);
 }
 
-void MyTemplatePoint3_accept_Tptr_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_accept_Tptr_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
   checkArguments("accept_Tptr",nargout,nargin-1,1);
@@ -750,7 +768,7 @@ void MyTemplatePoint3_accept_Tptr_61(int nargout, mxArray *out[], int nargin, co
   obj->accept_Tptr(value);
 }
 
-void MyTemplatePoint3_create_MixedPtrs_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_create_MixedPtrs_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
@@ -762,7 +780,7 @@ void MyTemplatePoint3_create_MixedPtrs_62(int nargout, mxArray *out[], int nargi
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
 }
 
-void MyTemplatePoint3_create_ptrs_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_create_ptrs_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
@@ -774,7 +792,7 @@ void MyTemplatePoint3_create_ptrs_63(int nargout, mxArray *out[], int nargin, co
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
 }
 
-void MyTemplatePoint3_return_T_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_return_T_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -784,7 +802,7 @@ void MyTemplatePoint3_return_T_64(int nargout, mxArray *out[], int nargin, const
   out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->return_T(value))),"gtsam.Point3", false);
 }
 
-void MyTemplatePoint3_return_Tptr_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_return_Tptr_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
@@ -794,7 +812,7 @@ void MyTemplatePoint3_return_Tptr_65(int nargout, mxArray *out[], int nargin, co
   out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point3", false);
 }
 
-void MyTemplatePoint3_return_ptrs_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_return_ptrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
   typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
@@ -808,25 +826,43 @@ void MyTemplatePoint3_return_ptrs_66(int nargout, mxArray *out[], int nargin, co
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
 }
 
-void MyTemplatePoint3_templatedMethod_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_templatedMethod_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
-  checkArguments("templatedMethod",nargout,nargin-1,1);
+  checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
+  Shared obj = unwrap_shared_ptr<MyTemplatePoint3>(in[0], "ptr_MyTemplatePoint3");
+  Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix");
+  obj->templatedMethod<Matrix>(t);
+}
+
+void MyTemplatePoint3_templatedMethod_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplatePoint3> Shared;
+  checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
   Shared obj = unwrap_shared_ptr<MyTemplatePoint3>(in[0], "ptr_MyTemplatePoint3");
   gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
   obj->templatedMethod<gtsam::Point2>(t);
 }
 
-void MyTemplatePoint3_templatedMethod_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_templatedMethod_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplatePoint3> Shared;
-  checkArguments("templatedMethod",nargout,nargin-1,1);
+  checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
   Shared obj = unwrap_shared_ptr<MyTemplatePoint3>(in[0], "ptr_MyTemplatePoint3");
   gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
   obj->templatedMethod<gtsam::Point3>(t);
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint3_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<MyTemplatePoint3> Shared;
+  checkArguments("templatedMethodVector",nargout,nargin-1,1);
+  Shared obj = unwrap_shared_ptr<MyTemplatePoint3>(in[0], "ptr_MyTemplatePoint3");
+  Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector");
+  obj->templatedMethod<Vector>(t);
+}
+
+void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
@@ -835,7 +871,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_69(int nargout, mxArray *out[
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
@@ -850,7 +886,7 @@ void MyFactorPosePoint2_constructor_70(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyFactorPosePoint2> Shared;
   checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
@@ -863,18 +899,18 @@ void MyFactorPosePoint2_deconstructor_71(int nargout, mxArray *out[], int nargin
   }
 }
 
-void aGlobalFunction_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void aGlobalFunction_76(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("aGlobalFunction",nargout,nargin,0);
   out[0] = wrap< Vector >(aGlobalFunction());
 }
-void overloadedGlobalFunction_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_77(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_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_78(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,2);
   int a = unwrap< int >(in[0]);
@@ -1062,61 +1098,73 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplatePoint2_templatedMethod_55(nargout, out, nargin-1, in+1);
       break;
     case 56:
-      MyTemplatePoint3_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_templatedMethod_56(nargout, out, nargin-1, in+1);
       break;
     case 57:
-      MyTemplatePoint3_upcastFromVoid_57(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_templatedMethod_57(nargout, out, nargin-1, in+1);
       break;
     case 58:
-      MyTemplatePoint3_constructor_58(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_collectorInsertAndMakeBase_58(nargout, out, nargin-1, in+1);
       break;
     case 59:
-      MyTemplatePoint3_deconstructor_59(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_upcastFromVoid_59(nargout, out, nargin-1, in+1);
       break;
     case 60:
-      MyTemplatePoint3_accept_T_60(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_constructor_60(nargout, out, nargin-1, in+1);
       break;
     case 61:
-      MyTemplatePoint3_accept_Tptr_61(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_deconstructor_61(nargout, out, nargin-1, in+1);
       break;
     case 62:
-      MyTemplatePoint3_create_MixedPtrs_62(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_accept_T_62(nargout, out, nargin-1, in+1);
       break;
     case 63:
-      MyTemplatePoint3_create_ptrs_63(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_accept_Tptr_63(nargout, out, nargin-1, in+1);
       break;
     case 64:
-      MyTemplatePoint3_return_T_64(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_create_MixedPtrs_64(nargout, out, nargin-1, in+1);
       break;
     case 65:
-      MyTemplatePoint3_return_Tptr_65(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_create_ptrs_65(nargout, out, nargin-1, in+1);
       break;
     case 66:
-      MyTemplatePoint3_return_ptrs_66(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_return_T_66(nargout, out, nargin-1, in+1);
       break;
     case 67:
-      MyTemplatePoint3_templatedMethod_67(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_return_Tptr_67(nargout, out, nargin-1, in+1);
       break;
     case 68:
-      MyTemplatePoint3_templatedMethod_68(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_return_ptrs_68(nargout, out, nargin-1, in+1);
       break;
     case 69:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_69(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_templatedMethod_69(nargout, out, nargin-1, in+1);
       break;
     case 70:
-      MyFactorPosePoint2_constructor_70(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_templatedMethod_70(nargout, out, nargin-1, in+1);
       break;
     case 71:
-      MyFactorPosePoint2_deconstructor_71(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_templatedMethod_71(nargout, out, nargin-1, in+1);
       break;
     case 72:
-      aGlobalFunction_72(nargout, out, nargin-1, in+1);
+      MyTemplatePoint3_templatedMethod_72(nargout, out, nargin-1, in+1);
       break;
     case 73:
-      overloadedGlobalFunction_73(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1);
       break;
     case 74:
-      overloadedGlobalFunction_74(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_constructor_74(nargout, out, nargin-1, in+1);
+      break;
+    case 75:
+      MyFactorPosePoint2_deconstructor_75(nargout, out, nargin-1, in+1);
+      break;
+    case 76:
+      aGlobalFunction_76(nargout, out, nargin-1, in+1);
+      break;
+    case 77:
+      overloadedGlobalFunction_77(nargout, out, nargin-1, in+1);
+      break;
+    case 78:
+      overloadedGlobalFunction_78(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 24758ed6e..fb912a880 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(73, varargin{:});
+        varargout{1} = geometry_wrapper(77, varargin{:});
       elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
-        varargout{1} = geometry_wrapper(74, varargin{:});
+        varargout{1} = geometry_wrapper(78, 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 5a6cee1a5..f6465fa95 100644
--- a/wrap/tests/geometry.h
+++ b/wrap/tests/geometry.h
@@ -104,7 +104,7 @@ template<T = {gtsam::Point2, gtsam::Point3}>
 virtual class MyTemplate : MyBase {
   MyTemplate();
 
-  template<ARG = {gtsam::Point2, gtsam::Point3}>
+  template<ARG = {gtsam::Point2, gtsam::Point3, Vector, Matrix}>
   void templatedMethod(const ARG& t);
 
   // Stress test templates and pointer combinations