diff --git a/interface_parser.py b/interface_parser.py
index 98180c9..73f9eba 100644
--- a/interface_parser.py
+++ b/interface_parser.py
@@ -86,14 +86,17 @@ class Typename(object):
         )
     ).setParseAction(lambda t: Typename(t.namespaces_name, t.instantiations))
 
-    def __init__(self, namespaces_name, instantiations=''):
+    def __init__(self, namespaces_name, instantiations=[]):
         self.namespaces = namespaces_name[:-1]
         self.name = namespaces_name[-1]
 
         if instantiations:
-            self.instantiations = instantiations.asList()
+            if not isinstance(instantiations, list):
+                self.instantiations = instantiations.asList()
+            else:
+                self.instantiations = instantiations
         else:
-            self.instantiations = ''
+            self.instantiations = []
         if self.name in ["Matrix", "Vector"] and not self.namespaces:
             self.namespaces = ["gtsam"]
 
diff --git a/matlab_wrapper.py b/matlab_wrapper.py
index ab6dde7..cb9c913 100755
--- a/matlab_wrapper.py
+++ b/matlab_wrapper.py
@@ -50,7 +50,9 @@ class MatlabWrapper(object):
     """Methods that should not be wrapped directly"""
     whitelist = ['serializable', 'serialize']
     """Datatypes that do not need to be checked in methods"""
-    not_check_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
+    not_check_type = []
+    """Data types that are primitive types"""
+    not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
     """Ignore the namespace for these datatypes"""
     ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3']
     """The amount of times the wrapper has created a call to
@@ -75,7 +77,7 @@ def __init__(self, module, module_name, top_module_namespace='', ignore_classes=
         self.module_name = module_name
         self.top_module_namespace = top_module_namespace
         self.ignore_classes = ignore_classes
-        self.verbose = True
+        self.verbose = False
 
     def _debug(self, message):
         if not self.verbose:
@@ -138,7 +140,7 @@ def _is_ptr(self, arg_type):
         """Determine if the interface_parser.Type should be treated as a
         pointer in the wrapper.
         """
-        return arg_type.is_ptr or (arg_type.typename.name not in self.not_check_type
+        return arg_type.is_ptr or (arg_type.typename.name not in self.not_ptr_type
                                    and arg_type.typename.name not in self.ignore_namespace
                                    and arg_type.typename.name != 'string')
 
@@ -195,9 +197,10 @@ def _format_type_name(self, type_name, separator='::', include_namespace=True, c
 
         if include_namespace:
             for namespace in type_name.namespaces:
-                if name not in self.ignore_namespace:
+                if name not in self.ignore_namespace and namespace != '':
                     formatted_type_name += namespace + separator
 
+            #self._debug("formatted_ns: {}, ns: {}".format(formatted_type_name, type_name.namespaces))
         if constructor:
             formatted_type_name += self.data_type.get(name) or name
         elif method:
@@ -205,9 +208,21 @@ def _format_type_name(self, type_name, separator='::', include_namespace=True, c
         else:
             formatted_type_name += name
 
+        if len(type_name.instantiations) == 1:
+            if separator == "::":  # C++
+                formatted_type_name += '<{}>'.format(self._format_type_name(type_name.instantiations[0],
+                                                                            include_namespace=include_namespace,
+                                                                            constructor=constructor, method=method))
+            else:
+                formatted_type_name += '{}'.format(self._format_type_name(
+                    type_name.instantiations[0],
+                    separator=separator,
+                    include_namespace=False,
+                    constructor=constructor, method=method
+                ))
         return formatted_type_name
 
-    def _format_return_type(self, return_type, include_namespace=False):
+    def _format_return_type(self, return_type, include_namespace=False, separator="::"):
         """Format return_type.
 
         Args:
@@ -217,11 +232,19 @@ def _format_return_type(self, return_type, include_namespace=False):
         return_wrap = ''
 
         if self._return_count(return_type) == 1:
-            return_wrap = self._format_type_name(return_type.type1.typename, include_namespace=include_namespace)
+            return_wrap = self._format_type_name(
+                return_type.type1.typename,
+                separator=separator,
+                include_namespace=include_namespace
+            )
         else:
             return_wrap = 'pair< {type1}, {type2} >'.format(
-                type1=self._format_type_name(return_type.type1.typename, include_namespace=include_namespace),
-                type2=self._format_type_name(return_type.type2.typename, include_namespace=include_namespace))
+                type1=self._format_type_name(
+                    return_type.type1.typename, separator=separator, include_namespace=include_namespace
+                ),
+                type2=self._format_type_name(
+                    return_type.type2.typename, separator=separator, include_namespace=include_namespace
+                ))
 
         return return_wrap
 
@@ -238,10 +261,10 @@ def _format_class_name(self, instantiated_class, separator=''):
         #
         # class_name += instantiated_class.name
         parentname = "".join([separator + x for x in parent_full_ns]) + separator
+
         class_name = parentname[2 * len(separator):]
 
-        if class_name != '':
-            class_name += instantiated_class.name
+        class_name += instantiated_class.name
 
         return class_name
 
@@ -322,6 +345,9 @@ def _wrap_variable_arguments(self, args, wrap_datatypes=True):
 
             check_type = self.data_type_param.get(name)
 
+            if self.data_type.get(check_type):
+                check_type = self.data_type[check_type]
+
             if check_type is None:
                 check_type = self._format_type_name(arg.ctype.typename, separator='.', constructor=not wrap_datatypes)
 
@@ -379,6 +405,9 @@ def _wrap_method_check_statement(self, args):
 
             check_type = self.data_type_param.get(name)
 
+            if self.data_type.get(check_type):
+                check_type = self.data_type[check_type]
+
             if check_type is None:
                 check_type = self._format_type_name(arg.ctype.typename, separator='.')
 
@@ -717,15 +746,17 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name, ctors
         if base_obj:
             base_obj = '\n' + base_obj
 
+        self._debug("class: {}, name: {}".format(inst_class.name, self._format_class_name(inst_class, separator=".")))
         methods_wrap += textwrap.indent(textwrap.dedent('''\
               else
-                error('Arguments do not match any overload of {class_name} constructor');
+                error('Arguments do not match any overload of {class_name_doc} constructor');
               end{base_obj}
               obj.ptr_{class_name} = my_ptr;
             end\n
         ''').format(namespace=namespace_name,
                     d='' if namespace_name == '' else '.',
-                    class_name="".join(inst_class.parent.full_namespaces()) + class_name,
+                    class_name_doc=self._format_class_name(inst_class, separator="."),
+                    class_name=self._format_class_name(inst_class, separator=""),
                     base_obj=base_obj),
                                         prefix='  ')
 
