Squashed 'wrap/' changes from 0124bcc45..07330d100

07330d100 Merge pull request #113 from borglab/fix/reserved-keywords ec6b8f037 update test f022ba516 update and ignore reserved keywords for both functions and methods 4f988e5ad Merge pull request #112 from borglab/fix/cleanup 55bba2e6d fix variable annotations 61720ca0b support python 3.5 in the CI 0975d6529 version bump e8109917c use args.list() instead of args.args_list 6d0a91d7d renames args_names to names 4ce060b44 Merge pull request #111 from borglab/fix/default-bug ce7eea318 updated tests to capture bug b7650ec07 Fix bug for default in methods 4108854c7 Merge pull request #107 from borglab/feature/print 528ee64ce Merge pull request #110 from borglab/fix/variable-annotation e069f8bfc use old style variable annotation 5fd300116 update test fixture a25c2df0f use separate function to wrap global functions 58499a74b Merge pull request #106 from borglab/feature/consistent_names 2fe92b693 rename from cpp_class to to_cpp 3a3ba5963 Merge pull request #105 from borglab/cleanup e27a7b833 unskip tests 0db1839c4 Merge pull request #104 from borglab/feature/forward-class-declaration e3e7fbb27 remove unused imports a3c125065 encapsulate parsing and instantiation within wrap method in a functional way 69bbbe992 wrap instantiated declaration dbc44e7d5 added test for forward declaration typedef 6bec3cb8b add template instantation for typedefs of forward declarations 8d70c15ed updated Declaration to allow for wrapping 0637c2b3f remove print debugging statement deb8291ac allow forward declarations to be used for typedefs 69d660899 Merge pull request #101 from borglab/feature/object-default-parameters ec5555e56 formatting and docs cdaabc043 Merge branch 'feature/object-default-parameters' of github.com:borglab/wrap into feature/object-default-parameters 8ab0b0fa7 new parsing rule 0638a1937 update DEFAULT_ARG rule to support vector initializer list 83d2b761c update tests 7bb8c5494 more tests 1eaf6ba4a refactor default arg feature and add more tests 94f373ca9 tests 534e8a6dd support object types as default arguments 05e8ea855 Merge pull request #99 from borglab/fix/default_arg_0 1fdfeae6a address review comments 25b109c3f fix matlab unit test 6bb1b0c46 fix declaration order in unit test "expected" 7ee2d5fa4 don't unquote QuotedStrings that way we don't have to deal with strings manually. c915f4963 failing unit test : literals are not wrapped properly d47b6e8be default arg of 0 - interface_parser unit test ccf693641 fix for allow default arg of 0 3534c06e9 unit tests for default arg of 0 git-subtree-dir: wrap git-subtree-split: 07330d10022130e4284743341ac9d54a0dcb3d9f
2021-06-16 13:51:43 -04:00 · 2021-06-16 13:51:43 -04:00 · 56bede07cd
parent 4a2d322a73
commit 56bede07cd
39 changed files with 827 additions and 383 deletions
--- a/.github/workflows/linux-ci.yml
+++ b/.github/workflows/linux-ci.yml
@ -10,7 +10,7 @@ jobs:
    strategy:
      fail-fast: false
      matrix:
-        python-version: [3.6, 3.7, 3.8, 3.9]
+        python-version: [3.5, 3.6, 3.7, 3.8, 3.9]

    steps:
      - name: Checkout
--- a/.github/workflows/macos-ci.yml
+++ b/.github/workflows/macos-ci.yml
@ -10,7 +10,7 @@ jobs:
    strategy:
      fail-fast: false
      matrix:
-        python-version: [3.6, 3.7, 3.8, 3.9]
+        python-version: [3.5, 3.6, 3.7, 3.8, 3.9]

    steps:
      - name: Checkout
--- a/DOCS.md
+++ b/DOCS.md
@ -100,7 +100,8 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the
        ```

 - Using classes defined in other modules
-    - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. `OtherClass` should be in the same project.
+    - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error.
+    - `OtherClass` may not be in the same project. If this is the case, include the header for the appropriate project `#include <other_project/OtherClass.h>`.

 - Virtual inheritance
    - Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace.
--- a/gtwrap/interface_parser/classes.py
+++ b/gtwrap/interface_parser/classes.py
@ -189,7 +189,7 @@ class Operator:

        # Check to ensure arg and return type are the same.
        if len(args) == 1 and self.operator not in ("()", "[]"):
-            assert args.args_list[0].ctype.typename.name == return_type.type1.typename.name, \
+            assert args.list()[0].ctype.typename.name == return_type.type1.typename.name, \
                "Mixed type overloading not supported. Both arg and return type must be the same."

    def __repr__(self) -> str:
--- a/gtwrap/interface_parser/declaration.py
+++ b/gtwrap/interface_parser/declaration.py
@ -15,6 +15,7 @@ from pyparsing import CharsNotIn, Optional
 from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON,
                     VIRTUAL)
 from .type import Typename
+from .utils import collect_namespaces


 class Include:
@ -42,11 +43,12 @@ class ForwardDeclaration:
                t.name, t.parent_type, t.is_virtual))

    def __init__(self,
-                 name: Typename,
+                 typename: Typename,
                 parent_type: str,
                 is_virtual: str,
                 parent: str = ''):
-        self.name = name
+        self.name = typename.name
+        self.typename = typename
        if parent_type:
            self.parent_type = parent_type
        else:
@ -55,6 +57,9 @@ class ForwardDeclaration:
        self.is_virtual = is_virtual
        self.parent = parent

+    def namespaces(self) -> list:
+        """Get the namespaces which this class is nested under as a list."""
+        return collect_namespaces(self)
+
    def __repr__(self) -> str:
-        return "ForwardDeclaration: {} {}({})".format(self.is_virtual,
-                                                      self.name, self.parent)
+        return "ForwardDeclaration: {} {}".format(self.is_virtual, self.name)
--- a/gtwrap/interface_parser/function.py
+++ b/gtwrap/interface_parser/function.py
@ -29,11 +29,13 @@ class Argument:
    void sayHello(/*`s` is the method argument with type `const string&`*/ const string& s);
    ```
    """
-    rule = ((Type.rule ^ TemplatedType.rule)("ctype") + IDENT("name") + \
-            Optional(EQUAL + (DEFAULT_ARG ^ Type.rule ^ TemplatedType.rule) + \
-                Optional(LPAREN + RPAREN)  # Needed to parse the parens for default constructors
-                )("default")
-            ).setParseAction(lambda t: Argument(t.ctype, t.name, t.default))
+    rule = ((Type.rule ^ TemplatedType.rule)("ctype")  #
+            + IDENT("name")  #
+            + Optional(EQUAL + DEFAULT_ARG)("default")
+            ).setParseAction(lambda t: Argument(
+                t.ctype,  #
+                t.name,  #
+                t.default[0] if isinstance(t.default, ParseResults) else None))

    def __init__(self,
                 ctype: Union[Type, TemplatedType],
@ -44,18 +46,8 @@ class Argument:
        else:
            self.ctype = ctype
        self.name = name
-        # If the length is 1, it's a regular type,
-        if len(default) == 1:
-            default = default[0]
-        # This means a tuple has been passed so we convert accordingly
-        elif len(default) > 1:
-            default = tuple(default.asList())
-        else:
-            # set to None explicitly so we can support empty strings
-            default = None
        self.default = default
-
-        self.parent: Union[ArgumentList, None] = None
+        self.parent = None  # type: Union[ArgumentList, None]

    def __repr__(self) -> str:
        return self.to_cpp()
@ -94,10 +86,14 @@ class ArgumentList:
    def __len__(self) -> int:
        return len(self.args_list)

-    def args_names(self) -> List[str]:
+    def names(self) -> List[str]:
        """Return a list of the names of all the arguments."""
        return [arg.name for arg in self.args_list]

+    def list(self) -> List[Argument]:
+        """Return a list of the names of all the arguments."""
+        return self.args_list
+
    def to_cpp(self, use_boost: bool) -> List[str]:
        """Generate the C++ code for wrapping."""
        return [arg.ctype.to_cpp(use_boost) for arg in self.args_list]
--- a/gtwrap/interface_parser/namespace.py
+++ b/gtwrap/interface_parser/namespace.py
@ -102,7 +102,7 @@ class Namespace:
        res = []
        for namespace in found_namespaces:
            classes_and_funcs = (c for c in namespace.content
-                                 if isinstance(c, (Class, GlobalFunction)))
+                                 if isinstance(c, (Class, GlobalFunction, ForwardDeclaration)))
            res += [c for c in classes_and_funcs if c.name == typename.name]
        if not res:
            raise ValueError("Cannot find class {} in module!".format(
--- a/gtwrap/interface_parser/tokens.py
+++ b/gtwrap/interface_parser/tokens.py
@ -10,9 +10,9 @@ All the token definitions.
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """

-from pyparsing import (Keyword, Literal, Or, QuotedString, Suppress, Word,
-                       alphanums, alphas, delimitedList, nums,
-                       pyparsing_common)
+from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress,
+                       Word, alphanums, alphas, nestedExpr, nums,
+                       originalTextFor, printables)

 # rule for identifiers (e.g. variable names)
 IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums)
@ -22,16 +22,20 @@ RAW_POINTER, SHARED_POINTER, REF = map(Literal, "@*&")
 LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;")
 LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=")

-# Encapsulating type for numbers, and single and double quoted strings.
-# The pyparsing_common utilities ensure correct coversion to the corresponding type.
-# E.g. pyparsing_common.number will convert 3.1415 to a float type.
-NUMBER_OR_STRING = (pyparsing_common.number ^ QuotedString('"') ^ QuotedString("'"))
-
-# A python tuple, e.g. (1, 9, "random", 3.1415)
-TUPLE = (LPAREN + delimitedList(NUMBER_OR_STRING) + RPAREN)
-
 # Default argument passed to functions/methods.
-DEFAULT_ARG = (NUMBER_OR_STRING ^ pyparsing_common.identifier ^ TUPLE)
+# Allow anything up to ',' or ';' except when they
+# appear inside matched expressions such as
+# (a, b) {c, b} "hello, world", templates, initializer lists, etc.
+DEFAULT_ARG = originalTextFor(
+    OneOrMore(
+        QuotedString('"') ^  # parse double quoted strings
+        QuotedString("'") ^  # parse single quoted strings
+        Word(printables, excludeChars="(){}[]<>,;") ^  # parse arbitrary words
+        nestedExpr(opener='(', closer=')') ^  # parse expression in parentheses
+        nestedExpr(opener='[', closer=']') ^  # parse expression in brackets
+        nestedExpr(opener='{', closer='}') ^  # parse expression in braces
+        nestedExpr(opener='<', closer='>')  # parse template expressions
+    ))

 CONST, VIRTUAL, CLASS, STATIC, PAIR, TEMPLATE, TYPEDEF, INCLUDE = map(
    Keyword,
--- a/gtwrap/interface_parser/variable.py
+++ b/gtwrap/interface_parser/variable.py
@ -12,7 +12,7 @@ Author: Varun Agrawal, Gerry Chen

 from pyparsing import Optional, ParseResults

-from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON, STATIC
+from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON
 from .type import TemplatedType, Type


@ -32,10 +32,12 @@ class Variable:
    """
    rule = ((Type.rule ^ TemplatedType.rule)("ctype")  #
            + IDENT("name")  #
-            #TODO(Varun) Add support for non-basic types
-            + Optional(EQUAL + (DEFAULT_ARG))("default")  #
+            + Optional(EQUAL + DEFAULT_ARG)("default")  #
            + SEMI_COLON  #
-            ).setParseAction(lambda t: Variable(t.ctype, t.name, t.default))
+            ).setParseAction(lambda t: Variable(
+                t.ctype,  #
+                t.name,  #
+                t.default[0] if isinstance(t.default, ParseResults) else None))

    def __init__(self,
                 ctype: Type,
@ -44,11 +46,7 @@ class Variable:
                 parent=''):
        self.ctype = ctype[0]  # ParseResult is a list
        self.name = name
-        if default:
-            self.default = default[0]
-        else:
-            self.default = None
-
+        self.default = default
        self.parent = parent

    def __repr__(self) -> str:
--- a/gtwrap/matlab_wrapper.py
+++ b/gtwrap/matlab_wrapper.py
@ -57,7 +57,7 @@ class MatlabWrapper(object):
    # Methods that should be ignored
    ignore_methods = ['pickle']
    # Datatypes that do not need to be checked in methods
-    not_check_type: list = []
+    not_check_type = []  # type: list
    # Data types that are primitive types
    not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
    # Ignore the namespace for these datatypes
@ -65,16 +65,18 @@ class MatlabWrapper(object):
    # The amount of times the wrapper has created a call to geometry_wrapper
    wrapper_id = 0
    # Map each wrapper id to what its collector function namespace, class, type, and string format
-    wrapper_map: dict = {}
+    wrapper_map = {}
    # Set of all the includes in the namespace
-    includes: Dict[parser.Include, int] = {}
+    includes = {}  # type: Dict[parser.Include, int]
    # Set of all classes in the namespace
-    classes: List[Union[parser.Class, instantiator.InstantiatedClass]] = []
-    classes_elems: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] = {}
+    classes = [
+    ]  # type: List[Union[parser.Class, instantiator.InstantiatedClass]]
+    classes_elems = {
+    }  # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int]
    # Id for ordering global functions in the wrapper
    global_function_id = 0
    # Files and their content
-    content: List[str] = []
+    content = []  # type: List[str]

    # Ensure the template file is always picked up from the correct directory.
    dir_path = osp.dirname(osp.realpath(__file__))
@ -82,11 +84,9 @@ class MatlabWrapper(object):
        wrapper_file_header = f.read()

    def __init__(self,
-                 module,
                 module_name,
                 top_module_namespace='',
                 ignore_classes=()):
-        self.module = module
        self.module_name = module_name
        self.top_module_namespace = top_module_namespace
        self.ignore_classes = ignore_classes
@ -374,14 +374,14 @@ class MatlabWrapper(object):
        """
        arg_wrap = ''

-        for i, arg in enumerate(args.args_list, 1):
+        for i, arg in enumerate(args.list(), 1):
            c_type = self._format_type_name(arg.ctype.typename,
                                            include_namespace=False)

            arg_wrap += '{c_type} {arg_name}{comma}'.format(
                c_type=c_type,
                arg_name=arg.name,
-                comma='' if i == len(args.args_list) else ', ')
+                comma='' if i == len(args.list()) else ', ')

        return arg_wrap

@ -396,7 +396,7 @@ class MatlabWrapper(object):
        """
        var_arg_wrap = ''

-        for i, arg in enumerate(args.args_list, 1):
+        for i, arg in enumerate(args.list(), 1):
            name = arg.ctype.typename.name
            if name in self.not_check_type:
                continue
@ -442,7 +442,7 @@ class MatlabWrapper(object):
        var_list_wrap = ''
        first = True

-        for i in range(1, len(args.args_list) + 1):
+        for i in range(1, len(args.list()) + 1):
            if first:
                var_list_wrap += 'varargin{{{}}}'.format(i)
                first = False
@ -462,9 +462,9 @@ class MatlabWrapper(object):
        if check_statement == '':
            check_statement = \
                'if length(varargin) == {param_count}'.format(
-                    param_count=len(args.args_list))
+                    param_count=len(args.list()))

-        for _, arg in enumerate(args.args_list):
+        for _, arg in enumerate(args.list()):
            name = arg.ctype.typename.name

            if name in self.not_check_type:
@ -516,7 +516,7 @@ class MatlabWrapper(object):
        params = ''
        body_args = ''

-        for arg in args.args_list:
+        for arg in args.list():
            if params != '':
                params += ','

@ -725,10 +725,10 @@ class MatlabWrapper(object):
            param_wrap += '      if' if i == 0 else '      elseif'
            param_wrap += ' length(varargin) == '

-            if len(overload.args.args_list) == 0:
+            if len(overload.args.list()) == 0:
                param_wrap += '0\n'
            else:
-                param_wrap += str(len(overload.args.args_list)) \
+                param_wrap += str(len(overload.args.list())) \
                              + self._wrap_variable_arguments(overload.args, False) + '\n'

            # Determine format of return and varargout statements
@ -825,14 +825,14 @@ class MatlabWrapper(object):
            methods_wrap += textwrap.indent(textwrap.dedent('''\
                elseif nargin == {len}{varargin}
                  {ptr}{wrapper}({num}{comma}{var_arg});
-            ''').format(len=len(ctor.args.args_list),
+            ''').format(len=len(ctor.args.list()),
                        varargin=self._wrap_variable_arguments(
                            ctor.args, False),
                        ptr=wrapper_return,
                        wrapper=self._wrapper_name(),
                        num=self._update_wrapper_id(
                            (namespace_name, inst_class, 'constructor', ctor)),
-                        comma='' if len(ctor.args.args_list) == 0 else ', ',
+                        comma='' if len(ctor.args.list()) == 0 else ', ',
                        var_arg=self._wrap_list_variable_arguments(ctor.args)),
                                            prefix='    ')

@ -938,7 +938,7 @@ class MatlabWrapper(object):
                           namespace_name,
                           inst_class,
                           methods,
-                           serialize=(False,)):
+                           serialize=(False, )):
        """Wrap the methods in the class.

        Args:
@ -1075,7 +1075,7 @@ class MatlabWrapper(object):
                        static_overload.return_type,
                        include_namespace=True,
                        separator="."),
-                    length=len(static_overload.args.args_list),
+                    length=len(static_overload.args.list()),
                    var_args_list=self._wrap_variable_arguments(
                        static_overload.args),
                    check_statement=check_statement,
@ -1097,7 +1097,8 @@ class MatlabWrapper(object):

            method_text += textwrap.indent(textwrap.dedent("""\
                                    end\n
-                """), prefix="  ")
+                """),
+                                           prefix="  ")

        if serialize:
            method_text += textwrap.indent(textwrap.dedent("""\
@ -1349,14 +1350,14 @@ class MatlabWrapper(object):

        else:
            if isinstance(method.parent, instantiator.InstantiatedClass):
-                method_name = method.parent.cpp_class() + "::"
+                method_name = method.parent.to_cpp() + "::"
            else:
                method_name = self._format_static_method(method, '::')
            method_name += method.name

        if "MeasureRange" in method_name:
            self._debug("method: {}, method: {}, inst: {}".format(
-                method_name, method.name, method.parent.cpp_class()))
+                method_name, method.name, method.parent.to_cpp()))

        obj = '  ' if return_1_name == 'void' else ''
        obj += '{}{}({})'.format(obj_start, method_name, params)
@ -1447,7 +1448,7 @@ class MatlabWrapper(object):
            extra = collector_func[4]

            class_name = collector_func[0] + collector_func[1].name
-            class_name_separated = collector_func[1].cpp_class()
+            class_name_separated = collector_func[1].to_cpp()
            is_method = isinstance(extra, parser.Method)
            is_static_method = isinstance(extra, parser.StaticMethod)

@ -1510,12 +1511,12 @@ class MatlabWrapper(object):
            elif extra == 'serialize':
                body += self.wrap_collector_function_serialize(
                    collector_func[1].name,
-                    full_name=collector_func[1].cpp_class(),
+                    full_name=collector_func[1].to_cpp(),
                    namespace=collector_func[0])
            elif extra == 'deserialize':
                body += self.wrap_collector_function_deserialize(
                    collector_func[1].name,
-                    full_name=collector_func[1].cpp_class(),
+                    full_name=collector_func[1].to_cpp(),
                    namespace=collector_func[0])
            elif is_method or is_static_method:
                method_name = ''
@ -1548,7 +1549,7 @@ class MatlabWrapper(object):
                    min1='-1' if is_method else '',
                    shared_obj=shared_obj,
                    method_name=method_name,
-                    num_args=len(extra.args.args_list),
+                    num_args=len(extra.args.list()),
                    body_args=body_args,
                    return_body=return_body)

@ -1565,10 +1566,11 @@ class MatlabWrapper(object):
                  checkArguments("{function_name}",nargout,nargin,{len});
            ''').format(function_name=collector_func[1].name,
                        id=self.global_function_id,
-                        len=len(collector_func[1].args.args_list))
+                        len=len(collector_func[1].args.list()))

            body += self._wrapper_unwrap_arguments(collector_func[1].args)[1]
-            body += self.wrap_collector_function_return(collector_func[1]) + '\n}\n'
+            body += self.wrap_collector_function_return(
+                collector_func[1]) + '\n}\n'

            collector_function += body

@ -1723,7 +1725,7 @@ class MatlabWrapper(object):
                    cls_insts += self._format_type_name(inst)

                typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \
-                    .format(original_class_name=cls.cpp_class(),
+                    .format(original_class_name=cls.to_cpp(),
                            class_name_sep=cls.name)

                class_name_sep = cls.name
@ -1734,7 +1736,7 @@ class MatlabWrapper(object):
                    boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(
                        class_name_sep, class_name)
            else:
-                class_name_sep = cls.cpp_class()
+                class_name_sep = cls.to_cpp()
                class_name = self._format_class_name(cls)

                if len(cls.original.namespaces()) > 1 and _has_serialization(
@ -1780,7 +1782,7 @@ class MatlabWrapper(object):

            if queue_set_next_case:
                ptr_ctor_frag += self.wrap_collector_function_upcast_from_void(
-                    id_val[1].name, idx, id_val[1].cpp_class())
+                    id_val[1].name, idx, id_val[1].to_cpp())

        wrapper_file += textwrap.dedent('''\
            {typedef_instances}
@ -1872,10 +1874,14 @@ class MatlabWrapper(object):
                    namespace=namespace),
                               prefix='  ')

-    def wrap(self):
+    def wrap(self, content):
        """High level function to wrap the project."""
-        self.wrap_namespace(self.module)
-        self.generate_wrapper(self.module)
+        # Parse the contents of the interface file
+        parsed_result = parser.Module.parseString(content)
+        # Instantiate the module
+        module = instantiator.instantiate_namespace(parsed_result)
+        self.wrap_namespace(module)
+        self.generate_wrapper(module)

        return self.content

--- a/gtwrap/pybind_wrapper.py
+++ b/gtwrap/pybind_wrapper.py
@ -23,48 +23,49 @@ class PybindWrapper:
    Class to generate binding code for Pybind11 specifically.
    """
    def __init__(self,
-                 module,
                 module_name,
                 top_module_namespaces='',
                 use_boost=False,
                 ignore_classes=(),
                 module_template=""):
-        self.module = module
        self.module_name = module_name
        self.top_module_namespaces = top_module_namespaces
        self.use_boost = use_boost
        self.ignore_classes = ignore_classes
        self._serializing_classes = list()
        self.module_template = module_template
-        self.python_keywords = ['print', 'lambda']
+        self.python_keywords = [
+            'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import',
+            'return', 'True', 'elif', 'in', 'try', 'and', 'else', 'is',
+            'while', 'as', 'except', 'lambda', 'with', 'assert', 'finally',
+            'nonlocal', 'yield', 'break', 'for', 'not', 'class', 'from', 'or',
+            'continue', 'global', 'pass'
+        ]

        # amount of indentation to add before each function/method declaration.
        self.method_indent = '\n' + (' ' * 8)

-    def _py_args_names(self, args_list):
+    def _py_args_names(self, args):
        """Set the argument names in Pybind11 format."""
-        names = args_list.args_names()
+        names = args.names()
        if names:
            py_args = []
-            for arg in args_list.args_list:
-                if isinstance(arg.default, str) and arg.default is not None:
-                    # string default arg
-                    arg.default = ' = "{arg.default}"'.format(arg=arg)
-                elif arg.default:  # Other types
-                    arg.default = ' = {arg.default}'.format(arg=arg)
+            for arg in args.list():
+                if arg.default is not None:
+                    default = ' = {arg.default}'.format(arg=arg)
                else:
-                    arg.default = ''
+                    default = ''
                argument = 'py::arg("{name}"){default}'.format(
-                    name=arg.name, default='{0}'.format(arg.default))
+                    name=arg.name, default='{0}'.format(default))
                py_args.append(argument)
            return ", " + ", ".join(py_args)
        else:
            return ''

-    def _method_args_signature_with_names(self, args_list):
-        """Define the method signature types with the argument names."""
-        cpp_types = args_list.to_cpp(self.use_boost)
-        names = args_list.args_names()
+    def _method_args_signature(self, args):
+        """Generate the argument types and names as per the method signature."""
+        cpp_types = args.to_cpp(self.use_boost)
+        names = args.names()
        types_names = [
            "{} {}".format(ctype, name)
            for ctype, name in zip(cpp_types, names)
@ -99,7 +100,8 @@ class PybindWrapper:
            serialize_method = self.method_indent + \
                ".def(\"serialize\", []({class_inst} self){{ return gtsam::serialize(*self); }})".format(class_inst=cpp_class + '*')
            deserialize_method = self.method_indent + \
-                     ".def(\"deserialize\", []({class_inst} self, string serialized){{ gtsam::deserialize(serialized, *self); }}, py::arg(\"serialized\"))" \
+                     '.def("deserialize", []({class_inst} self, string serialized)' \
+                     '{{ gtsam::deserialize(serialized, *self); }}, py::arg("serialized"))' \
                       .format(class_inst=cpp_class + '*')
            return serialize_method + deserialize_method

@ -112,20 +114,23 @@ class PybindWrapper:
            return pickle_method.format(cpp_class=cpp_class,
                                        indent=self.method_indent)

+        # Add underscore to disambiguate if the method name matches a python keyword
+        if py_method in self.python_keywords:
+            py_method = py_method + "_"
+
        is_method = isinstance(method, instantiator.InstantiatedMethod)
        is_static = isinstance(method, parser.StaticMethod)
        return_void = method.return_type.is_void()
-        args_names = method.args.args_names()
+        args_names = method.args.names()
        py_args_names = self._py_args_names(method.args)
-        args_signature_with_names = self._method_args_signature_with_names(
-            method.args)
+        args_signature_with_names = self._method_args_signature(method.args)

        caller = cpp_class + "::" if not is_method else "self->"
-        function_call = ('{opt_return} {caller}{function_name}'
+        function_call = ('{opt_return} {caller}{method_name}'
                         '({args_names});'.format(
                             opt_return='return' if not return_void else '',
                             caller=caller,
-                             function_name=cpp_method,
+                             method_name=cpp_method,
                             args_names=', '.join(args_names),
                         ))

@ -136,8 +141,7 @@ class PybindWrapper:
               '{py_args_names}){suffix}'.format(
                   prefix=prefix,
                   cdef="def_static" if is_static else "def",
-                   py_method=py_method if not py_method in self.python_keywords
-                   else py_method + "_",
+                   py_method=py_method,
                   opt_self="{cpp_class}* self".format(
                       cpp_class=cpp_class) if is_method else "",
                   opt_comma=', ' if is_method and args_names else '',
@ -181,15 +185,13 @@ class PybindWrapper:
                     suffix=''):
        """
        Wrap all the methods in the `cpp_class`.
-
-        This function is also used to wrap global functions.
        """
        res = ""
        for method in methods:

-            # To avoid type confusion for insert, currently unused
+            # To avoid type confusion for insert
            if method.name == 'insert' and cpp_class == 'gtsam::Values':
-                name_list = method.args.args_names()
+                name_list = method.args.names()
                type_list = method.args.to_cpp(self.use_boost)
                # inserting non-wrapped value types
                if type_list[0].strip() == 'size_t':
@ -214,7 +216,8 @@ class PybindWrapper:
                      module_var,
                      variable,
                      prefix='\n' + ' ' * 8):
-        """Wrap a variable that's not part of a class (i.e. global)
+        """
+        Wrap a variable that's not part of a class (i.e. global)
        """
        variable_value = ""
        if variable.default is None:
@ -288,23 +291,20 @@ class PybindWrapper:

    def wrap_enums(self, enums, instantiated_class, prefix=' ' * 4):
        """Wrap multiple enums defined in a class."""
-        cpp_class = instantiated_class.cpp_class()
+        cpp_class = instantiated_class.to_cpp()
        module_var = instantiated_class.name.lower()
        res = ''

        for enum in enums:
            res += "\n" + self.wrap_enum(
-                enum,
-                class_name=cpp_class,
-                module=module_var,
-                prefix=prefix)
+                enum, class_name=cpp_class, module=module_var, prefix=prefix)
        return res

    def wrap_instantiated_class(
            self, instantiated_class: instantiator.InstantiatedClass):
        """Wrap the class."""
        module_var = self._gen_module_var(instantiated_class.namespaces())
-        cpp_class = instantiated_class.cpp_class()
+        cpp_class = instantiated_class.to_cpp()
        if cpp_class in self.ignore_classes:
            return ""
        if instantiated_class.parent_class:
@ -356,10 +356,27 @@ class PybindWrapper:
                    wrapped_operators=self.wrap_operators(
                        instantiated_class.operators, cpp_class)))

+    def wrap_instantiated_declaration(
+            self, instantiated_decl: instantiator.InstantiatedDeclaration):
+        """Wrap the class."""
+        module_var = self._gen_module_var(instantiated_decl.namespaces())
+        cpp_class = instantiated_decl.to_cpp()
+        if cpp_class in self.ignore_classes:
+            return ""
+
+        res = (
+            '\n    py::class_<{cpp_class}, '
+            '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
+        ).format(shared_ptr_type=('boost' if self.use_boost else 'std'),
+                 cpp_class=cpp_class,
+                 class_name=instantiated_decl.name,
+                 module_var=module_var)
+        return res
+
    def wrap_stl_class(self, stl_class):
        """Wrap STL containers."""
        module_var = self._gen_module_var(stl_class.namespaces())
-        cpp_class = stl_class.cpp_class()
+        cpp_class = stl_class.to_cpp()
        if cpp_class in self.ignore_classes:
            return ""

@ -385,6 +402,59 @@ class PybindWrapper:
                    stl_class.properties, cpp_class),
            ))

+    def wrap_functions(self,
+                       functions,
+                       namespace,
+                       prefix='\n' + ' ' * 8,
+                       suffix=''):
+        """
+        Wrap all the global functions.
+        """
+        res = ""
+        for function in functions:
+
+            function_name = function.name
+
+            # Add underscore to disambiguate if the function name matches a python keyword
+            python_keywords = self.python_keywords + ['print']
+            if function_name in python_keywords:
+                function_name = function_name + "_"
+
+            cpp_method = function.to_cpp()
+
+            is_static = isinstance(function, parser.StaticMethod)
+            return_void = function.return_type.is_void()
+            args_names = function.args.names()
+            py_args_names = self._py_args_names(function.args)
+            args_signature = self._method_args_signature(function.args)
+
+            caller = namespace + "::"
+            function_call = ('{opt_return} {caller}{function_name}'
+                             '({args_names});'.format(
+                                 opt_return='return'
+                                 if not return_void else '',
+                                 caller=caller,
+                                 function_name=cpp_method,
+                                 args_names=', '.join(args_names),
+                             ))
+
+            ret = ('{prefix}.{cdef}("{function_name}",'
+                   '[]({args_signature}){{'
+                   '{function_call}'
+                   '}}'
+                   '{py_args_names}){suffix}'.format(
+                       prefix=prefix,
+                       cdef="def_static" if is_static else "def",
+                       function_name=function_name,
+                       args_signature=args_signature,
+                       function_call=function_call,
+                       py_args_names=py_args_names,
+                       suffix=suffix))
+
+            res += ret
+
+        return res
+
    def _partial_match(self, namespaces1, namespaces2):
        for i in range(min(len(namespaces1), len(namespaces2))):
            if namespaces1[i] != namespaces2[i]:
@ -460,6 +530,9 @@ class PybindWrapper:
                    wrapped += self.wrap_instantiated_class(element)
                    wrapped += self.wrap_enums(element.enums, element)

+                elif isinstance(element, instantiator.InstantiatedDeclaration):
+                    wrapped += self.wrap_instantiated_declaration(element)
+
                elif isinstance(element, parser.Variable):
                    variable_namespace = self._add_namespaces('', namespaces)
                    wrapped += self.wrap_variable(namespace=variable_namespace,
@ -476,7 +549,7 @@ class PybindWrapper:
                if isinstance(func, (parser.GlobalFunction,
                                     instantiator.InstantiatedGlobalFunction))
            ]
-            wrapped += self.wrap_methods(
+            wrapped += self.wrap_functions(
                all_funcs,
                self._add_namespaces('', namespaces)[:-2],
                prefix='\n' + ' ' * 4 + module_var,
@ -484,9 +557,14 @@ class PybindWrapper:
            )
        return wrapped, includes

-    def wrap(self):
+    def wrap(self, content):
        """Wrap the code in the interface file."""
-        wrapped_namespace, includes = self.wrap_namespace(self.module)
+        # Parse the contents of the interface file
+        module = parser.Module.parseString(content)
+        # Instantiate all templates
+        module = instantiator.instantiate_namespace(module)
+
+        wrapped_namespace, includes = self.wrap_namespace(module)

        # Export classes for serialization.
        boost_class_export = ""
--- a/gtwrap/template_instantiator.py
+++ b/gtwrap/template_instantiator.py
@ -95,10 +95,9 @@ def instantiate_args_list(args_list, template_typenames, instantiations,
    for arg in args_list:
        new_type = instantiate_type(arg.ctype, template_typenames,
                                    instantiations, cpp_typename)
-        default = [arg.default] if isinstance(arg, parser.Argument) else ''
-        instantiated_args.append(parser.Argument(name=arg.name,
-                                                 ctype=new_type,
-                                                 default=default))
+        instantiated_args.append(
+            parser.Argument(name=arg.name, ctype=new_type,
+                            default=arg.default))
    return instantiated_args


@ -131,7 +130,6 @@ def instantiate_name(original_name, instantiations):
    TODO(duy): To avoid conflicts, we should include the instantiation's
    namespaces, but I find that too verbose.
    """
-    inst_name = ''
    instantiated_names = []
    for inst in instantiations:
        # Ensure the first character of the type is capitalized
@ -172,7 +170,7 @@ class InstantiatedGlobalFunction(parser.GlobalFunction):
                cpp_typename='',
            )
            instantiated_args = instantiate_args_list(
-                original.args.args_list,
+                original.args.list(),
                self.original.template.typenames,
                self.instantiations,
                # Keyword type name `This` should already be replaced in the
@ -206,7 +204,13 @@ class InstantiatedGlobalFunction(parser.GlobalFunction):

 class InstantiatedMethod(parser.Method):
    """
-    We can only instantiate template methods with a single template parameter.
+    Instantiate method with template parameters.
+
+    E.g.
+    class A {
+        template<X, Y>
+        void func(X x, Y y);
+    }
    """
    def __init__(self, original, instantiations: List[parser.Typename] = ''):
        self.original = original
@ -216,7 +220,7 @@ class InstantiatedMethod(parser.Method):
        self.parent = original.parent

        # Check for typenames if templated.
-        # This way, we can gracefully handle bot templated and non-templated methois.
+        # This way, we can gracefully handle both templated and non-templated methods.
        typenames = self.original.template.typenames if self.original.template else []
        self.name = instantiate_name(original.name, self.instantiations)
        self.return_type = instantiate_return_type(
@ -229,7 +233,7 @@ class InstantiatedMethod(parser.Method):
        )

        instantiated_args = instantiate_args_list(
-            original.args.args_list,
+            original.args.list(),
            typenames,
            self.instantiations,
            # Keyword type name `This` should already be replaced in the
@ -342,7 +346,7 @@ class InstantiatedClass(parser.Class):
            "{ctors}\n{static_methods}\n{methods}".format(
               virtual="virtual" if self.is_virtual else '',
               name=self.name,
-               cpp_class=self.cpp_class(),
+               cpp_class=self.to_cpp(),
               parent_class=self.parent,
               ctors="\n".join([repr(ctor) for ctor in self.ctors]),
               methods="\n".join([repr(m) for m in self.methods]),
@ -364,7 +368,7 @@ class InstantiatedClass(parser.Class):

        for ctor in self.original.ctors:
            instantiated_args = instantiate_args_list(
-                ctor.args.args_list,
+                ctor.args.list(),
                typenames,
                self.instantiations,
                self.cpp_typename(),
@ -389,7 +393,7 @@ class InstantiatedClass(parser.Class):
        instantiated_static_methods = []
        for static_method in self.original.static_methods:
            instantiated_args = instantiate_args_list(
-                static_method.args.args_list, typenames, self.instantiations,
+                static_method.args.list(), typenames, self.instantiations,
                self.cpp_typename())
            instantiated_static_methods.append(
                parser.StaticMethod(
@ -426,7 +430,7 @@ class InstantiatedClass(parser.Class):
        class_instantiated_methods = []
        for method in self.original.methods:
            instantiated_args = instantiate_args_list(
-                method.args.args_list,
+                method.args.list(),
                typenames,
                self.instantiations,
                self.cpp_typename(),
@ -459,7 +463,7 @@ class InstantiatedClass(parser.Class):
        instantiated_operators = []
        for operator in self.original.operators:
            instantiated_args = instantiate_args_list(
-                operator.args.args_list,
+                operator.args.list(),
                typenames,
                self.instantiations,
                self.cpp_typename(),
@ -497,10 +501,6 @@ class InstantiatedClass(parser.Class):
        )
        return instantiated_properties

-    def cpp_class(self):
-        """Generate the C++ code for wrapping."""
-        return self.cpp_typename().to_cpp()
-
    def cpp_typename(self):
        """
        Return a parser.Typename including namespaces and cpp name of this
@ -516,8 +516,53 @@ class InstantiatedClass(parser.Class):
        namespaces_name.append(name)
        return parser.Typename(namespaces_name)

+    def to_cpp(self):
+        """Generate the C++ code for wrapping."""
+        return self.cpp_typename().to_cpp()

-def instantiate_namespace_inplace(namespace):
+
+class InstantiatedDeclaration(parser.ForwardDeclaration):
+    """
+    Instantiate typedefs of forward declarations.
+    This is useful when we wish to typedef a templated class
+    which is not defined in the current project.
+
+    E.g.
+        class FactorFromAnotherMother;
+
+        typedef FactorFromAnotherMother<gtsam::Pose3> FactorWeCanUse;
+    """
+    def __init__(self, original, instantiations=(), new_name=''):
+        super().__init__(original.typename,
+                         original.parent_type,
+                         original.is_virtual,
+                         parent=original.parent)
+
+        self.original = original
+        self.instantiations = instantiations
+        self.parent = original.parent
+
+        self.name = instantiate_name(
+            original.name, instantiations) if not new_name else new_name
+
+    def to_cpp(self):
+        """Generate the C++ code for wrapping."""
+        instantiated_names = [
+            inst.qualified_name() for inst in self.instantiations
+        ]
+        name = "{}<{}>".format(self.original.name,
+                               ",".join(instantiated_names))
+        namespaces_name = self.namespaces()
+        namespaces_name.append(name)
+        # Leverage Typename to generate the fully qualified C++ name
+        return parser.Typename(namespaces_name).to_cpp()
+
+    def __repr__(self):
+        return "Instantiated {}".format(
+            super(InstantiatedDeclaration, self).__repr__())
+
+
+def instantiate_namespace(namespace):
    """
    Instantiate the classes and other elements in the `namespace` content and
    assign it back to the namespace content attribute.
@ -567,7 +612,8 @@ def instantiate_namespace_inplace(namespace):
            original_element = top_level.find_class_or_function(
                typedef_inst.typename)

-            # Check if element is a typedef'd class or function.
+            # Check if element is a typedef'd class, function or
+            # forward declaration from another project.
            if isinstance(original_element, parser.Class):
                typedef_content.append(
                    InstantiatedClass(original_element,
@ -578,12 +624,19 @@ def instantiate_namespace_inplace(namespace):
                    InstantiatedGlobalFunction(
                        original_element, typedef_inst.typename.instantiations,
                        typedef_inst.new_name))
+            elif isinstance(original_element, parser.ForwardDeclaration):
+                typedef_content.append(
+                    InstantiatedDeclaration(
+                        original_element, typedef_inst.typename.instantiations,
+                        typedef_inst.new_name))

        elif isinstance(element, parser.Namespace):
-            instantiate_namespace_inplace(element)
+            element = instantiate_namespace(element)
            instantiated_content.append(element)
        else:
            instantiated_content.append(element)

    instantiated_content.extend(typedef_content)
    namespace.content = instantiated_content
+
+    return namespace
--- a/scripts/matlab_wrap.py
+++ b/scripts/matlab_wrap.py
@ -8,9 +8,8 @@ and invoked during the wrapping by CMake.

 import argparse
 import os
+import sys

-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper import MatlabWrapper, generate_content

 if __name__ == "__main__":
@ -51,18 +50,11 @@ if __name__ == "__main__":
    if not os.path.exists(args.src):
        os.mkdir(args.src)

-    module = parser.Module.parseString(content)
-
-    instantiator.instantiate_namespace_inplace(module)
-
-    import sys
-
    print("Ignoring classes: {}".format(args.ignore), file=sys.stderr)
-    wrapper = MatlabWrapper(module=module,
-                            module_name=args.module_name,
+    wrapper = MatlabWrapper(module_name=args.module_name,
                            top_module_namespace=top_module_namespaces,
                            ignore_classes=args.ignore)

-    cc_content = wrapper.wrap()
+    cc_content = wrapper.wrap(content)

    generate_content(cc_content, args.out)
--- a/scripts/pybind_wrap.py
+++ b/scripts/pybind_wrap.py
@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-
 """
 Helper script to wrap C++ to Python with Pybind.
 This script is installed via CMake to the user's binary directory
@ -10,8 +9,6 @@ and invoked during the wrapping by CMake.

 import argparse

-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.pybind_wrapper import PybindWrapper


@ -19,11 +16,10 @@ def main():
    """Main runner."""
    arg_parser = argparse.ArgumentParser(
        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
-    arg_parser.add_argument(
-        "--src",
-        type=str,
-        required=True,
-        help="Input interface .i/.h file")
+    arg_parser.add_argument("--src",
+                            type=str,
+                            required=True,
+                            help="Input interface .i/.h file")
    arg_parser.add_argument(
        "--module_name",
        type=str,
@ -62,7 +58,8 @@ def main():
        help="A space-separated list of classes to ignore. "
        "Class names must include their full namespaces.",
    )
-    arg_parser.add_argument("--template", type=str,
+    arg_parser.add_argument("--template",
+                            type=str,
                            help="The module template file")
    args = arg_parser.parse_args()

@ -74,14 +71,10 @@ def main():
    with open(args.src, "r") as f:
        content = f.read()

-    module = parser.Module.parseString(content)
-    instantiator.instantiate_namespace_inplace(module)
-
    with open(args.template, "r") as f:
        template_content = f.read()

    wrapper = PybindWrapper(
-        module=module,
        module_name=args.module_name,
        use_boost=args.use_boost,
        top_module_namespaces=top_module_namespaces,
@ -90,7 +83,7 @@ def main():
    )

    # Wrap the code and get back the cpp/cc code.
-    cc_content = wrapper.wrap()
+    cc_content = wrapper.wrap(content)

    # Generate the C++ code which Pybind11 will use.
    with open(args.out, "w") as f:
--- a/setup.py
+++ b/setup.py
@ -10,7 +10,7 @@ packages = find_packages()
 setup(
    name='gtwrap',
    description='Library to wrap C++ with Python and Matlab',
-    version='1.1.0',
+    version='2.0.0',
    author="Frank Dellaert et. al.",
    author_email="dellaert@gatech.edu",
    license='BSD',
--- a/tests/expected/matlab/MultipleTemplatesIntDouble.m
+++ b/tests/expected/matlab/MultipleTemplatesIntDouble.m
@ -9,7 +9,7 @@ classdef MultipleTemplatesIntDouble < handle
    function obj = MultipleTemplatesIntDouble(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(48, my_ptr);
+        class_wrapper(49, my_ptr);
      else
        error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor');
      end
@ -17,7 +17,7 @@ classdef MultipleTemplatesIntDouble < handle
    end

    function delete(obj)
-      class_wrapper(49, obj.ptr_MultipleTemplatesIntDouble);
+      class_wrapper(50, obj.ptr_MultipleTemplatesIntDouble);
    end

    function display(obj), obj.print(''); end
--- a/tests/expected/matlab/MultipleTemplatesIntFloat.m
+++ b/tests/expected/matlab/MultipleTemplatesIntFloat.m
@ -9,7 +9,7 @@ classdef MultipleTemplatesIntFloat < handle
    function obj = MultipleTemplatesIntFloat(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(50, my_ptr);
+        class_wrapper(51, my_ptr);
      else
        error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor');
      end
@ -17,7 +17,7 @@ classdef MultipleTemplatesIntFloat < handle
    end

    function delete(obj)
-      class_wrapper(51, obj.ptr_MultipleTemplatesIntFloat);
+      class_wrapper(52, obj.ptr_MultipleTemplatesIntFloat);
    end

    function display(obj), obj.print(''); end
--- a/tests/expected/matlab/MyFactorPosePoint2.m
+++ b/tests/expected/matlab/MyFactorPosePoint2.m
@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle
    function obj = MyFactorPosePoint2(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(52, my_ptr);
+        class_wrapper(56, 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 = class_wrapper(53, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = class_wrapper(57, varargin{1}, varargin{2}, varargin{3}, varargin{4});
      else
        error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
      end
@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle
    end

    function delete(obj)
-      class_wrapper(54, obj.ptr_MyFactorPosePoint2);
+      class_wrapper(58, obj.ptr_MyFactorPosePoint2);
    end

    function display(obj), obj.print(''); end
@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle
      % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter')
-        class_wrapper(55, this, varargin{:});
+        class_wrapper(59, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function MyFactorPosePoint2.print');
--- a/tests/expected/matlab/MyVector12.m
+++ b/tests/expected/matlab/MyVector12.m
@ -12,9 +12,9 @@ classdef MyVector12 < handle
    function obj = MyVector12(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(45, my_ptr);
+        class_wrapper(46, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(46);
+        my_ptr = class_wrapper(47);
      else
        error('Arguments do not match any overload of MyVector12 constructor');
      end
@ -22,7 +22,7 @@ classdef MyVector12 < handle
    end

    function delete(obj)
-      class_wrapper(47, obj.ptr_MyVector12);
+      class_wrapper(48, obj.ptr_MyVector12);
    end

    function display(obj), obj.print(''); end
--- a/tests/expected/matlab/MyVector3.m
+++ b/tests/expected/matlab/MyVector3.m
@ -12,9 +12,9 @@ classdef MyVector3 < handle
    function obj = MyVector3(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(42, my_ptr);
+        class_wrapper(43, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(43);
+        my_ptr = class_wrapper(44);
      else
        error('Arguments do not match any overload of MyVector3 constructor');
      end
@ -22,7 +22,7 @@ classdef MyVector3 < handle
    end

    function delete(obj)
-      class_wrapper(44, obj.ptr_MyVector3);
+      class_wrapper(45, obj.ptr_MyVector3);
    end

    function display(obj), obj.print(''); end
--- a/tests/expected/matlab/PrimitiveRefDouble.m
+++ b/tests/expected/matlab/PrimitiveRefDouble.m
@ -19,9 +19,9 @@ classdef PrimitiveRefDouble < handle
    function obj = PrimitiveRefDouble(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(38, my_ptr);
+        class_wrapper(39, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(39);
+        my_ptr = class_wrapper(40);
      else
        error('Arguments do not match any overload of PrimitiveRefDouble constructor');
      end
@ -29,7 +29,7 @@ classdef PrimitiveRefDouble < handle
    end

    function delete(obj)
-      class_wrapper(40, obj.ptr_PrimitiveRefDouble);
+      class_wrapper(41, obj.ptr_PrimitiveRefDouble);
    end

    function display(obj), obj.print(''); end
@ -43,7 +43,7 @@ classdef PrimitiveRefDouble < handle
      % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(41, varargin{:});
+        varargout{1} = class_wrapper(42, varargin{:});
        return
      end

--- a/tests/expected/matlab/TemplatedFunctionRot3.m
+++ b/tests/expected/matlab/TemplatedFunctionRot3.m
@ -1,6 +1,6 @@
 function varargout = TemplatedFunctionRot3(varargin)
      if length(varargin) == 1 && isa(varargin{1},'gtsam.Rot3')
-        functions_wrapper(11, varargin{:});
+        functions_wrapper(14, varargin{:});
      else
        error('Arguments do not match any overload of function TemplatedFunctionRot3');
      end
--- a/tests/expected/matlab/Test.m
+++ b/tests/expected/matlab/Test.m
@ -10,6 +10,7 @@
 %create_MixedPtrs() : returns pair< Test, Test >
 %create_ptrs() : returns pair< Test, Test >
 %get_container() : returns std::vector<testing::Test>
+%lambda() : returns void
 %print() : returns void
 %return_Point2Ptr(bool value) : returns Point2
 %return_Test(Test value) : returns Test
@ -98,11 +99,21 @@ classdef Test < handle
      error('Arguments do not match any overload of function Test.get_container');
    end

+    function varargout = lambda(this, varargin)
+      % LAMBDA usage: lambda() : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        class_wrapper(18, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.lambda');
+    end
+
    function varargout = print(this, varargin)
      % PRINT usage: print() : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        class_wrapper(18, this, varargin{:});
+        class_wrapper(19, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.print');
@ -112,7 +123,7 @@ classdef Test < handle
      % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'logical')
-        varargout{1} = class_wrapper(19, this, varargin{:});
+        varargout{1} = class_wrapper(20, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_Point2Ptr');
@ -122,7 +133,7 @@ classdef Test < handle
      % RETURN_TEST usage: return_Test(Test value) : returns Test
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = class_wrapper(20, this, varargin{:});
+        varargout{1} = class_wrapper(21, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_Test');
@ -132,7 +143,7 @@ classdef Test < handle
      % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = class_wrapper(21, this, varargin{:});
+        varargout{1} = class_wrapper(22, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_TestPtr');
@ -142,7 +153,7 @@ classdef Test < handle
      % RETURN_BOOL usage: return_bool(bool value) : returns bool
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'logical')
-        varargout{1} = class_wrapper(22, this, varargin{:});
+        varargout{1} = class_wrapper(23, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_bool');
@ -152,7 +163,7 @@ classdef Test < handle
      % RETURN_DOUBLE usage: return_double(double value) : returns double
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(23, this, varargin{:});
+        varargout{1} = class_wrapper(24, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_double');
@ -162,7 +173,7 @@ classdef Test < handle
      % RETURN_FIELD usage: return_field(Test t) : returns bool
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = class_wrapper(24, this, varargin{:});
+        varargout{1} = class_wrapper(25, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_field');
@ -172,7 +183,7 @@ classdef Test < handle
      % RETURN_INT usage: return_int(int value) : returns int
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = class_wrapper(25, this, varargin{:});
+        varargout{1} = class_wrapper(26, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_int');
@ -182,7 +193,7 @@ classdef Test < handle
      % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(26, this, varargin{:});
+        varargout{1} = class_wrapper(27, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_matrix1');
@ -192,7 +203,7 @@ classdef Test < handle
      % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(27, this, varargin{:});
+        varargout{1} = class_wrapper(28, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_matrix2');
@ -202,13 +213,13 @@ classdef Test < handle
      % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = class_wrapper(28, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(29, this, varargin{:});
        return
      end
      % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix >
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        [ varargout{1} varargout{2} ] = class_wrapper(29, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_pair');
@ -218,7 +229,7 @@ classdef Test < handle
      % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
-        [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_ptrs');
@ -228,7 +239,7 @@ classdef Test < handle
      % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = class_wrapper(31, this, varargin{:});
+        varargout{1} = class_wrapper(32, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_size_t');
@ -238,7 +249,7 @@ classdef Test < handle
      % RETURN_STRING usage: return_string(string value) : returns string
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'char')
-        varargout{1} = class_wrapper(32, this, varargin{:});
+        varargout{1} = class_wrapper(33, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_string');
@ -248,7 +259,7 @@ classdef Test < handle
      % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = class_wrapper(33, this, varargin{:});
+        varargout{1} = class_wrapper(34, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_vector1');
@ -258,19 +269,13 @@ classdef Test < handle
      % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = class_wrapper(34, this, varargin{:});
+        varargout{1} = class_wrapper(35, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_vector2');
    end

    function varargout = set_container(this, varargin)
-      % SET_CONTAINER usage: set_container(vector<Test> container) : returns void
-      % Doxygen can be found at https://gtsam.org/doxygen/
-      if length(varargin) == 1 && isa(varargin{1},'std.vectorTest')
-        class_wrapper(35, this, varargin{:});
-        return
-      end
      % SET_CONTAINER usage: set_container(vector<Test> container) : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'std.vectorTest')
@ -283,6 +288,12 @@ classdef Test < handle
        class_wrapper(37, this, varargin{:});
        return
      end
+      % SET_CONTAINER usage: set_container(vector<Test> container) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorTest')
+        class_wrapper(38, this, varargin{:});
+        return
+      end
      error('Arguments do not match any overload of function Test.set_container');
    end

--- a/tests/expected/matlab/class_wrapper.cpp
+++ b/tests/expected/matlab/class_wrapper.cpp
@ -33,6 +33,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;

@ -90,6 +92,12 @@ void _deleteAllObjects()
    collector_MultipleTemplatesIntFloat.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
@ -304,14 +312,21 @@ void Test_get_container_17(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap_shared_ptr(boost::make_shared<std::vector<testing::Test>>(obj->get_container()),"std.vectorTest", false);
 }

-void Test_print_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_lambda_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("lambda",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  obj->lambda();
+}
+
+void Test_print_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("print",nargout,nargin-1,0);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
  obj->print();
 }

-void Test_return_Point2Ptr_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Point2Ptr_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_Point2Ptr",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -322,7 +337,7 @@ void Test_return_Point2Ptr_19(int nargout, mxArray *out[], int nargin, const mxA
  }
 }

-void Test_return_Test_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Test_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_Test",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -330,7 +345,7 @@ void Test_return_Test_20(int nargout, mxArray *out[], int nargin, const mxArray
  out[0] = wrap_shared_ptr(boost::make_shared<Test>(obj->return_Test(value)),"Test", false);
 }

-void Test_return_TestPtr_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_TestPtr_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_TestPtr",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -338,7 +353,7 @@ void Test_return_TestPtr_21(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
 }

-void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_bool_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_bool",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -346,7 +361,7 @@ void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray
  out[0] = wrap< bool >(obj->return_bool(value));
 }

-void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_double_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_double",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -354,7 +369,7 @@ void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap< double >(obj->return_double(value));
 }

-void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_field_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_field",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -362,7 +377,7 @@ void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray
  out[0] = wrap< bool >(obj->return_field(t));
 }

-void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_int_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_int",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -370,7 +385,7 @@ void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray *
  out[0] = wrap< int >(obj->return_int(value));
 }

-void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix1_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_matrix1",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -378,7 +393,7 @@ void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Matrix >(obj->return_matrix1(value));
 }

-void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix2_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_matrix2",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -386,7 +401,7 @@ void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Matrix >(obj->return_matrix2(value));
 }

-void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_pair",nargout,nargin-1,2);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -397,7 +412,7 @@ void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap< Matrix >(pairResult.second);
 }

-void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_pair",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -407,7 +422,7 @@ void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap< Matrix >(pairResult.second);
 }

-void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_ptrs_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_ptrs",nargout,nargin-1,2);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -418,7 +433,7 @@ void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }

-void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_size_t_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_size_t",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -426,7 +441,7 @@ void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap< size_t >(obj->return_size_t(value));
 }

-void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_string_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_string",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -434,7 +449,7 @@ void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap< string >(obj->return_string(value));
 }

-void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector1_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_vector1",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -442,7 +457,7 @@ void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Vector >(obj->return_vector1(value));
 }

-void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("return_vector2",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -450,14 +465,6 @@ void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Vector >(obj->return_vector2(value));
 }

-void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("set_container",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  boost::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], "ptr_stdvectorTest");
-  obj->set_container(*container);
-}
-
 void Test_set_container_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("set_container",nargout,nargin-1,1);
@ -474,7 +481,15 @@ void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArra
  obj->set_container(*container);
 }

-void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_set_container_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("set_container",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  boost::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], "ptr_stdvectorTest");
+  obj->set_container(*container);
+}
+
+void PrimitiveRefDouble_collectorInsertAndMakeBase_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@ -483,7 +498,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[
  collector_PrimitiveRefDouble.insert(self);
 }

-void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@ -494,7 +509,7 @@ void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin,
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
  checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1);
@ -507,14 +522,14 @@ void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin
  }
 }

-void PrimitiveRefDouble_Brutal_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_Brutal_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1);
  double t = unwrap< double >(in[0]);
  out[0] = wrap_shared_ptr(boost::make_shared<PrimitiveRef<double>>(PrimitiveRef<double>::Brutal(t)),"PrimitiveRefdouble", false);
 }

-void MyVector3_collectorInsertAndMakeBase_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyVector<3>> Shared;
@ -523,7 +538,7 @@ void MyVector3_collectorInsertAndMakeBase_42(int nargout, mxArray *out[], int na
  collector_MyVector3.insert(self);
 }

-void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyVector<3>> Shared;
@ -534,7 +549,7 @@ void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxA
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<MyVector<3>> Shared;
  checkArguments("delete_MyVector3",nargout,nargin,1);
@ -547,7 +562,7 @@ void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const m
  }
 }

-void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyVector<12>> Shared;
@ -556,7 +571,7 @@ void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int n
  collector_MyVector12.insert(self);
 }

-void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyVector<12>> Shared;
@ -567,7 +582,7 @@ void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mx
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<MyVector<12>> Shared;
  checkArguments("delete_MyVector12",nargout,nargin,1);
@ -580,7 +595,7 @@ void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const
  }
 }

-void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
@ -589,7 +604,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArr
  collector_MultipleTemplatesIntDouble.insert(self);
 }

-void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
  checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1);
@ -602,7 +617,7 @@ void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], in
  }
 }

-void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
@ -611,7 +626,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArra
  collector_MultipleTemplatesIntFloat.insert(self);
 }

-void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
  checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1);
@ -624,7 +639,45 @@ void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int
  }
 }

-void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<ForwardKinematics> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_ForwardKinematics.insert(self);
+}
+
+void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<ForwardKinematics> Shared;
+
+  gtdynamics::Robot& robot = *unwrap_shared_ptr< gtdynamics::Robot >(in[0], "ptr_gtdynamicsRobot");
+  string& start_link_name = *unwrap_shared_ptr< string >(in[1], "ptr_string");
+  string& end_link_name = *unwrap_shared_ptr< string >(in[2], "ptr_string");
+  gtsam::Values& joint_angles = *unwrap_shared_ptr< gtsam::Values >(in[3], "ptr_gtsamValues");
+  gtsam::Pose3& l2Tp = *unwrap_shared_ptr< gtsam::Pose3 >(in[4], "ptr_gtsamPose3");
+  Shared *self = new Shared(new ForwardKinematics(robot,start_link_name,end_link_name,joint_angles,l2Tp));
+  collector_ForwardKinematics.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<ForwardKinematics> Shared;
+  checkArguments("delete_ForwardKinematics",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_ForwardKinematics::iterator item;
+  item = collector_ForwardKinematics.find(self);
+  if(item != collector_ForwardKinematics.end()) {
+    delete self;
+    collector_ForwardKinematics.erase(item);
+  }
+}
+
+void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -633,7 +686,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[
  collector_MyFactorPosePoint2.insert(self);
 }

-void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -648,7 +701,7 @@ void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin,
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_58(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);
@ -661,7 +714,7 @@ void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin
  }
 }

-void MyFactorPosePoint2_print_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("print",nargout,nargin-1,2);
  auto obj = unwrap_shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>(in[0], "ptr_MyFactorPosePoint2");
@ -737,58 +790,58 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      Test_get_container_17(nargout, out, nargin-1, in+1);
      break;
    case 18:
-      Test_print_18(nargout, out, nargin-1, in+1);
+      Test_lambda_18(nargout, out, nargin-1, in+1);
      break;
    case 19:
-      Test_return_Point2Ptr_19(nargout, out, nargin-1, in+1);
+      Test_print_19(nargout, out, nargin-1, in+1);
      break;
    case 20:
-      Test_return_Test_20(nargout, out, nargin-1, in+1);
+      Test_return_Point2Ptr_20(nargout, out, nargin-1, in+1);
      break;
    case 21:
-      Test_return_TestPtr_21(nargout, out, nargin-1, in+1);
+      Test_return_Test_21(nargout, out, nargin-1, in+1);
      break;
    case 22:
-      Test_return_bool_22(nargout, out, nargin-1, in+1);
+      Test_return_TestPtr_22(nargout, out, nargin-1, in+1);
      break;
    case 23:
-      Test_return_double_23(nargout, out, nargin-1, in+1);
+      Test_return_bool_23(nargout, out, nargin-1, in+1);
      break;
    case 24:
-      Test_return_field_24(nargout, out, nargin-1, in+1);
+      Test_return_double_24(nargout, out, nargin-1, in+1);
      break;
    case 25:
-      Test_return_int_25(nargout, out, nargin-1, in+1);
+      Test_return_field_25(nargout, out, nargin-1, in+1);
      break;
    case 26:
-      Test_return_matrix1_26(nargout, out, nargin-1, in+1);
+      Test_return_int_26(nargout, out, nargin-1, in+1);
      break;
    case 27:
-      Test_return_matrix2_27(nargout, out, nargin-1, in+1);
+      Test_return_matrix1_27(nargout, out, nargin-1, in+1);
      break;
    case 28:
-      Test_return_pair_28(nargout, out, nargin-1, in+1);
+      Test_return_matrix2_28(nargout, out, nargin-1, in+1);
      break;
    case 29:
      Test_return_pair_29(nargout, out, nargin-1, in+1);
      break;
    case 30:
-      Test_return_ptrs_30(nargout, out, nargin-1, in+1);
+      Test_return_pair_30(nargout, out, nargin-1, in+1);
      break;
    case 31:
-      Test_return_size_t_31(nargout, out, nargin-1, in+1);
+      Test_return_ptrs_31(nargout, out, nargin-1, in+1);
      break;
    case 32:
-      Test_return_string_32(nargout, out, nargin-1, in+1);
+      Test_return_size_t_32(nargout, out, nargin-1, in+1);
      break;
    case 33:
-      Test_return_vector1_33(nargout, out, nargin-1, in+1);
+      Test_return_string_33(nargout, out, nargin-1, in+1);
      break;
    case 34:
-      Test_return_vector2_34(nargout, out, nargin-1, in+1);
+      Test_return_vector1_34(nargout, out, nargin-1, in+1);
      break;
    case 35:
-      Test_set_container_35(nargout, out, nargin-1, in+1);
+      Test_return_vector2_35(nargout, out, nargin-1, in+1);
      break;
    case 36:
      Test_set_container_36(nargout, out, nargin-1, in+1);
@ -797,58 +850,70 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      Test_set_container_37(nargout, out, nargin-1, in+1);
      break;
    case 38:
-      PrimitiveRefDouble_collectorInsertAndMakeBase_38(nargout, out, nargin-1, in+1);
+      Test_set_container_38(nargout, out, nargin-1, in+1);
      break;
    case 39:
-      PrimitiveRefDouble_constructor_39(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_collectorInsertAndMakeBase_39(nargout, out, nargin-1, in+1);
      break;
    case 40:
-      PrimitiveRefDouble_deconstructor_40(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_constructor_40(nargout, out, nargin-1, in+1);
      break;
    case 41:
-      PrimitiveRefDouble_Brutal_41(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_deconstructor_41(nargout, out, nargin-1, in+1);
      break;
    case 42:
-      MyVector3_collectorInsertAndMakeBase_42(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_Brutal_42(nargout, out, nargin-1, in+1);
      break;
    case 43:
-      MyVector3_constructor_43(nargout, out, nargin-1, in+1);
+      MyVector3_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1);
      break;
    case 44:
-      MyVector3_deconstructor_44(nargout, out, nargin-1, in+1);
+      MyVector3_constructor_44(nargout, out, nargin-1, in+1);
      break;
    case 45:
-      MyVector12_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1);
+      MyVector3_deconstructor_45(nargout, out, nargin-1, in+1);
      break;
    case 46:
-      MyVector12_constructor_46(nargout, out, nargin-1, in+1);
+      MyVector12_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1);
      break;
    case 47:
-      MyVector12_deconstructor_47(nargout, out, nargin-1, in+1);
+      MyVector12_constructor_47(nargout, out, nargin-1, in+1);
      break;
    case 48:
-      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1);
+      MyVector12_deconstructor_48(nargout, out, nargin-1, in+1);
      break;
    case 49:
-      MultipleTemplatesIntDouble_deconstructor_49(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(nargout, out, nargin-1, in+1);
      break;
    case 50:
-      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_deconstructor_50(nargout, out, nargin-1, in+1);
      break;
    case 51:
-      MultipleTemplatesIntFloat_deconstructor_51(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
      break;
    case 52:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_deconstructor_52(nargout, out, nargin-1, in+1);
      break;
    case 53:
-      MyFactorPosePoint2_constructor_53(nargout, out, nargin-1, in+1);
+      ForwardKinematics_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1);
      break;
    case 54:
-      MyFactorPosePoint2_deconstructor_54(nargout, out, nargin-1, in+1);
+      ForwardKinematics_constructor_54(nargout, out, nargin-1, in+1);
      break;
    case 55:
-      MyFactorPosePoint2_print_55(nargout, out, nargin-1, in+1);
+      ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1);
+      break;
+    case 56:
+      MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1);
+      break;
+    case 57:
+      MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1);
+      break;
+    case 58:
+      MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1);
+      break;
+    case 59:
+      MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1);
      break;
    }
  } catch(const std::exception& e) {
--- a/tests/expected/matlab/functions_wrapper.cpp
+++ b/tests/expected/matlab/functions_wrapper.cpp
@ -33,6 +33,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;

@ -90,6 +92,12 @@ void _deleteAllObjects()
    collector_MultipleTemplatesIntFloat.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
@ -198,9 +206,10 @@ void MultiTemplatedFunctionDoubleSize_tDouble_7(int nargout, mxArray *out[], int
 }
 void DefaultFuncInt_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("DefaultFuncInt",nargout,nargin,1);
+  checkArguments("DefaultFuncInt",nargout,nargin,2);
  int a = unwrap< int >(in[0]);
-  DefaultFuncInt(a);
+  int b = unwrap< int >(in[1]);
+  DefaultFuncInt(a,b);
 }
 void DefaultFuncString_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
@ -215,7 +224,30 @@ void DefaultFuncObj_10(int nargout, mxArray *out[], int nargin, const mxArray *i
  gtsam::KeyFormatter& keyFormatter = *unwrap_shared_ptr< gtsam::KeyFormatter >(in[0], "ptr_gtsamKeyFormatter");
  DefaultFuncObj(keyFormatter);
 }
-void TemplatedFunctionRot3_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void DefaultFuncZero_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("DefaultFuncZero",nargout,nargin,5);
+  int a = unwrap< int >(in[0]);
+  int b = unwrap< int >(in[1]);
+  double c = unwrap< double >(in[2]);
+  bool d = unwrap< bool >(in[3]);
+  bool e = unwrap< bool >(in[4]);
+  DefaultFuncZero(a,b,c,d,e);
+}
+void DefaultFuncVector_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("DefaultFuncVector",nargout,nargin,2);
+  std::vector<int>& i = *unwrap_shared_ptr< std::vector<int> >(in[0], "ptr_stdvectorint");
+  std::vector<string>& s = *unwrap_shared_ptr< std::vector<string> >(in[1], "ptr_stdvectorstring");
+  DefaultFuncVector(i,s);
+}
+void setPose_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("setPose",nargout,nargin,1);
+  gtsam::Pose3& pose = *unwrap_shared_ptr< gtsam::Pose3 >(in[0], "ptr_gtsamPose3");
+  setPose(pose);
+}
+void TemplatedFunctionRot3_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("TemplatedFunctionRot3",nargout,nargin,1);
  gtsam::Rot3& t = *unwrap_shared_ptr< gtsam::Rot3 >(in[0], "ptr_gtsamRot3");
@ -267,7 +299,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      DefaultFuncObj_10(nargout, out, nargin-1, in+1);
      break;
    case 11:
-      TemplatedFunctionRot3_11(nargout, out, nargin-1, in+1);
+      DefaultFuncZero_11(nargout, out, nargin-1, in+1);
+      break;
+    case 12:
+      DefaultFuncVector_12(nargout, out, nargin-1, in+1);
+      break;
+    case 13:
+      setPose_13(nargout, out, nargin-1, in+1);
+      break;
+    case 14:
+      TemplatedFunctionRot3_14(nargout, out, nargin-1, in+1);
      break;
    }
  } catch(const std::exception& e) {
--- a/tests/expected/matlab/geometry_wrapper.cpp
+++ b/tests/expected/matlab/geometry_wrapper.cpp
@ -36,6 +36,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@ -97,6 +99,12 @@ void _deleteAllObjects()
    collector_MultipleTemplatesIntFloat.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
--- a/tests/expected/matlab/inheritance_wrapper.cpp
+++ b/tests/expected/matlab/inheritance_wrapper.cpp
@ -38,6 +38,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@ -107,6 +109,12 @@ void _deleteAllObjects()
    collector_MultipleTemplatesIntFloat.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
@ -564,12 +572,12 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx
  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
 }

-void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("set_container",nargout,nargin-1,1);
+  checkArguments("return_vector2",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  boost::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], "ptr_stdvectorTest");
-  obj->set_container(*container);
+  Vector value = unwrap< Vector >(in[1]);
+  out[0] = wrap< Vector >(obj->return_vector2(value));
 }

 void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@ -716,7 +724,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1);
      break;
    case 35:
-      Test_set_container_35(nargout, out, nargin-1, in+1);
+      Test_return_vector2_35(nargout, out, nargin-1, in+1);
      break;
    case 36:
      ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);
--- a/tests/expected/matlab/namespaces_wrapper.cpp
+++ b/tests/expected/matlab/namespaces_wrapper.cpp
@ -8,6 +8,7 @@
 #include <folder/path/to/Test.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/Values.h>
 #include <path/to/ns1.h>
 #include <path/to/ns1/ClassB.h>
 #include <path/to/ns2.h>
@ -43,6 +44,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@ -69,6 +72,8 @@ typedef std::set<boost::shared_ptr<ns2::ClassC>*> Collector_ns2ClassC;
 static Collector_ns2ClassC collector_ns2ClassC;
 typedef std::set<boost::shared_ptr<ClassD>*> Collector_ClassD;
 static Collector_ClassD collector_ClassD;
+typedef std::set<boost::shared_ptr<gtsam::Values>*> Collector_gtsamValues;
+static Collector_gtsamValues collector_gtsamValues;

 void _deleteAllObjects()
 {
@ -124,6 +129,12 @@ void _deleteAllObjects()
    collector_MultipleTemplatesIntFloat.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
@ -202,6 +213,12 @@ void _deleteAllObjects()
    collector_ClassD.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin();
+      iter != collector_gtsamValues.end(); ) {
+    delete *iter;
+    collector_gtsamValues.erase(iter++);
+    anyDeleted = true;
+  } }
  if(anyDeleted)
    cout <<
      "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@ -491,6 +508,69 @@ void ClassD_deconstructor_25(int nargout, mxArray *out[], int nargin, const mxAr
  }
 }

+void gtsamValues_collectorInsertAndMakeBase_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_gtsamValues.insert(self);
+}
+
+void gtsamValues_constructor_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+
+  Shared *self = new Shared(new gtsam::Values());
+  collector_gtsamValues.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamValues_constructor_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+
+  gtsam::Values& other = *unwrap_shared_ptr< gtsam::Values >(in[0], "ptr_gtsamValues");
+  Shared *self = new Shared(new gtsam::Values(other));
+  collector_gtsamValues.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamValues_deconstructor_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+  checkArguments("delete_gtsamValues",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_gtsamValues::iterator item;
+  item = collector_gtsamValues.find(self);
+  if(item != collector_gtsamValues.end()) {
+    delete self;
+    collector_gtsamValues.erase(item);
+  }
+}
+
+void gtsamValues_insert_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("insert",nargout,nargin-1,2);
+  auto obj = unwrap_shared_ptr<gtsam::Values>(in[0], "ptr_gtsamValues");
+  size_t j = unwrap< size_t >(in[1]);
+  Vector vector = unwrap< Vector >(in[2]);
+  obj->insert(j,vector);
+}
+
+void gtsamValues_insert_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("insert",nargout,nargin-1,2);
+  auto obj = unwrap_shared_ptr<gtsam::Values>(in[0], "ptr_gtsamValues");
+  size_t j = unwrap< size_t >(in[1]);
+  Matrix matrix = unwrap< Matrix >(in[2]);
+  obj->insert(j,matrix);
+}
+

 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
@ -581,6 +661,24 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
    case 25:
      ClassD_deconstructor_25(nargout, out, nargin-1, in+1);
      break;
+    case 26:
+      gtsamValues_collectorInsertAndMakeBase_26(nargout, out, nargin-1, in+1);
+      break;
+    case 27:
+      gtsamValues_constructor_27(nargout, out, nargin-1, in+1);
+      break;
+    case 28:
+      gtsamValues_constructor_28(nargout, out, nargin-1, in+1);
+      break;
+    case 29:
+      gtsamValues_deconstructor_29(nargout, out, nargin-1, in+1);
+      break;
+    case 30:
+      gtsamValues_insert_30(nargout, out, nargin-1, in+1);
+      break;
+    case 31:
+      gtsamValues_insert_31(nargout, out, nargin-1, in+1);
+      break;
    }
  } catch(const std::exception& e) {
    mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());
--- a/tests/expected/matlab/special_cases_wrapper.cpp
+++ b/tests/expected/matlab/special_cases_wrapper.cpp
@ -9,6 +9,7 @@
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/Values.h>
 #include <path/to/ns1.h>
 #include <path/to/ns1/ClassB.h>
 #include <path/to/ns2.h>
@ -46,6 +47,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@ -72,6 +75,8 @@ typedef std::set<boost::shared_ptr<ns2::ClassC>*> Collector_ns2ClassC;
 static Collector_ns2ClassC collector_ns2ClassC;
 typedef std::set<boost::shared_ptr<ClassD>*> Collector_ClassD;
 static Collector_ClassD collector_ClassD;
+typedef std::set<boost::shared_ptr<gtsam::Values>*> Collector_gtsamValues;
+static Collector_gtsamValues collector_gtsamValues;
 typedef std::set<boost::shared_ptr<gtsam::NonlinearFactorGraph>*> Collector_gtsamNonlinearFactorGraph;
 static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph;
 typedef std::set<boost::shared_ptr<gtsam::SfmTrack>*> Collector_gtsamSfmTrack;
@ -135,6 +140,12 @@ void _deleteAllObjects()
    collector_MultipleTemplatesIntFloat.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
      iter != collector_MyFactorPosePoint2.end(); ) {
    delete *iter;
@ -213,6 +224,12 @@ void _deleteAllObjects()
    collector_ClassD.erase(iter++);
    anyDeleted = true;
  } }
+  { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin();
+      iter != collector_gtsamValues.end(); ) {
+    delete *iter;
+    collector_gtsamValues.erase(iter++);
+    anyDeleted = true;
+  } }
  { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin();
      iter != collector_gtsamNonlinearFactorGraph.end(); ) {
    delete *iter;
--- a/tests/expected/python/class_pybind.cpp
+++ b/tests/expected/python/class_pybind.cpp
@ -55,13 +55,14 @@ PYBIND11_MODULE(class_py, m_) {
        .def("create_ptrs",[](Test* self){return self->create_ptrs();})
        .def("create_MixedPtrs",[](Test* self){return self->create_MixedPtrs();})
        .def("return_ptrs",[](Test* self, std::shared_ptr<Test> p1, std::shared_ptr<Test> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
-        .def("print_",[](Test* self){ py::scoped_ostream_redirect output; self->print();})
+        .def("print",[](Test* self){ py::scoped_ostream_redirect output; self->print();})
        .def("__repr__",
                    [](const Test& self){
                        gtsam::RedirectCout redirect;
                        self.print();
                        return redirect.str();
                    })
+        .def("lambda_",[](Test* self){ self->lambda();})
        .def("set_container",[](Test* self, std::vector<testing::Test> container){ self->set_container(container);}, py::arg("container"))
        .def("set_container",[](Test* self, std::vector<std::shared_ptr<testing::Test>> container){ self->set_container(container);}, py::arg("container"))
        .def("set_container",[](Test* self, std::vector<testing::Test&> container){ self->set_container(container);}, py::arg("container"))
@ -82,9 +83,12 @@ PYBIND11_MODULE(class_py, m_) {

    py::class_<MultipleTemplates<int, float>, std::shared_ptr<MultipleTemplates<int, float>>>(m_, "MultipleTemplatesIntFloat");

+    py::class_<ForwardKinematics, std::shared_ptr<ForwardKinematics>>(m_, "ForwardKinematics")
+        .def(py::init<const gtdynamics::Robot&, const string&, const string&, const gtsam::Values&, const gtsam::Pose3&>(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3());
+
    py::class_<MyFactor<gtsam::Pose2, gtsam::Matrix>, std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>>(m_, "MyFactorPosePoint2")
        .def(py::init<size_t, size_t, double, const std::shared_ptr<gtsam::noiseModel::Base>>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel"))
-        .def("print_",[](MyFactor<gtsam::Pose2, gtsam::Matrix>* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter)
+        .def("print",[](MyFactor<gtsam::Pose2, gtsam::Matrix>* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter)
        .def("__repr__",
                    [](const MyFactor<gtsam::Pose2, gtsam::Matrix>& self, const string& s, const gtsam::KeyFormatter& keyFormatter){
                        gtsam::RedirectCout redirect;
@ -92,6 +96,7 @@ PYBIND11_MODULE(class_py, m_) {
                        return redirect.str();
                    }, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter);

+    py::class_<SuperCoolFactor<gtsam::Pose3>, std::shared_ptr<SuperCoolFactor<gtsam::Pose3>>>(m_, "SuperCoolFactorPose3")

 #include "python/specializations.h"

--- a/tests/expected/python/functions_pybind.cpp
+++ b/tests/expected/python/functions_pybind.cpp
@ -30,9 +30,12 @@ PYBIND11_MODULE(functions_py, m_) {
    m_.def("overloadedGlobalFunction",[](int a, double b){return ::overloadedGlobalFunction(a, b);}, py::arg("a"), py::arg("b"));
    m_.def("MultiTemplatedFunctionStringSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction<string,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
    m_.def("MultiTemplatedFunctionDoubleSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction<double,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
-    m_.def("DefaultFuncInt",[](int a){ ::DefaultFuncInt(a);}, py::arg("a") = 123);
+    m_.def("DefaultFuncInt",[](int a, int b){ ::DefaultFuncInt(a, b);}, py::arg("a") = 123, py::arg("b") = 0);
    m_.def("DefaultFuncString",[](const string& s, const string& name){ ::DefaultFuncString(s, name);}, py::arg("s") = "hello", py::arg("name") = "");
    m_.def("DefaultFuncObj",[](const gtsam::KeyFormatter& keyFormatter){ ::DefaultFuncObj(keyFormatter);}, py::arg("keyFormatter") = gtsam::DefaultKeyFormatter);
+    m_.def("DefaultFuncZero",[](int a, int b, double c, bool d, bool e){ ::DefaultFuncZero(a, b, c, d, e);}, py::arg("a") = 0, py::arg("b"), py::arg("c") = 0.0, py::arg("d") = false, py::arg("e"));
+    m_.def("DefaultFuncVector",[](const std::vector<int>& i, const std::vector<string>& s){ ::DefaultFuncVector(i, s);}, py::arg("i") = {1, 2, 3}, py::arg("s") = {"borglab", "gtsam"});
+    m_.def("setPose",[](const gtsam::Pose3& pose){ ::setPose(pose);}, py::arg("pose") = gtsam::Pose3());
    m_.def("TemplatedFunctionRot3",[](const gtsam::Rot3& t){ ::TemplatedFunction<Rot3>(t);}, py::arg("t"));

 #include "python/specializations.h"
--- a/tests/expected/python/namespaces_pybind.cpp
+++ b/tests/expected/python/namespaces_pybind.cpp
@ -11,6 +11,7 @@
 #include "path/to/ns2.h"
 #include "path/to/ns2/ClassA.h"
 #include "path/to/ns3.h"
+#include "gtsam/nonlinear/Values.h"

 #include "wrap/serialization.h"
 #include <boost/serialization/export.hpp>
@ -57,7 +58,16 @@ PYBIND11_MODULE(namespaces_py, m_) {
    py::class_<ClassD, std::shared_ptr<ClassD>>(m_, "ClassD")
        .def(py::init<>());

-    m_.attr("aGlobalVar") = aGlobalVar;
+    m_.attr("aGlobalVar") = aGlobalVar;    pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule");
+
+    py::class_<gtsam::Values, std::shared_ptr<gtsam::Values>>(m_gtsam, "Values")
+        .def(py::init<>())
+        .def(py::init<const gtsam::Values&>(), py::arg("other"))
+        .def("insert_vector",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector"))
+        .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector"))
+        .def("insert_matrix",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix"))
+        .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix"));
+

 #include "python/specializations.h"

--- a/tests/fixtures/class.i
+++ b/tests/fixtures/class.i
@ -61,7 +61,10 @@ class Test {
  pair<Test ,Test*> create_MixedPtrs () const;
  pair<Test*,Test*> return_ptrs (Test* p1, Test* p2) const;

+  // This should be callable as .print() in python
  void print() const;
+  // Since this is a reserved keyword, it should be updated to `lambda_`
+  void lambda() const;

  void set_container(std::vector<testing::Test> container);
  void set_container(std::vector<testing::Test*> container);
@ -106,3 +109,14 @@ class MyVector {
 // Class with multiple instantiated templates
 template<T = {int}, U = {double, float}>
 class MultipleTemplates {};
+
+// Test for default args in constructor
+class ForwardKinematics {
+  ForwardKinematics(const gtdynamics::Robot& robot,
+                    const string& start_link_name, const string& end_link_name,
+                    const gtsam::Values& joint_angles,
+                    const gtsam::Pose3& l2Tp = gtsam::Pose3());
+};
+
+class SuperCoolFactor;
+typedef SuperCoolFactor<gtsam::Pose3> SuperCoolFactorPose3;
--- a/tests/fixtures/functions.i
+++ b/tests/fixtures/functions.i
@ -28,6 +28,11 @@ void TemplatedFunction(const T& t);
 typedef TemplatedFunction<gtsam::Rot3> TemplatedFunctionRot3;

 // Check default arguments
-void DefaultFuncInt(int a = 123);
+void DefaultFuncInt(int a = 123, int b = 0);
 void DefaultFuncString(const string& s = "hello", const string& name = "");
 void DefaultFuncObj(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void DefaultFuncZero(int a = 0, int b, double c = 0.0, bool d = false, bool e);
+void DefaultFuncVector(const std::vector<int> &i = {1, 2, 3}, const std::vector<string> &s = {"borglab", "gtsam"});
+
+// Test for non-trivial default constructor
+void setPose(const gtsam::Pose3& pose = gtsam::Pose3());
--- a/tests/fixtures/namespaces.i
+++ b/tests/fixtures/namespaces.i
@ -60,3 +60,14 @@ class ClassD {
 };

 int aGlobalVar;
+
+namespace gtsam {
+  #include <gtsam/nonlinear/Values.h>
+class Values {
+  Values();
+  Values(const gtsam::Values& other);
+
+  void insert(size_t j, Vector vector);
+  void insert(size_t j, Matrix matrix);
+};
+}
--- a/tests/test_docs.py
+++ b/tests/test_docs.py
@ -41,7 +41,6 @@ class TestDocument(unittest.TestCase):
    OUTPUT_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, OUTPUT_XML_DIR))
    EXPECTED_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, EXPECTED_XML_DIR))

-    @unittest.skip("DOC_DIR_PATH doesn't exist")
    def test_generate_xml(self):
        """Test parse_xml.generate_xml"""
        if path.exists(self.OUTPUT_XML_DIR_PATH):
@ -65,7 +64,6 @@ class TestDocument(unittest.TestCase):

        self.assertTrue(not dircmp.diff_files and not dircmp.funny_files)

-    @unittest.skip("DOC_DIR_PATH doesn't exist")
    def test_parse(self):
        """Test the parsing of the XML generated by Doxygen."""
        docs = parser.ParseDoxygenXML(self.DOC_DIR_PATH,
--- a/tests/test_interface_parser.py
+++ b/tests/test_interface_parser.py
@ -142,9 +142,9 @@ class TestInterfaceParser(unittest.TestCase):
            "const C6* c6"
        args = ArgumentList.rule.parseString(arg_string)[0]

-        self.assertEqual(7, len(args.args_list))
+        self.assertEqual(7, len(args.list()))
        self.assertEqual(['a', 'c1', 'c2', 'c3', 'c4', 'c5', 'c6'],
-                         args.args_names())
+                         args.names())

    def test_argument_list_qualifiers(self):
        """
@ -153,7 +153,7 @@ class TestInterfaceParser(unittest.TestCase):
        """
        arg_string = "double x1, double* x2, double& x3, double@ x4, " \
            "const double x5, const double* x6, const double& x7, const double@ x8"
-        args = ArgumentList.rule.parseString(arg_string)[0].args_list
+        args = ArgumentList.rule.parseString(arg_string)[0].list()
        self.assertEqual(8, len(args))
        self.assertFalse(args[1].ctype.is_ptr and args[1].ctype.is_shared_ptr
                         and args[1].ctype.is_ref)
@ -169,7 +169,7 @@ class TestInterfaceParser(unittest.TestCase):
        """Test arguments list where the arguments can be templated."""
        arg_string = "std::pair<string, double> steps, vector<T*> vector_of_pointers"
        args = ArgumentList.rule.parseString(arg_string)[0]
-        args_list = args.args_list
+        args_list = args.list()
        self.assertEqual(2, len(args_list))
        self.assertEqual("std::pair<string, double>",
                         args_list[0].ctype.to_cpp(False))
@ -180,30 +180,62 @@ class TestInterfaceParser(unittest.TestCase):

    def test_default_arguments(self):
        """Tests any expression that is a valid default argument"""
-        args = ArgumentList.rule.parseString(
-            "string c = \"\", string s=\"hello\", int a=3, "
-            "int b, double pi = 3.1415, "
-            "gtsam::KeyFormatter kf = gtsam::DefaultKeyFormatter, "
-            "std::vector<size_t> p = std::vector<size_t>(), "
-            "std::vector<size_t> l = (1, 2, 'name', \"random\", 3.1415)"
-        )[0].args_list
+        args = ArgumentList.rule.parseString("""
+            string c = "", int z = 0, double z2 = 0.0, bool f = false,
+            string s="hello"+"goodbye", char c='a', int a=3,
+            int b, double pi = 3.1415""")[0].list()

        # Test for basic types
-        self.assertEqual(args[0].default, "")
-        self.assertEqual(args[1].default, "hello")
-        self.assertEqual(args[2].default, 3)
+        self.assertEqual(args[0].default, '""')
+        self.assertEqual(args[1].default, '0')
+        self.assertEqual(args[2].default, '0.0')
+        self.assertEqual(args[3].default, "false")
+        self.assertEqual(args[4].default, '"hello"+"goodbye"')
+        self.assertEqual(args[5].default, "'a'")
+        self.assertEqual(args[6].default, '3')
        # No default argument should set `default` to None
-        self.assertIsNone(args[3].default)
+        self.assertIsNone(args[7].default)
+        self.assertEqual(args[8].default, '3.1415')

-        self.assertEqual(args[4].default, 3.1415)
+        arg0 = 'gtsam::DefaultKeyFormatter'
+        arg1 = 'std::vector<size_t>()'
+        arg2 = '{1, 2}'
+        arg3 = '[&c1, &c2](string s=5, int a){return s+"hello"+a+c1+c2;}'
+        arg4 = 'gtsam::Pose3()'
+        arg5 = 'Factor<gtsam::Pose3, gtsam::Point3>()'
+        arg6 = 'gtsam::Point3(1, 2, 3)'
+        arg7 = 'ns::Class<T, U>(3, 2, 1, "name")'
+
+        argument_list = """
+            gtsam::KeyFormatter kf = {arg0},
+            std::vector<size_t> v = {arg1},
+            std::vector<size_t> l = {arg2},
+            gtsam::KeyFormatter lambda = {arg3},
+            gtsam::Pose3 p = {arg4},
+            Factor<gtsam::Pose3, gtsam::Point3> x = {arg5},
+            gtsam::Point3 x = {arg6},
+            ns::Class<T, U> obj = {arg7}
+            """.format(arg0=arg0,
+                       arg1=arg1,
+                       arg2=arg2,
+                       arg3=arg3,
+                       arg4=arg4,
+                       arg5=arg5,
+                       arg6=arg6,
+                       arg7=arg7)
+        args = ArgumentList.rule.parseString(argument_list)[0].list()

        # Test non-basic type
-        self.assertEqual(repr(args[5].default.typename),
-                         'gtsam::DefaultKeyFormatter')
+        self.assertEqual(args[0].default, arg0)
        # Test templated type
-        self.assertEqual(repr(args[6].default.typename), 'std::vector<size_t>')
-        # Test for allowing list as default argument
-        self.assertEqual(args[7].default, (1, 2, 'name', "random", 3.1415))
+        self.assertEqual(args[1].default, arg1)
+        self.assertEqual(args[2].default, arg2)
+        self.assertEqual(args[3].default, arg3)
+        self.assertEqual(args[4].default, arg4)
+        self.assertEqual(args[5].default, arg5)
+        self.assertEqual(args[6].default, arg6)
+        # Test for default argument with multiple templates and params
+        self.assertEqual(args[7].default, arg7)

    def test_return_type(self):
        """Test ReturnType"""
@ -273,6 +305,15 @@ class TestInterfaceParser(unittest.TestCase):
        self.assertEqual("f", ret.name)
        self.assertEqual(3, len(ret.args))

+        ret = Constructor.rule.parseString(
+            """ForwardKinematics(const gtdynamics::Robot& robot,
+                    const string& start_link_name, const string& end_link_name,
+                    const gtsam::Values& joint_angles,
+                    const gtsam::Pose3& l2Tp = gtsam::Pose3());""")[0]
+        self.assertEqual("ForwardKinematics", ret.name)
+        self.assertEqual(5, len(ret.args))
+        self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default)
+
    def test_operator_overload(self):
        """Test for operator overloading."""
        # Unary operator
@ -296,7 +337,7 @@ class TestInterfaceParser(unittest.TestCase):
                         ret.return_type.type1.typename.to_cpp())
        self.assertTrue(len(ret.args) == 1)
        self.assertEqual("const gtsam::Vector2 &",
-                         repr(ret.args.args_list[0].ctype))
+                         repr(ret.args.list()[0].ctype))
        self.assertTrue(not ret.is_unary)

    def test_typedef_template_instantiation(self):
@ -392,6 +433,16 @@ class TestInterfaceParser(unittest.TestCase):
        self.assertEqual(0, len(ret.properties))
        self.assertTrue(not ret.is_virtual)

+    def test_templated_class(self):
+        """Test a templated class."""
+        ret = Class.rule.parseString("""
+        template<POSE, POINT>
+        class MyFactor {};
+        """)[0]
+
+        self.assertEqual("MyFactor", ret.name)
+        self.assertEqual("<POSE, POINT>", repr(ret.template))
+
    def test_class_inheritance(self):
        """Test for class inheritance."""
        ret = Class.rule.parseString("""
@ -446,8 +497,7 @@ class TestInterfaceParser(unittest.TestCase):
        fwd = ForwardDeclaration.rule.parseString(
            "virtual class Test:gtsam::Point3;")[0]

-        fwd_name = fwd.name
-        self.assertEqual("Test", fwd_name.name)
+        self.assertEqual("Test", fwd.name)
        self.assertTrue(fwd.is_virtual)

    def test_function(self):
@ -469,14 +519,26 @@ class TestInterfaceParser(unittest.TestCase):
        variable = Variable.rule.parseString("string kGravity = 9.81;")[0]
        self.assertEqual(variable.name, "kGravity")
        self.assertEqual(variable.ctype.typename.name, "string")
-        self.assertEqual(variable.default, 9.81)
+        self.assertEqual(variable.default, "9.81")

        variable = Variable.rule.parseString(
            "const string kGravity = 9.81;")[0]
        self.assertEqual(variable.name, "kGravity")
        self.assertEqual(variable.ctype.typename.name, "string")
        self.assertTrue(variable.ctype.is_const)
-        self.assertEqual(variable.default, 9.81)
+        self.assertEqual(variable.default, "9.81")
+
+        variable = Variable.rule.parseString(
+            "gtsam::Pose3 wTc = gtsam::Pose3();")[0]
+        self.assertEqual(variable.name, "wTc")
+        self.assertEqual(variable.ctype.typename.name, "Pose3")
+        self.assertEqual(variable.default, "gtsam::Pose3()")
+
+        variable = Variable.rule.parseString(
+            "gtsam::Pose3 wTc = gtsam::Pose3(1, 2, 0);")[0]
+        self.assertEqual(variable.name, "wTc")
+        self.assertEqual(variable.ctype.typename.name, "Pose3")
+        self.assertEqual(variable.default, "gtsam::Pose3(1, 2, 0)")

    def test_enumerator(self):
        """Test for enumerator."""
--- a/tests/test_matlab_wrapper.py
+++ b/tests/test_matlab_wrapper.py
@ -15,8 +15,6 @@ from loguru import logger

 sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))

-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper import MatlabWrapper


@ -117,19 +115,14 @@ class TestWrap(unittest.TestCase):
        if not osp.exists(self.MATLAB_ACTUAL_DIR):
            os.mkdir(self.MATLAB_ACTUAL_DIR)

-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        # Create MATLAB wrapper instance
        wrapper = MatlabWrapper(
-            module=module,
            module_name='geometry',
            top_module_namespace=['gtsam'],
            ignore_classes=[''],
        )

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        self.generate_content(cc_content)

@ -148,18 +141,13 @@ class TestWrap(unittest.TestCase):
        if not osp.exists(self.MATLAB_ACTUAL_DIR):
            os.mkdir(self.MATLAB_ACTUAL_DIR)

-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        wrapper = MatlabWrapper(
-            module=module,
            module_name='functions',
            top_module_namespace=['gtsam'],
            ignore_classes=[''],
        )

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        self.generate_content(cc_content)

@ -181,18 +169,13 @@ class TestWrap(unittest.TestCase):
        if not osp.exists(self.MATLAB_ACTUAL_DIR):
            os.mkdir(self.MATLAB_ACTUAL_DIR)

-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        wrapper = MatlabWrapper(
-            module=module,
            module_name='class',
            top_module_namespace=['gtsam'],
            ignore_classes=[''],
        )

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        self.generate_content(cc_content)

@ -214,18 +197,13 @@ class TestWrap(unittest.TestCase):
        if not osp.exists(self.MATLAB_ACTUAL_DIR):
            os.mkdir(self.MATLAB_ACTUAL_DIR)

-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        wrapper = MatlabWrapper(
-            module=module,
            module_name='inheritance',
            top_module_namespace=['gtsam'],
            ignore_classes=[''],
        )

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        self.generate_content(cc_content)

@ -237,7 +215,7 @@ class TestWrap(unittest.TestCase):
        for file in files:
            self.compare_and_diff(file)

-    def test_namspaces(self):
+    def test_namespaces(self):
        """
        Test interface file with full namespace definition.
        """
@ -247,18 +225,13 @@ class TestWrap(unittest.TestCase):
        if not osp.exists(self.MATLAB_ACTUAL_DIR):
            os.mkdir(self.MATLAB_ACTUAL_DIR)

-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        wrapper = MatlabWrapper(
-            module=module,
            module_name='namespaces',
            top_module_namespace=['gtsam'],
            ignore_classes=[''],
        )

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        self.generate_content(cc_content)

@ -282,29 +255,25 @@ class TestWrap(unittest.TestCase):
        if not osp.exists(self.MATLAB_ACTUAL_DIR):
            os.mkdir(self.MATLAB_ACTUAL_DIR)

-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        wrapper = MatlabWrapper(
-            module=module,
            module_name='special_cases',
            top_module_namespace=['gtsam'],
            ignore_classes=[''],
        )

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        self.generate_content(cc_content)

        files = [
            'special_cases_wrapper.cpp',
            '+gtsam/PinholeCameraCal3Bundler.m',
-            '+gtsam/NonlinearFactorGraph.m', 
+            '+gtsam/NonlinearFactorGraph.m',
        ]

        for file in files:
            self.compare_and_diff(file)

+
 if __name__ == '__main__':
    unittest.main()
--- a/tests/test_pybind_wrapper.py
+++ b/tests/test_pybind_wrapper.py
@ -16,8 +16,6 @@ sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
 sys.path.append(
    osp.normpath(osp.abspath(osp.join(__file__, '../../../build/wrap'))))

-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.pybind_wrapper import PybindWrapper

 sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
@ -37,23 +35,18 @@ class TestWrap(unittest.TestCase):
        """
        Common function to wrap content.
        """
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
        with open(osp.join(self.TEST_DIR,
                           "pybind_wrapper.tpl")) as template_file:
            module_template = template_file.read()

        # Create Pybind wrapper instance
-        wrapper = PybindWrapper(module=module,
-                                module_name=module_name,
+        wrapper = PybindWrapper(module_name=module_name,
                                use_boost=False,
                                top_module_namespaces=[''],
                                ignore_classes=[''],
                                module_template=module_template)

-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)

        output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp")

@ -165,10 +158,10 @@ class TestWrap(unittest.TestCase):
        with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f:
            content = f.read()

-        output = self.wrap_content(content, 'enum_py',
-                                   self.PYTHON_ACTUAL_DIR)
+        output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR)

        self.compare_and_diff('enum_pybind.cpp', output)

+
 if __name__ == '__main__':
    unittest.main()