@@ -817,7 +848,7 @@ def wrap_class_methods(self, namespace_name, inst_class, methods, serialize=[Fal
                                                    prefix='  ')
 
                     # Determine format of return and varargout statements
-                    return_type = self._format_return_type(overload.return_type, include_namespace=True)
+                    return_type = self._format_return_type(overload.return_type, include_namespace=True, separator=".")
 
                     if self._return_count(overload.return_type) == 1:
                         varargout = '' \
@@ -854,7 +885,7 @@ def wrap_class_methods(self, namespace_name, inst_class, methods, serialize=[Fal
 
                 final_statement = textwrap.indent(textwrap.dedent("""\
                     error('Arguments do not match any overload of function {class_name}.{method_name}');
-                """),
+                """.format(class_name=class_name, method_name=method_name)),
                                                   prefix='  ')
                 method_text += final_statement + 'end\n\n'
 
@@ -895,7 +926,7 @@ def wrap_static_methods(self, namespace_name, instantiated_class, serialize):
                                   name_upper_case=static_overload.name,
                                   args=self._wrap_args(static_overload.args),
                                   return_type=self._format_return_type(static_overload.return_type,
-                                                                       include_namespace=True),
+                                                                       include_namespace=True, separator="."),
                                   length=len(static_overload.args.args_list),
                                   var_args_list=self._wrap_variable_arguments(static_overload.args),
                                   check_statement=check_statement,
@@ -909,7 +940,7 @@ def wrap_static_methods(self, namespace_name, instantiated_class, serialize):
 
             method_text += textwrap.indent(textwrap.dedent("""\
                     error('Arguments do not match any overload of function {class_name}.{method_name}');
-                """),
+                """.format(class_name=class_name, method_name=static_overload.name)),
                                            prefix='    ')
             method_text += textwrap.indent(textwrap.dedent('''\
                                     end\n
@@ -1161,11 +1192,9 @@ def wrap_collector_function_return(self, method):
                         shared_obj = '{obj},"{method_name_sep}"'.format(obj=obj, method_name_sep=sep_method_name('.'))
                     else:
                         self._debug("Non-PTR: {}, {}".format(return_1, type(return_1)))
-                        # FIXME: This is a very dirty hack!!!
-                        if isinstance(return_1, instantiator.InstantiatedClass):
-                            method_name_sep_dot = "".join(return_1.parent.full_namespaces()) + return_1.name
-                        else:
-                            method_name_sep_dot = sep_method_name('.')
+                        self._debug("Inner type is: {}, {}".format(return_1.typename.name, sep_method_name('.')))
+                        self._debug("Inner type instantiations: {}".format(return_1.typename.instantiations))
+                        method_name_sep_dot = sep_method_name('.')
                         shared_obj = 'boost::make_shared<{method_name_sep_col}>({obj}),"{method_name_sep_dot}"' \
                             .format(
                             method_name=return_1.typename.name,
@@ -1427,7 +1456,7 @@ def generate_wrapper(self, namespace):
               bool anyDeleted = false;
         ''')
         rtti_reg_start = textwrap.dedent('''\
-            void _gtsam_RTTIRegister() {{
+            void _{module_name}_RTTIRegister() {{
               const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created");
               if(!alreadyCreated) {{
                 std::map<std::string, std::string> types;
diff --git a/template_instantiator.py b/template_instantiator.py
index 2b0b943..3d98e96 100644
--- a/template_instantiator.py
+++ b/template_instantiator.py
@@ -1,7 +1,7 @@
 import interface_parser as parser
 
 
-def instantiate_type(ctype, template_typenames, instantiations, cpp_typename):
+def instantiate_type(ctype, template_typenames, instantiations, cpp_typename, instantiated_class=None):
     """
     Instantiate template typename for @p ctype.
     @return If ctype's name is in the @p template_typenames, return the
@@ -20,6 +20,17 @@ def instantiate_type(ctype, template_typenames, instantiations, cpp_typename):
             is_basis=ctype.is_basis,
         )
     elif str_arg_typename == 'This':
+        # import sys
+        if instantiated_class:
+            name = instantiated_class.original.name
+            namespaces_name = instantiated_class.namespaces()
+            namespaces_name.append(name)
+            # print("INST: {}, {}, CPP: {}, CLS: {}".format(
+            #     ctype, instantiations, cpp_typename, instantiated_class.instantiations
+            # ), file=sys.stderr)
+            cpp_typename = parser.Typename(
+                namespaces_name, instantiations=[inst for inst in instantiated_class.instantiations]
+            )
         return parser.Type(
             typename=cpp_typename,
             is_const=ctype.is_const,
@@ -58,13 +69,13 @@ def instantiate_args_list(args_list, template_typenames, instantiations,
 
 
 def instantiate_return_type(return_type, template_typenames, instantiations,
-                            cpp_typename):
+                            cpp_typename, instantiated_class=None):
     new_type1 = instantiate_type(
-        return_type.type1, template_typenames, instantiations, cpp_typename)
+        return_type.type1, template_typenames, instantiations, cpp_typename, instantiated_class=instantiated_class)
     if return_type.type2:
         new_type2 = instantiate_type(
             return_type.type2, template_typenames, instantiations,
-            cpp_typename)
+            cpp_typename, instantiated_class=instantiated_class)
     else:
         new_type2 = ''
     return parser.ReturnType(new_type1, new_type2)
@@ -215,7 +226,7 @@ def instantiate_static_methods(self):
                 static_method.args.args_list,
                 self.original.template.typenames,
                 self.instantiations,
-                self.cpp_typename(),
+                self.cpp_typename()
             )
             instantiated_static_methods.append(
                 parser.StaticMethod(
@@ -225,6 +236,7 @@ def instantiate_static_methods(self):
                         self.original.template.typenames,
                         self.instantiations,
                         self.cpp_typename(),
+                        instantiated_class=self
                     ),
                     args=parser.ArgumentList(instantiated_args),
                     parent=self,
diff --git a/tests/expected-matlab/+gtsam/Point2.m b/tests/expected-matlab/+gtsam/Point2.m
index 9d0c8c6..d80a3b0 100644
--- a/tests/expected-matlab/+gtsam/Point2.m
+++ b/tests/expected-matlab/+gtsam/Point2.m
@@ -29,7 +29,7 @@
       elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
         my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
       else
-        error('Arguments do not match any overload of gtsamPoint2 constructor');
+        error('Arguments do not match any overload of gtsam.Point2 constructor');
       end
       obj.ptr_gtsamPoint2 = my_ptr;
     end
@@ -45,21 +45,21 @@ function delete(obj)
     function varargout = argChar(this, varargin)
       % ARGCHAR usage: argChar(char a) : returns void
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'char')
         geometry_wrapper(4, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.argChar');
     end
 
     function varargout = argUChar(this, varargin)
       % ARGUCHAR usage: argUChar(unsigned char a) : returns void
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'unsigned char')
         geometry_wrapper(5, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.argUChar');
     end
 
     function varargout = dim(this, varargin)
@@ -69,7 +69,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(6, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.dim');
     end
 
     function varargout = eigenArguments(this, varargin)
@@ -79,7 +79,7 @@ function delete(obj)
         geometry_wrapper(7, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.eigenArguments');
     end
 
     function varargout = returnChar(this, varargin)
@@ -89,7 +89,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(8, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.returnChar');
     end
 
     function varargout = vectorConfusion(this, varargin)
@@ -99,7 +99,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(9, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.vectorConfusion');
     end
 
     function varargout = x(this, varargin)
@@ -109,7 +109,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(10, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.x');
     end
 
     function varargout = y(this, varargin)
@@ -119,7 +119,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(11, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point2.y');
     end
 
   end
diff --git a/tests/expected-matlab/+gtsam/Point3.m b/tests/expected-matlab/+gtsam/Point3.m
index 72bb72d..a2f9a20 100644
--- a/tests/expected-matlab/+gtsam/Point3.m
+++ b/tests/expected-matlab/+gtsam/Point3.m
@@ -8,7 +8,7 @@
 %norm() : returns double
 %
 %-------Static Methods-------
-%StaticFunctionRet(double z) : returns gtsam::Point3
+%StaticFunctionRet(double z) : returns Point3
 %staticFunction() : returns double
 %
 %-------Serialization Interface-------
@@ -27,7 +27,7 @@
       elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
         my_ptr = geometry_wrapper(13, varargin{1}, varargin{2}, varargin{3});
       else
-        error('Arguments do not match any overload of gtsamPoint3 constructor');
+        error('Arguments do not match any overload of gtsam.Point3 constructor');
       end
       obj.ptr_gtsamPoint3 = my_ptr;
     end
@@ -47,7 +47,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(15, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function gtsam.Point3.norm');
     end
 
     function varargout = string_serialize(this, varargin)
@@ -68,14 +68,14 @@ function delete(obj)
 
   methods(Static = true)
     function varargout = StaticFunctionRet(varargin)
-      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
+      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns Point3
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'double')
         varargout{1} = geometry_wrapper(17, varargin{:});
         return
       end
 
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Point3.StaticFunctionRet');
     end
 
     function varargout = StaticFunction(varargin)
@@ -86,7 +86,7 @@ function delete(obj)
         return
       end
 
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Point3.staticFunction');
     end
 
     function varargout = string_deserialize(varargin)
diff --git a/tests/expected-matlab/MyFactorPosePoint2.m b/tests/expected-matlab/MyFactorPosePoint2.m
index b55488b..dc8538b 100644
--- a/tests/expected-matlab/MyFactorPosePoint2.m
+++ b/tests/expected-matlab/MyFactorPosePoint2.m
@@ -12,9 +12,9 @@
     function obj = MyFactorPosePoint2(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(82, my_ptr);
+        geometry_wrapper(84, 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(83, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = geometry_wrapper(85, varargin{1}, varargin{2}, varargin{3}, varargin{4});
       else
         error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
       end
@@ -22,7 +22,7 @@
     end
 
     function delete(obj)
-      geometry_wrapper(84, obj.ptr_MyFactorPosePoint2);
+      geometry_wrapper(86, obj.ptr_MyFactorPosePoint2);
     end
 
     function display(obj), obj.print(''); end
diff --git a/tests/expected-matlab/MyTemplateMatrix.m b/tests/expected-matlab/MyTemplateMatrix.m
index d5873d8..06d4543 100644
--- a/tests/expected-matlab/MyTemplateMatrix.m
+++ b/tests/expected-matlab/MyTemplateMatrix.m
@@ -13,10 +13,17 @@
 %return_Tptr(Matrix value) : returns Matrix
 %return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
 %templatedMethodMatrix(Matrix t) : returns Matrix
-%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
+%templatedMethodPoint2(Point2 t) : returns Point2
+%templatedMethodPoint3(Point3 t) : returns Point3
 %templatedMethodVector(Vector t) : returns Vector
 %
+%-------Static Methods-------
+%Level(Matrix K) : returns MyTemplate<Matrix>
+%
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns MyTemplateMatrix
+%
 classdef MyTemplateMatrix < MyBase
   properties
     ptr_MyTemplateMatrix = 0
@@ -27,11 +34,11 @@
         if nargin == 2
           my_ptr = varargin{2};
         else
-          my_ptr = geometry_wrapper(62, varargin{2});
+          my_ptr = geometry_wrapper(63, varargin{2});
         end
-        base_ptr = geometry_wrapper(61, my_ptr);
+        base_ptr = geometry_wrapper(62, my_ptr);
       elseif nargin == 0
-        [ my_ptr, base_ptr ] = geometry_wrapper(63);
+        [ my_ptr, base_ptr ] = geometry_wrapper(64);
       else
         error('Arguments do not match any overload of MyTemplateMatrix constructor');
       end
@@ -40,7 +47,7 @@
     end
 
     function delete(obj)
-      geometry_wrapper(64, obj.ptr_MyTemplateMatrix);
+      geometry_wrapper(65, obj.ptr_MyTemplateMatrix);
     end
 
     function display(obj), obj.print(''); end
@@ -51,114 +58,125 @@ function delete(obj)
       % ACCEPT_T usage: accept_T(Matrix 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},'double')
-        geometry_wrapper(65, this, varargin{:});
+        geometry_wrapper(66, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.accept_T');
     end
 
     function varargout = accept_Tptr(this, varargin)
       % ACCEPT_TPTR usage: accept_Tptr(Matrix 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},'double')
-        geometry_wrapper(66, this, varargin{:});
+        geometry_wrapper(67, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.accept_Tptr');
     end
 
     function varargout = create_MixedPtrs(this, varargin)
       % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Matrix, Matrix >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = geometry_wrapper(67, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(68, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.create_MixedPtrs');
     end
 
     function varargout = create_ptrs(this, varargin)
       % CREATE_PTRS usage: create_ptrs() : returns pair< Matrix, Matrix >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = geometry_wrapper(68, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(69, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.create_ptrs');
     end
 
     function varargout = return_T(this, varargin)
       % RETURN_T usage: return_T(Matrix value) : returns Matrix
       % 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')
-        varargout{1} = geometry_wrapper(69, this, varargin{:});
+        varargout{1} = geometry_wrapper(70, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.return_T');
     end
 
     function varargout = return_Tptr(this, varargin)
       % RETURN_TPTR usage: return_Tptr(Matrix value) : returns Matrix
       % 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')
-        varargout{1} = geometry_wrapper(70, this, varargin{:});
+        varargout{1} = geometry_wrapper(71, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.return_Tptr');
     end
 
     function varargout = return_ptrs(this, varargin)
       % RETURN_PTRS usage: return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(71, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(72, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.return_ptrs');
     end
 
     function varargout = templatedMethodMatrix(this, varargin)
       % TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
       % 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')
-        varargout{1} = geometry_wrapper(72, this, varargin{:});
+        varargout{1} = geometry_wrapper(73, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodMatrix');
     end
 
     function varargout = templatedMethodPoint2(this, varargin)
-      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns Point2
       % 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')
-        varargout{1} = geometry_wrapper(73, this, varargin{:});
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(74, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodPoint2');
     end
 
     function varargout = templatedMethodPoint3(this, varargin)
-      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : 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},'gtsam.Point3')
-        varargout{1} = geometry_wrapper(74, this, varargin{:});
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==3 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(75, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodPoint3');
     end
 
     function varargout = templatedMethodVector(this, varargin)
       % TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
       % 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') && size(varargin{1},2)==1
-        varargout{1} = geometry_wrapper(75, this, varargin{:});
+        varargout{1} = geometry_wrapper(76, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethodVector');
     end
 
   end
 
   methods(Static = true)
+    function varargout = Level(varargin)
+      % LEVEL usage: Level(Matrix K) : returns MyTemplateMatrix
+      % 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')
+        varargout{1} = geometry_wrapper(77, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function MyTemplateMatrix.Level');
+    end
+
   end
 end
diff --git a/tests/expected-matlab/MyTemplatePoint2.m b/tests/expected-matlab/MyTemplatePoint2.m
index d3a1cbc..a0cb8c7 100644
--- a/tests/expected-matlab/MyTemplatePoint2.m
+++ b/tests/expected-matlab/MyTemplatePoint2.m
@@ -7,16 +7,23 @@
 %-------Methods-------
 %accept_T(Point2 value) : returns void
 %accept_Tptr(Point2 value) : returns void
-%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
-%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 >
+%create_MixedPtrs() : returns pair< Point2, Point2 >
+%create_ptrs() : returns pair< Point2, Point2 >
+%return_T(Point2 value) : returns Point2
+%return_Tptr(Point2 value) : returns Point2
+%return_ptrs(Point2 p1, Point2 p2) : returns pair< Point2, Point2 >
 %templatedMethodMatrix(Matrix t) : returns Matrix
-%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
-%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
+%templatedMethodPoint2(Point2 t) : returns Point2
+%templatedMethodPoint3(Point3 t) : returns Point3
 %templatedMethodVector(Vector t) : returns Vector
 %
+%-------Static Methods-------
+%Level(Point2 K) : returns MyTemplate<Point2>
+%
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns MyTemplatePoint2
+%
 classdef MyTemplatePoint2 < MyBase
   properties
     ptr_MyTemplatePoint2 = 0
@@ -50,71 +57,71 @@ function delete(obj)
     function varargout = accept_T(this, varargin)
       % ACCEPT_T usage: accept_T(Point2 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.Point2')
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
         geometry_wrapper(50, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
     end
 
     function varargout = accept_Tptr(this, varargin)
       % ACCEPT_TPTR usage: accept_Tptr(Point2 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.Point2')
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
         geometry_wrapper(51, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
     end
 
     function varargout = create_MixedPtrs(this, varargin)
-      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
+      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Point2, Point2 >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 0
         [ varargout{1} varargout{2} ] = geometry_wrapper(52, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.create_MixedPtrs');
     end
 
     function varargout = create_ptrs(this, varargin)
-      % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
+      % CREATE_PTRS usage: create_ptrs() : returns pair< Point2, Point2 >
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
       if length(varargin) == 0
         [ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.create_ptrs');
     end
 
     function varargout = return_T(this, varargin)
-      % RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
+      % RETURN_T usage: return_T(Point2 value) : returns Point2
       % 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')
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
         varargout{1} = geometry_wrapper(54, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
     end
 
     function varargout = return_Tptr(this, varargin)
-      % RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
+      % RETURN_TPTR usage: return_Tptr(Point2 value) : returns Point2
       % 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')
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
         varargout{1} = geometry_wrapper(55, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
     end
 
     function varargout = return_ptrs(this, varargin)
-      % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
+      % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< Point2, Point2 >
       % 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.Point2') && isa(varargin{2},'gtsam.Point2')
+      if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1 && isa(varargin{2},'double') && size(varargin{2},1)==2 && size(varargin{2},2)==1
         [ varargout{1} varargout{2} ] = geometry_wrapper(56, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
     end
 
     function varargout = templatedMethodMatrix(this, varargin)
@@ -124,27 +131,27 @@ function delete(obj)
         varargout{1} = geometry_wrapper(57, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodMatrix');
     end
 
     function varargout = templatedMethodPoint2(this, varargin)
-      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns Point2
       % 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')
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==2 && size(varargin{1},2)==1
         varargout{1} = geometry_wrapper(58, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodPoint2');
     end
 
     function varargout = templatedMethodPoint3(this, varargin)
-      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : 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},'gtsam.Point3')
+      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},1)==3 && size(varargin{1},2)==1
         varargout{1} = geometry_wrapper(59, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodPoint3');
     end
 
     function varargout = templatedMethodVector(this, varargin)
@@ -154,11 +161,22 @@ function delete(obj)
         varargout{1} = geometry_wrapper(60, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethodVector');
     end
 
   end
 
   methods(Static = true)
+    function varargout = Level(varargin)
+      % LEVEL usage: Level(Point2 K) : returns MyTemplatePoint2
+      % 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') && size(varargin{1},1)==2 && size(varargin{1},2)==1
+        varargout{1} = geometry_wrapper(61, varargin{:});
+        return
+      end
+
+      error('Arguments do not match any overload of function MyTemplatePoint2.Level');
+    end
+
   end
 end
diff --git a/tests/expected-matlab/MyVector12.m b/tests/expected-matlab/MyVector12.m
index 9d32c28..e5afd4e 100644
--- a/tests/expected-matlab/MyVector12.m
+++ b/tests/expected-matlab/MyVector12.m
@@ -12,9 +12,9 @@
     function obj = MyVector12(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(79, my_ptr);
+        geometry_wrapper(81, my_ptr);
       elseif nargin == 0
-        my_ptr = geometry_wrapper(80);
+        my_ptr = geometry_wrapper(82);
       else
         error('Arguments do not match any overload of MyVector12 constructor');
       end
@@ -22,7 +22,7 @@
     end
 
     function delete(obj)
-      geometry_wrapper(81, obj.ptr_MyVector12);
+      geometry_wrapper(83, obj.ptr_MyVector12);
     end
 
     function display(obj), obj.print(''); end
diff --git a/tests/expected-matlab/MyVector3.m b/tests/expected-matlab/MyVector3.m
index ef4b90a..e660ef2 100644
--- a/tests/expected-matlab/MyVector3.m
+++ b/tests/expected-matlab/MyVector3.m
@@ -12,9 +12,9 @@
     function obj = MyVector3(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(76, my_ptr);
+        geometry_wrapper(78, my_ptr);
       elseif nargin == 0
-        my_ptr = geometry_wrapper(77);
+        my_ptr = geometry_wrapper(79);
       else
         error('Arguments do not match any overload of MyVector3 constructor');
       end
@@ -22,7 +22,7 @@
     end
 
     function delete(obj)
-      geometry_wrapper(78, obj.ptr_MyVector3);
+      geometry_wrapper(80, obj.ptr_MyVector3);
     end
 
     function display(obj), obj.print(''); end
diff --git a/tests/expected-matlab/Test.m b/tests/expected-matlab/Test.m
index 81c6647..4d2a674 100644
--- a/tests/expected-matlab/Test.m
+++ b/tests/expected-matlab/Test.m
@@ -10,7 +10,7 @@
 %create_MixedPtrs() : returns pair< Test, Test >
 %create_ptrs() : returns pair< Test, Test >
 %print() : returns void
-%return_Point2Ptr(bool value) : returns gtsam::Point2
+%return_Point2Ptr(bool value) : returns Point2
 %return_Test(Test value) : returns Test
 %return_TestPtr(Test value) : returns Test
 %return_bool(bool value) : returns bool
@@ -60,7 +60,7 @@ function delete(obj)
         geometry_wrapper(24, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.arg_EigenConstRef');
     end
 
     function varargout = create_MixedPtrs(this, varargin)
@@ -70,7 +70,7 @@ function delete(obj)
         [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.create_MixedPtrs');
     end
 
     function varargout = create_ptrs(this, varargin)
@@ -80,7 +80,7 @@ function delete(obj)
         [ varargout{1} varargout{2} ] = geometry_wrapper(26, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.create_ptrs');
     end
 
     function varargout = print(this, varargin)
@@ -90,17 +90,17 @@ function delete(obj)
         geometry_wrapper(27, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.print');
     end
 
     function varargout = return_Point2Ptr(this, varargin)
-      % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns gtsam::Point2
+      % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'logical')
         varargout{1} = geometry_wrapper(28, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_Point2Ptr');
     end
 
     function varargout = return_Test(this, varargin)
@@ -110,7 +110,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(29, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_Test');
     end
 
     function varargout = return_TestPtr(this, varargin)
@@ -120,27 +120,27 @@ function delete(obj)
         varargout{1} = geometry_wrapper(30, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_TestPtr');
     end
 
     function varargout = return_bool(this, varargin)
       % RETURN_BOOL usage: return_bool(bool value) : returns bool
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'logical')
         varargout{1} = geometry_wrapper(31, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_bool');
     end
 
     function varargout = return_double(this, varargin)
       % RETURN_DOUBLE usage: return_double(double value) : returns double
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'double')
         varargout{1} = geometry_wrapper(32, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_double');
     end
 
     function varargout = return_field(this, varargin)
@@ -150,17 +150,17 @@ function delete(obj)
         varargout{1} = geometry_wrapper(33, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_field');
     end
 
     function varargout = return_int(this, varargin)
       % RETURN_INT usage: return_int(int value) : returns int
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'numeric')
         varargout{1} = geometry_wrapper(34, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_int');
     end
 
     function varargout = return_matrix1(this, varargin)
@@ -170,7 +170,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(35, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_matrix1');
     end
 
     function varargout = return_matrix2(this, varargin)
@@ -180,7 +180,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(36, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_matrix2');
     end
 
     function varargout = return_pair(this, varargin)
@@ -190,7 +190,7 @@ function delete(obj)
         [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_pair');
     end
 
     function varargout = return_ptrs(this, varargin)
@@ -200,17 +200,17 @@ function delete(obj)
         [ varargout{1} varargout{2} ] = geometry_wrapper(38, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_ptrs');
     end
 
     function varargout = return_size_t(this, varargin)
       % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
       % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
-      if length(varargin) == 1
+      if length(varargin) == 1 && isa(varargin{1},'numeric')
         varargout{1} = geometry_wrapper(39, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_size_t');
     end
 
     function varargout = return_string(this, varargin)
@@ -220,7 +220,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(40, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_string');
     end
 
     function varargout = return_vector1(this, varargin)
@@ -230,7 +230,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(41, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_vector1');
     end
 
     function varargout = return_vector2(this, varargin)
@@ -240,7 +240,7 @@ function delete(obj)
         varargout{1} = geometry_wrapper(42, this, varargin{:});
         return
       end
-      error('Arguments do not match any overload of function {class_name}.{method_name}');
+      error('Arguments do not match any overload of function Test.return_vector2');
     end
 
   end
diff --git a/tests/expected-matlab/aGlobalFunction.m b/tests/expected-matlab/aGlobalFunction.m
index e507ad3..643886a 100644
--- a/tests/expected-matlab/aGlobalFunction.m
+++ b/tests/expected-matlab/aGlobalFunction.m
@@ -1,6 +1,6 @@
 function varargout = aGlobalFunction(varargin)
       if length(varargin) == 0
-        varargout{1} = geometry_wrapper(85, varargin{:});
+        varargout{1} = geometry_wrapper(87, varargin{:});
       else
         error('Arguments do not match any overload of function aGlobalFunction');
       end
diff --git a/tests/expected-matlab/geometry_wrapper.cpp b/tests/expected-matlab/geometry_wrapper.cpp
index bf8775d..2f79ec0 100644
--- a/tests/expected-matlab/geometry_wrapper.cpp
+++ b/tests/expected-matlab/geometry_wrapper.cpp
@@ -22,20 +22,20 @@ typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
 static Collector_gtsamPoint2 collector_gtsamPoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point3>*> Collector_gtsamPoint3;
 static Collector_gtsamPoint3 collector_gtsamPoint3;
-typedef std::set<boost::shared_ptr<Test>*> Collector_;
-static Collector_ collector_;
-typedef std::set<boost::shared_ptr<MyBase>*> Collector_;
-static Collector_ collector_;
-typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_;
-static Collector_ collector_;
-typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_;
-static Collector_ collector_;
-typedef std::set<boost::shared_ptr<MyVector3>*> Collector_;
-static Collector_ collector_;
-typedef std::set<boost::shared_ptr<MyVector12>*> Collector_;
-static Collector_ collector_;
-typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_;
-static Collector_ collector_;
+typedef std::set<boost::shared_ptr<Test>*> Collector_Test;
+static Collector_Test collector_Test;
+typedef std::set<boost::shared_ptr<MyBase>*> Collector_MyBase;
+static Collector_MyBase collector_MyBase;
+typedef std::set<boost::shared_ptr<MyTemplatePoint2>*> Collector_MyTemplatePoint2;
+static Collector_MyTemplatePoint2 collector_MyTemplatePoint2;
+typedef std::set<boost::shared_ptr<MyTemplateMatrix>*> Collector_MyTemplateMatrix;
+static Collector_MyTemplateMatrix collector_MyTemplateMatrix;
+typedef std::set<boost::shared_ptr<MyVector3>*> Collector_MyVector3;
+static Collector_MyVector3 collector_MyVector3;
+typedef std::set<boost::shared_ptr<MyVector12>*> Collector_MyVector12;
+static Collector_MyVector12 collector_MyVector12;
+typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
+static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 
 void _deleteAllObjects()
 {
@@ -55,46 +55,46 @@ void _deleteAllObjects()
     collector_gtsamPoint3.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_Test::iterator iter = collector_Test.begin();
+      iter != collector_Test.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_Test.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_MyBase::iterator iter = collector_MyBase.begin();
+      iter != collector_MyBase.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_MyBase.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin();
+      iter != collector_MyTemplatePoint2.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_MyTemplatePoint2.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin();
+      iter != collector_MyTemplateMatrix.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_MyTemplateMatrix.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin();
+      iter != collector_MyVector3.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_MyVector3.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin();
+      iter != collector_MyVector12.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_MyVector12.erase(iter++);
     anyDeleted = true;
   } }
-  { for(Collector_::iterator iter = collector_.begin();
-      iter != collector_.end(); ) {
+  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
+      iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
-    collector_.erase(iter++);
+    collector_MyFactorPosePoint2.erase(iter++);
     anyDeleted = true;
   } }
   if(anyDeleted)
@@ -105,13 +105,13 @@ void _deleteAllObjects()
   std::cout.rdbuf(outbuf);
 }
 
-void _gtsam_RTTIRegister() {
+void _geometry_RTTIRegister() {
   const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_geometry_rttiRegistry_created");
   if(!alreadyCreated) {
     std::map<std::string, std::string> types;
-    types.insert(std::make_pair(typeid(MyBase).name(), ""));
-    types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), ""));
-    types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), ""));
+    types.insert(std::make_pair(typeid(MyBase).name(), "MyBase"));
+    types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2"));
+    types.insert(std::make_pair(typeid(MyTemplateMatrix).name(), "MyTemplateMatrix"));
 
     mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry");
     if(!registry)
@@ -298,7 +298,7 @@ void gtsamPoint3_StaticFunctionRet_17(int nargout, mxArray *out[], int nargin, c
 {
   checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1);
   double z = unwrap< double >(in[0]);
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false);
+  out[0] = wrap< Point3 >(gtsam::Point3::StaticFunctionRet(z));
 }
 
 void gtsamPoint3_staticFunction_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -402,7 +402,10 @@ void Test_return_Point2Ptr_28(int nargout, mxArray *out[], int nargin, const mxA
   checkArguments("return_Point2Ptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   bool value = unwrap< bool >(in[1]);
-  out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false);
+  {
+  boost::shared_ptr<Point2> shared(obj->return_Point2Ptr(value));
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void Test_return_Test_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -608,7 +611,7 @@ void MyTemplatePoint2_accept_T_50(int nargout, mxArray *out[], int nargin, const
 {
   checkArguments("accept_T",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
+  Point2 value = unwrap< Point2 >(in[1]);
   obj->accept_T(value);
 }
 
@@ -616,7 +619,7 @@ void MyTemplatePoint2_accept_Tptr_51(int nargout, mxArray *out[], int nargin, co
 {
   checkArguments("accept_Tptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
+  Point2 value = unwrap< Point2 >(in[1]);
   obj->accept_Tptr(value);
 }
 
@@ -625,8 +628,11 @@ void MyTemplatePoint2_create_MixedPtrs_52(int nargout, mxArray *out[], int nargi
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
   auto pairResult = obj->create_MixedPtrs();
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(pairResult.first),"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
+  out[0] = wrap< Point2 >(pairResult.first);
+  {
+  boost::shared_ptr<Point2> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void MyTemplatePoint2_create_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -634,35 +640,50 @@ void MyTemplatePoint2_create_ptrs_53(int nargout, mxArray *out[], int nargin, co
   checkArguments("create_ptrs",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
   auto pairResult = obj->create_ptrs();
-  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
+  {
+  boost::shared_ptr<Point2> shared(pairResult.first);
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
+  {
+  boost::shared_ptr<Point2> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void MyTemplatePoint2_return_T_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_T",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->return_T(value)),"gtsam.Point2", false);
+  Point2 value = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->return_T(value));
 }
 
 void MyTemplatePoint2_return_Tptr_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_Tptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false);
+  Point2 value = unwrap< Point2 >(in[1]);
+  {
+  boost::shared_ptr<Point2> shared(obj->return_Tptr(value));
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void MyTemplatePoint2_return_ptrs_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_ptrs",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  boost::shared_ptr<gtsam::Point2> p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  boost::shared_ptr<gtsam::Point2> p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2");
+  Point2 p1 = unwrap< Point2 >(in[1]);
+  Point2 p2 = unwrap< Point2 >(in[2]);
   auto pairResult = obj->return_ptrs(p1,p2);
-  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
-  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
+  {
+  boost::shared_ptr<Point2> shared(pairResult.first);
+  out[0] = wrap_shared_ptr(shared,"Point2");
+  }
+  {
+  boost::shared_ptr<Point2> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Point2");
+  }
 }
 
 void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -677,16 +698,16 @@ void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin
 {
   checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
+  Point2 t = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
 }
 
 void MyTemplatePoint2_templatedMethod_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Point2>>(in[0], "ptr_MyTemplatePoint2");
-  gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
+  Point3 t = unwrap< Point3 >(in[1]);
+  out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
 }
 
 void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -697,7 +718,14 @@ void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin
   out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
 }
 
-void MyTemplateMatrix_collectorInsertAndMakeBase_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplatePoint2_Level_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("MyTemplatePoint2.Level",nargout,nargin,1);
+  Point2 K = unwrap< Point2 >(in[0]);
+  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Point2>>(MyTemplate<gtsam::Point2>::Level(K)),"MyTemplatePoint2", false);
+}
+
+void MyTemplateMatrix_collectorInsertAndMakeBase_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
@@ -710,7 +738,7 @@ void MyTemplateMatrix_collectorInsertAndMakeBase_61(int nargout, mxArray *out[],
   *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);
 }
 
-void MyTemplateMatrix_upcastFromVoid_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
+void MyTemplateMatrix_upcastFromVoid_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
   boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));
@@ -719,7 +747,7 @@ void MyTemplateMatrix_upcastFromVoid_62(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;
 }
 
-void MyTemplateMatrix_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_constructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
@@ -734,7 +762,7 @@ void MyTemplateMatrix_constructor_63(int nargout, mxArray *out[], int nargin, co
   *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);
 }
 
-void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_deconstructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyTemplate<gtsam::Matrix>> Shared;
   checkArguments("delete_MyTemplateMatrix",nargout,nargin,1);
@@ -747,7 +775,7 @@ void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin,
   }
 }
 
-void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_accept_T_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("accept_T",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -755,7 +783,7 @@ void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const
   obj->accept_T(value);
 }
 
-void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_accept_Tptr_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("accept_Tptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -763,7 +791,7 @@ void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, co
   obj->accept_Tptr(value);
 }
 
-void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_create_MixedPtrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -775,7 +803,7 @@ void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargi
   }
 }
 
-void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_create_ptrs_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_ptrs",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -790,7 +818,7 @@ void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, co
   }
 }
 
-void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_return_T_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_T",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -798,7 +826,7 @@ void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const
   out[0] = wrap< Matrix >(obj->return_T(value));
 }
 
-void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_return_Tptr_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_Tptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -809,7 +837,7 @@ void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, co
   }
 }
 
-void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_return_ptrs_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_ptrs",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -826,7 +854,7 @@ void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, co
   }
 }
 
-void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -834,23 +862,23 @@ void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin
   out[0] = wrap< Matrix >(obj->templatedMethod<gtsam::Matrix>(t));
 }
 
-void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_templatedMethod_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
-  gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
+  Point2 t = unwrap< Point2 >(in[1]);
+  out[0] = wrap< Point2 >(obj->templatedMethod<gtsam::Point2>(t));
 }
 
-void MyTemplateMatrix_templatedMethod_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
-  gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
+  Point3 t = unwrap< Point3 >(in[1]);
+  out[0] = wrap< Point3 >(obj->templatedMethod<gtsam::Point3>(t));
 }
 
-void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_templatedMethod_76(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("templatedMethodVector",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<MyTemplate<gtsam::Matrix>>(in[0], "ptr_MyTemplateMatrix");
@@ -858,7 +886,14 @@ void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin
   out[0] = wrap< Vector >(obj->templatedMethod<gtsam::Vector>(t));
 }
 
-void MyVector3_collectorInsertAndMakeBase_76(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyTemplateMatrix_Level_77(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("MyTemplateMatrix.Level",nargout,nargin,1);
+  Matrix K = unwrap< Matrix >(in[0]);
+  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
+}
+
+void MyVector3_collectorInsertAndMakeBase_78(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<3>> Shared;
@@ -867,7 +902,7 @@ void MyVector3_collectorInsertAndMakeBase_76(int nargout, mxArray *out[], int na
   collector_MyVector3.insert(self);
 }
 
-void MyVector3_constructor_77(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_constructor_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<3>> Shared;
@@ -878,7 +913,7 @@ void MyVector3_constructor_77(int nargout, mxArray *out[], int nargin, const mxA
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector3_deconstructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_deconstructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyVector<3>> Shared;
   checkArguments("delete_MyVector3",nargout,nargin,1);
@@ -891,7 +926,7 @@ void MyVector3_deconstructor_78(int nargout, mxArray *out[], int nargin, const m
   }
 }
 
-void MyVector12_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_collectorInsertAndMakeBase_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<12>> Shared;
@@ -900,7 +935,7 @@ void MyVector12_collectorInsertAndMakeBase_79(int nargout, mxArray *out[], int n
   collector_MyVector12.insert(self);
 }
 
-void MyVector12_constructor_80(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_constructor_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<12>> Shared;
@@ -911,7 +946,7 @@ void MyVector12_constructor_80(int nargout, mxArray *out[], int nargin, const mx
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector12_deconstructor_81(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_deconstructor_83(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyVector<12>> Shared;
   checkArguments("delete_MyVector12",nargout,nargin,1);
@@ -924,7 +959,7 @@ void MyVector12_deconstructor_81(int nargout, mxArray *out[], int nargin, const
   }
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_82(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_collectorInsertAndMakeBase_84(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -933,7 +968,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_82(int nargout, mxArray *out[
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_83(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_85(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -948,7 +983,7 @@ void MyFactorPosePoint2_constructor_83(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_84(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_86(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
   checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
@@ -961,18 +996,18 @@ void MyFactorPosePoint2_deconstructor_84(int nargout, mxArray *out[], int nargin
   }
 }
 
-void aGlobalFunction_85(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void aGlobalFunction_87(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("aGlobalFunction",nargout,nargin,0);
   out[0] = wrap< Vector >(aGlobalFunction());
 }
-void overloadedGlobalFunction_86(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_88(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_87(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void overloadedGlobalFunction_89(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("overloadedGlobalFunction",nargout,nargin,2);
   int a = unwrap< int >(in[0]);
@@ -1175,40 +1210,40 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplatePoint2_templatedMethod_60(nargout, out, nargin-1, in+1);
       break;
     case 61:
-      MyTemplateMatrix_collectorInsertAndMakeBase_61(nargout, out, nargin-1, in+1);
+      MyTemplatePoint2_Level_61(nargout, out, nargin-1, in+1);
       break;
     case 62:
-      MyTemplateMatrix_upcastFromVoid_62(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_collectorInsertAndMakeBase_62(nargout, out, nargin-1, in+1);
       break;
     case 63:
-      MyTemplateMatrix_constructor_63(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_upcastFromVoid_63(nargout, out, nargin-1, in+1);
       break;
     case 64:
-      MyTemplateMatrix_deconstructor_64(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_constructor_64(nargout, out, nargin-1, in+1);
       break;
     case 65:
-      MyTemplateMatrix_accept_T_65(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_deconstructor_65(nargout, out, nargin-1, in+1);
       break;
     case 66:
-      MyTemplateMatrix_accept_Tptr_66(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_accept_T_66(nargout, out, nargin-1, in+1);
       break;
     case 67:
-      MyTemplateMatrix_create_MixedPtrs_67(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_accept_Tptr_67(nargout, out, nargin-1, in+1);
       break;
     case 68:
-      MyTemplateMatrix_create_ptrs_68(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_create_MixedPtrs_68(nargout, out, nargin-1, in+1);
       break;
     case 69:
-      MyTemplateMatrix_return_T_69(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_create_ptrs_69(nargout, out, nargin-1, in+1);
       break;
     case 70:
-      MyTemplateMatrix_return_Tptr_70(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_return_T_70(nargout, out, nargin-1, in+1);
       break;
     case 71:
-      MyTemplateMatrix_return_ptrs_71(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_return_Tptr_71(nargout, out, nargin-1, in+1);
       break;
     case 72:
-      MyTemplateMatrix_templatedMethod_72(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_return_ptrs_72(nargout, out, nargin-1, in+1);
       break;
     case 73:
       MyTemplateMatrix_templatedMethod_73(nargout, out, nargin-1, in+1);
@@ -1220,40 +1255,46 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_templatedMethod_75(nargout, out, nargin-1, in+1);
       break;
     case 76:
-      MyVector3_collectorInsertAndMakeBase_76(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_templatedMethod_76(nargout, out, nargin-1, in+1);
       break;
     case 77:
-      MyVector3_constructor_77(nargout, out, nargin-1, in+1);
+      MyTemplateMatrix_Level_77(nargout, out, nargin-1, in+1);
       break;
     case 78:
-      MyVector3_deconstructor_78(nargout, out, nargin-1, in+1);
+      MyVector3_collectorInsertAndMakeBase_78(nargout, out, nargin-1, in+1);
       break;
     case 79:
-      MyVector12_collectorInsertAndMakeBase_79(nargout, out, nargin-1, in+1);
+      MyVector3_constructor_79(nargout, out, nargin-1, in+1);
       break;
     case 80:
-      MyVector12_constructor_80(nargout, out, nargin-1, in+1);
+      MyVector3_deconstructor_80(nargout, out, nargin-1, in+1);
       break;
     case 81:
-      MyVector12_deconstructor_81(nargout, out, nargin-1, in+1);
+      MyVector12_collectorInsertAndMakeBase_81(nargout, out, nargin-1, in+1);
       break;
     case 82:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_82(nargout, out, nargin-1, in+1);
+      MyVector12_constructor_82(nargout, out, nargin-1, in+1);
       break;
     case 83:
-      MyFactorPosePoint2_constructor_83(nargout, out, nargin-1, in+1);
+      MyVector12_deconstructor_83(nargout, out, nargin-1, in+1);
       break;
     case 84:
-      MyFactorPosePoint2_deconstructor_84(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_collectorInsertAndMakeBase_84(nargout, out, nargin-1, in+1);
       break;
     case 85:
-      aGlobalFunction_85(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_constructor_85(nargout, out, nargin-1, in+1);
       break;
     case 86:
-      overloadedGlobalFunction_86(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_deconstructor_86(nargout, out, nargin-1, in+1);
       break;
     case 87:
-      overloadedGlobalFunction_87(nargout, out, nargin-1, in+1);
+      aGlobalFunction_87(nargout, out, nargin-1, in+1);
+      break;
+    case 88:
+      overloadedGlobalFunction_88(nargout, out, nargin-1, in+1);
+      break;
+    case 89:
+      overloadedGlobalFunction_89(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {
diff --git a/tests/expected-matlab/overloadedGlobalFunction.m b/tests/expected-matlab/overloadedGlobalFunction.m
index 458779e..e354ebc 100644
--- a/tests/expected-matlab/overloadedGlobalFunction.m
+++ b/tests/expected-matlab/overloadedGlobalFunction.m
@@ -1,8 +1,8 @@
 function varargout = overloadedGlobalFunction(varargin)
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = geometry_wrapper(86, varargin{:});
+        varargout{1} = geometry_wrapper(88, varargin{:});
       elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
-        varargout{1} = geometry_wrapper(87, varargin{:});
+        varargout{1} = geometry_wrapper(89, varargin{:});
       else
         error('Arguments do not match any overload of function overloadedGlobalFunction');
       end
diff --git a/tests/expected-python/geometry_pybind.cpp b/tests/expected-python/geometry_pybind.cpp
index 1d68930..9b56256 100644
--- a/tests/expected-python/geometry_pybind.cpp
+++ b/tests/expected-python/geometry_pybind.cpp
@@ -108,7 +108,8 @@ PYBIND11_MODULE(geometry_py, m_) {
         .def("return_T",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& value){return self->return_T(value);}, py::arg("value"))
         .def("create_ptrs",[](MyTemplate<gtsam::Point2>* self){return self->create_ptrs();})
         .def("create_MixedPtrs",[](MyTemplate<gtsam::Point2>* self){return self->create_MixedPtrs();})
-        .def("return_ptrs",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& p1,const std::shared_ptr<gtsam::Point2>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"));
+        .def("return_ptrs",[](MyTemplate<gtsam::Point2>* self,const std::shared_ptr<gtsam::Point2>& p1,const std::shared_ptr<gtsam::Point2>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def_static("Level",[](const gtsam::Point2& K){return MyTemplate<gtsam::Point2>::Level(K);}, py::arg("K"));
 
     py::class_<MyTemplate<gtsam::Matrix>, MyBase, std::shared_ptr<MyTemplate<gtsam::Matrix>>>(m_, "MyTemplateMatrix")
         .def(py::init<>())
@@ -122,7 +123,8 @@ PYBIND11_MODULE(geometry_py, m_) {
         .def("return_T",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& value){return self->return_T(value);}, py::arg("value"))
         .def("create_ptrs",[](MyTemplate<gtsam::Matrix>* self){return self->create_ptrs();})
         .def("create_MixedPtrs",[](MyTemplate<gtsam::Matrix>* self){return self->create_MixedPtrs();})
-        .def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& p1,const std::shared_ptr<gtsam::Matrix>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"));
+        .def("return_ptrs",[](MyTemplate<gtsam::Matrix>* self,const std::shared_ptr<gtsam::Matrix>& p1,const std::shared_ptr<gtsam::Matrix>& p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
+        .def_static("Level",[](const gtsam::Matrix& K){return MyTemplate<gtsam::Matrix>::Level(K);}, py::arg("K"));
 
     py::class_<MyVector<3>, std::shared_ptr<MyVector<3>>>(m_, "MyVector3")
         .def(py::init<>());
diff --git a/tests/geometry.h b/tests/geometry.h
index 74db80c..ddbc182 100644
--- a/tests/geometry.h
+++ b/tests/geometry.h
@@ -118,6 +118,8 @@ virtual class MyTemplate : MyBase {
   pair<T*,T*> create_ptrs () const;
   pair<T ,T*> create_MixedPtrs () const;
   pair<T*,T*> return_ptrs (T* p1, T* p2) const;
+
+  static This Level(const T& K);
 };
 
 // A doubly templated class
diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py
index 7f7bd1b..b3ca1a3 100644
--- a/tests/test_matlab_wrapper.py
+++ b/tests/test_matlab_wrapper.py
@@ -106,44 +106,34 @@ def test_geometry_matlab(self):
 
         self._generate_content(cc_content)
 
+        def compare_and_diff(file):
+            output = self.MATLAB_ACTUAL_DIR + file
+            expected = self.MATLAB_TEST_DIR + file
+            success = filecmp.cmp(output, expected)
+            if not success:
+                print("Differ in file: {}".format(file))
+                os.system("diff {} {}".format(output, expected))
+            self.assertTrue(success)
+
         self.assertTrue(os.path.isdir(self.MATLAB_ACTUAL_DIR + '+gtsam'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + '+gtsam/Point2.m',
-            self.MATLAB_TEST_DIR + '+gtsam/Point2.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + '+gtsam/Point3.m',
-            self.MATLAB_TEST_DIR + '+gtsam/Point3.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'Test.m',
-            self.MATLAB_TEST_DIR + 'Test.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'MyBase.m',
-            self.MATLAB_TEST_DIR + 'MyBase.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'MyTemplatePoint2.m',
-            self.MATLAB_TEST_DIR + 'MyTemplatePoint2.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'MyTemplateMatrix.m',
-            self.MATLAB_TEST_DIR + 'MyTemplateMatrix.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'MyVector3.m',
-            self.MATLAB_TEST_DIR + 'MyVector3.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'MyVector12.m',
-            self.MATLAB_TEST_DIR + 'MyVector12.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'MyFactorPosePoint2.m',
-            self.MATLAB_TEST_DIR + 'MyFactorPosePoint2.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'aGlobalFunction.m',
-            self.MATLAB_TEST_DIR + 'aGlobalFunction.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'overloadedGlobalFunction.m',
-            self.MATLAB_TEST_DIR + 'overloadedGlobalFunction.m'))
-        self.assertTrue(filecmp.cmp(
-            self.MATLAB_ACTUAL_DIR + 'geometry_wrapper.cpp',
-            self.MATLAB_TEST_DIR + 'geometry_wrapper.cpp'))
 
+        files = [
+            '+gtsam/Point2.m',
+            '+gtsam/Point3.m',
+            'Test.m',
+            'MyBase.m',
+            'MyTemplatePoint2.m',
+            'MyTemplateMatrix.m',
+            'MyVector3.m',
+            'MyVector12.m',
+            'MyFactorPosePoint2.m',
+            'aGlobalFunction.m',
+            'overloadedGlobalFunction.m',
+            'geometry_wrapper.cpp'
+        ]
+
+        for file in files:
+            compare_and_diff(file)
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py
index daa13b0..80032cb 100644
--- a/tests/test_pybind_wrapper.py
+++ b/tests/test_pybind_wrapper.py
@@ -60,10 +60,12 @@ def test_geometry_python(self):
         with open(output, 'w') as f:
             f.write(cc_content)
 
-        self.assertTrue(filecmp.cmp(
-            output,
-            path.join(self.TEST_DIR, 'expected-python/geometry_pybind.cpp'))
-        )
+        expected = path.join(self.TEST_DIR, 'expected-python/geometry_pybind.cpp')
+        success = filecmp.cmp(output, expected)
+
+        if not success:
+            os.system("diff {} {}".format(output, expected))
+        self.assertTrue(success)
 
 if __name__ == '__main__':
     unittest.main()