Merge pull request #953 from borglab/fix/matlab-wrapper

Fix Matlab Wrapper
2021-12-07 12:45:30 -05:00 · 2021-12-07 12:45:30 -05:00 · 16dc333072
parent c7781961e6 1cd93d84e6
commit 16dc333072
52 changed files with 678 additions and 280 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -87,6 +87,13 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
        CACHE STRING "The Python version to use for wrapping")
    # Set the include directory for matlab.h
    set(GTWRAP_INCLUDE_NAME "wrap")
+
+    # Copy matlab.h to the correct folder.
+    configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
+               ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
+    # Add the include directories so that matlab.h can be found
+    include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
+
    add_subdirectory(wrap)
    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
 endif()
--- a/gtsam/base/base.i
+++ b/gtsam/base/base.i
@ -38,7 +38,7 @@ class DSFMap {
  DSFMap();
  KEY find(const KEY& key) const;
  void merge(const KEY& x, const KEY& y);
-  std::map<KEY, Set> sets();
+  std::map<KEY, This::Set> sets();
 };

 class IndexPairSet {
--- a/gtsam/basis/basis.i
+++ b/gtsam/basis/basis.i
@ -140,7 +140,7 @@ class FitBasis {
  static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
      const std::map<double, double>& sequence,
      const gtsam::noiseModel::Base* model, size_t N);
-  Parameters parameters() const;
+  This::Parameters parameters() const;
 };

 }  // namespace gtsam
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@ -168,7 +168,7 @@ template <POSE>
 virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
  PoseTranslationPrior(size_t key, const POSE& pose_z,
                       const gtsam::noiseModel::Base* noiseModel);
-  POSE measured() const;
+  POSE::Translation measured() const;

  // enabling serialization functionality
  void serialize() const;
@ -185,7 +185,7 @@ template <POSE>
 virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
  PoseRotationPrior(size_t key, const POSE& pose_z,
                    const gtsam::noiseModel::Base* noiseModel);
-  POSE measured() const;
+  POSE::Rotation measured() const;
 };

 typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
--- a/matlab/+gtsam/VisualISAMInitialize.m
+++ b/matlab/+gtsam/VisualISAMInitialize.m
@ -12,11 +12,11 @@ end
 isam = ISAM2(params);

 %% Set Noise parameters
-noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]', true);
 %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]');
-noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1);
-noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0);
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]', true);
+noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1, true);
+noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0, true);

 %% Add constraints/priors
 % TODO: should not be from ground truth!
--- a/matlab/CMakeLists.txt
+++ b/matlab/CMakeLists.txt
@ -64,8 +64,21 @@ set(ignore
    gtsam::Point3
    gtsam::CustomFactor)

+set(interface_files
+    ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/base/base.i
+    ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i
+    ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
+    ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
+    ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/sfm/sfm.i
+    ${GTSAM_SOURCE_DIR}/gtsam/navigation/navigation.i
+)
 # Wrap
-matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"
+matlab_wrap("${interface_files}" "gtsam" "${GTSAM_ADDITIONAL_LIBRARIES}"
            "" "${mexFlags}" "${ignore}")

 # Wrap version for gtsam_unstable
--- a/matlab/gtsam_tests/testJacobianFactor.m
+++ b/matlab/gtsam_tests/testJacobianFactor.m
@ -32,7 +32,7 @@ x1 = 3;
 % the RHS
 b2=[-1;1.5;2;-1];
 sigmas = [1;1;1;1];
-model4 = noiseModel.Diagonal.Sigmas(sigmas);
+model4 = noiseModel.Diagonal.Sigmas(sigmas, true);
 combined = JacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);

 % eliminate the first variable (x2) in the combined factor, destructive !
@ -74,7 +74,7 @@ Bx1 = [
 % the RHS
 b1= [0.0;0.894427];

-model2 = noiseModel.Diagonal.Sigmas([1;1]);
+model2 = noiseModel.Diagonal.Sigmas([1;1], true);
 expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2);

 % check if the result matches the combined (reduced) factor
--- a/matlab/gtsam_tests/testKalmanFilter.m
+++ b/matlab/gtsam_tests/testKalmanFilter.m
@ -23,13 +23,13 @@ import gtsam.*
 F = eye(2,2);
 B = eye(2,2);
 u = [1.0; 0.0];
-modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1]);
+modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
 Q = 0.01*eye(2,2);
 H = eye(2,2);
 z1 = [1.0, 0.0]';
 z2 = [2.0, 0.0]';
 z3 = [3.0, 0.0]';
-modelR = noiseModel.Diagonal.Sigmas([0.1;0.1]);
+modelR = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
 R = 0.01*eye(2,2);

 %% Create the set of expected output TestValues
--- a/matlab/gtsam_tests/testLocalizationExample.m
+++ b/matlab/gtsam_tests/testLocalizationExample.m
@ -17,7 +17,7 @@ graph = NonlinearFactorGraph;

 %% Add two odometry factors
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));

@ -27,7 +27,7 @@ groundTruth = Values;
 groundTruth.insert(1, Pose2(0.0, 0.0, 0.0));
 groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
 groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
-model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
+model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10], true);
 for i=1:3
    graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
 end
--- a/matlab/gtsam_tests/testOdometryExample.m
+++ b/matlab/gtsam_tests/testOdometryExample.m
@ -17,12 +17,12 @@ graph = NonlinearFactorGraph;

 %% Add a Gaussian prior on pose x_1
 priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); % 30cm std on x,y, 0.1 rad on theta
 graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph

 %% Add two odometry factors
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));

--- a/matlab/gtsam_tests/testPlanarSLAMExample.m
+++ b/matlab/gtsam_tests/testPlanarSLAMExample.m
@ -30,18 +30,18 @@ graph = NonlinearFactorGraph;

 %% Add prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph

 %% Add odometry
 odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));

 %% Add bearing/range measurement factors
 degrees = pi/180;
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
--- a/matlab/gtsam_tests/testPose2SLAMExample.m
+++ b/matlab/gtsam_tests/testPose2SLAMExample.m
@ -26,19 +26,19 @@ graph = NonlinearFactorGraph;
 %% Add prior
 % gaussian for prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph

 %% Add odometry
 % general noisemodel for odometry
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
 graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise));
 graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));

 %% Add pose constraint
-model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));

 %% Initialize to noisy points
--- a/matlab/gtsam_tests/testPose3SLAMExample.m
+++ b/matlab/gtsam_tests/testPose3SLAMExample.m
@ -21,7 +21,7 @@ p1 = hexagon.atPose3(1);
 fg = NonlinearFactorGraph;
 fg.add(NonlinearEqualityPose3(0, p0));
 delta = p0.between(p1);
-covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180], true);
 fg.add(BetweenFactorPose3(0,1, delta, covariance));
 fg.add(BetweenFactorPose3(1,2, delta, covariance));
 fg.add(BetweenFactorPose3(2,3, delta, covariance));
--- a/matlab/gtsam_tests/testSFMExample.m
+++ b/matlab/gtsam_tests/testSFMExample.m
@ -25,7 +25,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
 graph = NonlinearFactorGraph;

 %% Add factors for all measurements
-measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma, true);
 for i=1:length(data.Z)
    for k=1:length(data.Z{i})
        j = data.J{i}{k};
@ -33,9 +33,9 @@ for i=1:length(data.Z)
    end
 end

-posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas, true);
 graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
-pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma, true);
 graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));

 %% Initial estimate
--- a/matlab/gtsam_tests/testSerialization.m
+++ b/matlab/gtsam_tests/testSerialization.m
@ -45,30 +45,30 @@ graph = NonlinearFactorGraph;

 % Prior factor
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph

 % Between Factors
 odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 % Range Factors
-rNoise = noiseModel.Diagonal.Sigmas([0.2]);
+rNoise = noiseModel.Diagonal.Sigmas([0.2], true);
 graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise));
 graph.add(RangeFactor2D(i2, j1, 2, rNoise));
 graph.add(RangeFactor2D(i3, j2, 2, rNoise));

 % Bearing Factors
 degrees = pi/180;
-bNoise = noiseModel.Diagonal.Sigmas([0.1]);
+bNoise = noiseModel.Diagonal.Sigmas([0.1], true);
 graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
 graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
 graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));

 % BearingRange Factors
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
--- a/matlab/gtsam_tests/testStereoVOExample.m
+++ b/matlab/gtsam_tests/testStereoVOExample.m
@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose));
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
 K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0], true);

 %% Add measurements
 % pose 1
--- a/wrap/cmake/MatlabWrap.cmake
+++ b/wrap/cmake/MatlabWrap.cmake
@ -62,10 +62,10 @@ macro(find_and_configure_matlab)
 endmacro()

 # Consistent and user-friendly wrap function
-function(matlab_wrap interfaceHeader linkLibraries
+function(matlab_wrap interfaceHeader moduleName linkLibraries
         extraIncludeDirs extraMexFlags ignore_classes)
  find_and_configure_matlab()
-  wrap_and_install_library("${interfaceHeader}" "${linkLibraries}"
+  wrap_and_install_library("${interfaceHeader}" "${moduleName}" "${linkLibraries}"
                           "${extraIncludeDirs}" "${extraMexFlags}"
                           "${ignore_classes}")
 endfunction()
@ -77,6 +77,7 @@ endfunction()
 # Arguments:
 #
 # interfaceHeader:  The relative path to the wrapper interface definition file.
+# moduleName:       The name of the wrapped module, e.g. gtsam
 # linkLibraries:    Any *additional* libraries to link.  Your project library
 # (e.g. `lba`), libraries it depends on, and any necessary MATLAB libraries will
 # be linked automatically.  So normally, leave this empty.
@ -85,15 +86,15 @@ endfunction()
 # extraMexFlags:    Any *additional* flags to pass to the compiler when building
 # the wrap code.  Normally, leave this empty.
 # ignore_classes:  List of classes to ignore in the wrapping.
-function(wrap_and_install_library interfaceHeader linkLibraries
+function(wrap_and_install_library interfaceHeader moduleName linkLibraries
         extraIncludeDirs extraMexFlags ignore_classes)
-  wrap_library_internal("${interfaceHeader}" "${linkLibraries}"
+  wrap_library_internal("${interfaceHeader}" "${moduleName}" "${linkLibraries}"
                        "${extraIncludeDirs}" "${mexFlags}")
-  install_wrapped_library_internal("${interfaceHeader}")
+  install_wrapped_library_internal("${moduleName}")
 endfunction()

 # Internal function that wraps a library and compiles the wrapper
-function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
+function(wrap_library_internal interfaceHeader moduleName linkLibraries extraIncludeDirs
         extraMexFlags)
  if(UNIX AND NOT APPLE)
    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@ -120,7 +121,6 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
  # Extract module name from interface header file name
  get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
  get_filename_component(modulePath "${interfaceHeader}" PATH)
-  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)

  # Paths for generated files
  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
@ -136,8 +136,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
  # explicit link libraries list so that the next block of code can unpack any
  # static libraries
  set(automaticDependencies "")
-  foreach(lib ${moduleName} ${linkLibraries})
-    # message("MODULE NAME: ${moduleName}")
+  foreach(lib ${module} ${linkLibraries})
    if(TARGET "${lib}")
      get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
      # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
@ -176,7 +175,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
  set(otherLibraryTargets "")
  set(otherLibraryNontargets "")
  set(otherSourcesAndObjects "")
-  foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
+  foreach(lib ${module} ${linkLibraries} ${automaticDependencies})
    if(TARGET "${lib}")
      if(WRAP_MEX_BUILD_STATIC_MODULE)
        get_target_property(target_sources ${lib} SOURCES)
@ -250,7 +249,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
    COMMAND
      ${CMAKE_COMMAND} -E env
      "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader}
+      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src "${interfaceHeader}"
      --module_name ${moduleName} --out ${generated_files_path}
      --top_module_namespaces ${moduleName} --ignore ${ignore_classes}
    VERBATIM
@ -324,8 +323,8 @@ endfunction()

 # Internal function that installs a wrap toolbox
 function(install_wrapped_library_internal interfaceHeader)
-  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
+  get_filename_component(module "${interfaceHeader}" NAME_WE)
+  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${module}")

  # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash
  # on the directory name here prevents creating the top-level module name
--- a/wrap/gtwrap/interface_parser/type.py
+++ b/wrap/gtwrap/interface_parser/type.py
@ -53,6 +53,10 @@ class Typename:
        self.name = t[-1]  # the name is the last element in this list
        self.namespaces = t[:-1]

+        # If the first namespace is empty string, just get rid of it.
+        if self.namespaces and self.namespaces[0] == '':
+            self.namespaces.pop(0)
+
        if instantiations:
            if isinstance(instantiations, Sequence):
                self.instantiations = instantiations  # type: ignore
@ -92,8 +96,8 @@ class Typename:
        else:
            cpp_name = self.name
        return '{}{}{}'.format(
-            "::".join(self.namespaces[idx:]),
-            "::" if self.namespaces[idx:] else "",
+            "::".join(self.namespaces),
+            "::" if self.namespaces else "",
            cpp_name,
        )

--- a/wrap/gtwrap/matlab_wrapper/mixins.py
+++ b/wrap/gtwrap/matlab_wrapper/mixins.py
@ -108,7 +108,7 @@ class FormatMixin:
        elif is_method:
            formatted_type_name += self.data_type_param.get(name) or name
        else:
-            formatted_type_name += name
+            formatted_type_name += str(name)

        if separator == "::":  # C++
            templates = []
@ -192,10 +192,9 @@ class FormatMixin:
        method = ''

        if isinstance(static_method, parser.StaticMethod):
-            method += "".join([separator + x for x in static_method.parent.namespaces()]) + \
-                      separator + static_method.parent.name + separator
+            method += static_method.parent.to_cpp() + separator

-        return method[2 * len(separator):]
+        return method

    def _format_global_function(self,
                                function: Union[parser.GlobalFunction, Any],
--- a/wrap/gtwrap/matlab_wrapper/wrapper.py
+++ b/wrap/gtwrap/matlab_wrapper/wrapper.py
@ -239,18 +239,18 @@ class MatlabWrapper(CheckMixin, FormatMixin):

        return var_list_wrap

-    def _wrap_method_check_statement(self, args):
+    def _wrap_method_check_statement(self, args: parser.ArgumentList):
        """
        Wrap the given arguments into either just a varargout call or a
        call in an if statement that checks if the parameters are accurate.
+
+        TODO Update this method so that default arguments are supported.
        """
-        check_statement = ''
        arg_id = 1

-        if check_statement == '':
-            check_statement = \
-                'if length(varargin) == {param_count}'.format(
-                    param_count=len(args.list()))
+        param_count = len(args)
+        check_statement = 'if length(varargin) == {param_count}'.format(
+            param_count=param_count)

        for _, arg in enumerate(args.list()):
            name = arg.ctype.typename.name
@ -809,7 +809,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):

        for static_method in static_methods:
            format_name = list(static_method[0].name)
-            format_name[0] = format_name[0].upper()
+            format_name[0] = format_name[0]

            if static_method[0].name in self.ignore_methods:
                continue
@ -850,12 +850,13 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                    wrapper=self._wrapper_name(),
                    id=self._update_wrapper_id(
                        (namespace_name, instantiated_class,
-                         static_overload.name, static_overload)),
+                        static_overload.name, static_overload)),
                    class_name=instantiated_class.name,
                    end_statement=end_statement),
-                                               prefix='    ')
+                                            prefix='    ')

-            #TODO Figure out what is static_overload doing here.
+            # If the arguments don't match any of the checks above,
+            # throw an error with the class and method name.
            method_text += textwrap.indent(textwrap.dedent("""\
                    error('Arguments do not match any overload of function {class_name}.{method_name}');
                """.format(class_name=class_name,
@ -1081,7 +1082,6 @@ class MatlabWrapper(CheckMixin, FormatMixin):
        obj_start = ''

        if isinstance(method, instantiator.InstantiatedMethod):
-            # method_name = method.original.name
            method_name = method.to_cpp()
            obj_start = 'obj->'

@ -1090,6 +1090,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                #     self._format_type_name(method.instantiations))
                method = method.to_cpp()

+        elif isinstance(method, instantiator.InstantiatedStaticMethod):
+            method_name = self._format_static_method(method, '::')
+            method_name += method.original.name
+
        elif isinstance(method, parser.GlobalFunction):
            method_name = self._format_global_function(method, '::')
            method_name += method.name
@ -1250,7 +1254,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                method_name = ''

                if is_static_method:
-                    method_name = self._format_static_method(extra) + '.'
+                    method_name = self._format_static_method(extra, '.')

                method_name += extra.name

@ -1567,23 +1571,23 @@ class MatlabWrapper(CheckMixin, FormatMixin):

    def wrap(self, files, path):
        """High level function to wrap the project."""
+        content = ""
        modules = {}
        for file in files:
            with open(file, 'r') as f:
-                content = f.read()
+                content += f.read()

-            # Parse the contents of the interface file
-            parsed_result = parser.Module.parseString(content)
-            # print(parsed_result)
+        # Parse the contents of the interface file
+        parsed_result = parser.Module.parseString(content)

-            # Instantiate the module
-            module = instantiator.instantiate_namespace(parsed_result)
+        # Instantiate the module
+        module = instantiator.instantiate_namespace(parsed_result)

-            if module.name in modules:
-                modules[module.
-                        name].content[0].content += module.content[0].content
-            else:
-                modules[module.name] = module
+        if module.name in modules:
+            modules[
+                module.name].content[0].content += module.content[0].content
+        else:
+            modules[module.name] = module

        for module in modules.values():
            # Wrap the full namespace
--- a/wrap/gtwrap/template_instantiator/helpers.py
+++ b/wrap/gtwrap/template_instantiator/helpers.py
@ -55,16 +55,14 @@ def instantiate_type(
    # make a deep copy so that there is no overwriting of original template params
    ctype = deepcopy(ctype)

-    # Check if the return type has template parameters
+    # Check if the return type has template parameters as the typename's name
    if ctype.typename.instantiations:
        for idx, instantiation in enumerate(ctype.typename.instantiations):
            if instantiation.name in template_typenames:
                template_idx = template_typenames.index(instantiation.name)
-                ctype.typename.instantiations[
-                    idx] = instantiations[  # type: ignore
-                        template_idx]
+                ctype.typename.instantiations[idx].name =\
+                    instantiations[template_idx]

-        return ctype

    str_arg_typename = str(ctype.typename)

@ -125,9 +123,18 @@ def instantiate_type(

    # Case when 'This' is present in the type namespace, e.g `This::Subclass`.
    elif 'This' in str_arg_typename:
-        # Simply get the index of `This` in the namespace and replace it with the instantiated name.
-        namespace_idx = ctype.typename.namespaces.index('This')
-        ctype.typename.namespaces[namespace_idx] = cpp_typename.name
+        # Check if `This` is in the namespaces
+        if 'This' in ctype.typename.namespaces:
+            # Simply get the index of `This` in the namespace and
+            # replace it with the instantiated name.
+            namespace_idx = ctype.typename.namespaces.index('This')
+            ctype.typename.namespaces[namespace_idx] = cpp_typename.name
+        # Else check if it is in the template namespace, e.g vector<This::Value>
+        else:
+            for idx, instantiation in enumerate(ctype.typename.instantiations):
+                if 'This' in instantiation.namespaces:
+                    ctype.typename.instantiations[idx].namespaces = \
+                        cpp_typename.namespaces + [cpp_typename.name]
        return ctype

    else:
--- a/wrap/tests/expected/matlab/+gtsam/GeneralSFMFactorCal3Bundler.m
+++ b/wrap/tests/expected/matlab/+gtsam/GeneralSFMFactorCal3Bundler.m
@ -0,0 +1,31 @@
+%class GeneralSFMFactorCal3Bundler, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef GeneralSFMFactorCal3Bundler < handle
+  properties
+    ptr_gtsamGeneralSFMFactorCal3Bundler = 0
+  end
+  methods
+    function obj = GeneralSFMFactorCal3Bundler(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        special_cases_wrapper(7, my_ptr);
+      else
+        error('Arguments do not match any overload of gtsam.GeneralSFMFactorCal3Bundler constructor');
+      end
+      obj.ptr_gtsamGeneralSFMFactorCal3Bundler = my_ptr;
+    end
+
+    function delete(obj)
+      special_cases_wrapper(8, obj.ptr_gtsamGeneralSFMFactorCal3Bundler);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/+gtsam/Point3.m
+++ b/wrap/tests/expected/matlab/+gtsam/Point3.m
@ -78,7 +78,7 @@ classdef Point3 < handle
      error('Arguments do not match any overload of function Point3.StaticFunctionRet');
    end

-    function varargout = StaticFunction(varargin)
+    function varargout = staticFunction(varargin)
      % STATICFUNCTION usage: staticFunction() : returns double
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
--- a/wrap/tests/expected/matlab/+gtsam/SfmTrack.m
+++ b/wrap/tests/expected/matlab/+gtsam/SfmTrack.m
@ -0,0 +1,31 @@
+%class SfmTrack, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef SfmTrack < handle
+  properties
+    ptr_gtsamSfmTrack = 0
+  end
+  methods
+    function obj = SfmTrack(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        special_cases_wrapper(3, my_ptr);
+      else
+        error('Arguments do not match any overload of gtsam.SfmTrack constructor');
+      end
+      obj.ptr_gtsamSfmTrack = my_ptr;
+    end
+
+    function delete(obj)
+      special_cases_wrapper(4, obj.ptr_gtsamSfmTrack);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/+gtsam/Values.m
+++ b/wrap/tests/expected/matlab/+gtsam/Values.m
@ -0,0 +1,59 @@
+%class Values, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%Values()
+%Values(Values other)
+%
+%-------Methods-------
+%insert(size_t j, Vector vector) : returns void
+%insert(size_t j, Matrix matrix) : returns void
+%
+classdef Values < handle
+  properties
+    ptr_gtsamValues = 0
+  end
+  methods
+    function obj = Values(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        namespaces_wrapper(26, my_ptr);
+      elseif nargin == 0
+        my_ptr = namespaces_wrapper(27);
+      elseif nargin == 1 && isa(varargin{1},'gtsam.Values')
+        my_ptr = namespaces_wrapper(28, varargin{1});
+      else
+        error('Arguments do not match any overload of gtsam.Values constructor');
+      end
+      obj.ptr_gtsamValues = my_ptr;
+    end
+
+    function delete(obj)
+      namespaces_wrapper(29, obj.ptr_gtsamValues);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = insert(this, varargin)
+      % INSERT usage: insert(size_t j, Vector vector) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') && size(varargin{2},2)==1
+        namespaces_wrapper(30, this, varargin{:});
+        return
+      end
+      % INSERT usage: insert(size_t j, Matrix matrix) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
+        namespaces_wrapper(31, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Values.insert');
+    end
+
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/+ns2/ClassA.m
+++ b/wrap/tests/expected/matlab/+ns2/ClassA.m
@ -74,7 +74,7 @@ classdef ClassA < handle
  end

  methods(Static = true)
-    function varargout = Afunction(varargin)
+    function varargout = afunction(varargin)
      % AFUNCTION usage: afunction() : returns double
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
--- a/wrap/tests/expected/matlab/DefaultFuncInt.m
+++ b/wrap/tests/expected/matlab/DefaultFuncInt.m
@ -0,0 +1,6 @@
+function varargout = DefaultFuncInt(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric')
+        functions_wrapper(8, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncInt');
+      end
--- a/wrap/tests/expected/matlab/DefaultFuncObj.m
+++ b/wrap/tests/expected/matlab/DefaultFuncObj.m
@ -0,0 +1,6 @@
+function varargout = DefaultFuncObj(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.KeyFormatter')
+        functions_wrapper(10, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncObj');
+      end
--- a/wrap/tests/expected/matlab/DefaultFuncString.m
+++ b/wrap/tests/expected/matlab/DefaultFuncString.m
@ -0,0 +1,6 @@
+function varargout = DefaultFuncString(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'char')
+        functions_wrapper(9, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncString');
+      end
--- a/wrap/tests/expected/matlab/DefaultFuncVector.m
+++ b/wrap/tests/expected/matlab/DefaultFuncVector.m
@ -0,0 +1,6 @@
+function varargout = DefaultFuncVector(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'std.vectornumeric') && isa(varargin{2},'std.vectorchar')
+        functions_wrapper(12, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncVector');
+      end
--- a/wrap/tests/expected/matlab/DefaultFuncZero.m
+++ b/wrap/tests/expected/matlab/DefaultFuncZero.m
@ -0,0 +1,6 @@
+function varargout = DefaultFuncZero(varargin)
+      if length(varargin) == 5 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'logical') && isa(varargin{5},'logical')
+        functions_wrapper(11, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncZero');
+      end
--- a/wrap/tests/expected/matlab/ForwardKinematics.m
+++ b/wrap/tests/expected/matlab/ForwardKinematics.m
@ -0,0 +1,36 @@
+%class ForwardKinematics, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%ForwardKinematics(Robot robot, string start_link_name, string end_link_name, Values joint_angles, Pose3 l2Tp)
+%
+classdef ForwardKinematics < handle
+  properties
+    ptr_ForwardKinematics = 0
+  end
+  methods
+    function obj = ForwardKinematics(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        class_wrapper(55, my_ptr);
+      elseif nargin == 5 && isa(varargin{1},'gtdynamics.Robot') && isa(varargin{2},'char') && isa(varargin{3},'char') && isa(varargin{4},'gtsam.Values') && isa(varargin{5},'gtsam.Pose3')
+        my_ptr = class_wrapper(56, varargin{1}, varargin{2}, varargin{3}, varargin{4}, varargin{5});
+      else
+        error('Arguments do not match any overload of ForwardKinematics constructor');
+      end
+      obj.ptr_ForwardKinematics = my_ptr;
+    end
+
+    function delete(obj)
+      class_wrapper(57, obj.ptr_ForwardKinematics);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/ForwardKinematicsFactor.m
+++ b/wrap/tests/expected/matlab/ForwardKinematicsFactor.m
@ -0,0 +1,36 @@
+%class ForwardKinematicsFactor, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
+  properties
+    ptr_ForwardKinematicsFactor = 0
+  end
+  methods
+    function obj = ForwardKinematicsFactor(varargin)
+      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        if nargin == 2
+          my_ptr = varargin{2};
+        else
+          my_ptr = inheritance_wrapper(36, varargin{2});
+        end
+        base_ptr = inheritance_wrapper(35, my_ptr);
+      else
+        error('Arguments do not match any overload of ForwardKinematicsFactor constructor');
+      end
+      obj = obj@gtsam.BetweenFactorPose3(uint64(5139824614673773682), base_ptr);
+      obj.ptr_ForwardKinematicsFactor = my_ptr;
+    end
+
+    function delete(obj)
+      inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/FunDouble.m
+++ b/wrap/tests/expected/matlab/FunDouble.m
@ -3,6 +3,7 @@
 %
 %-------Methods-------
 %multiTemplatedMethodStringSize_t(double d, string t, size_t u) : returns Fun<double>
+%sets() : returns std::map<double,Fun<double>::double>
 %templatedMethodString(double d, string t) : returns Fun<double>
 %
 %-------Static Methods-------
@ -46,11 +47,21 @@ classdef FunDouble < handle
      error('Arguments do not match any overload of function FunDouble.multiTemplatedMethodStringSize_t');
    end

+    function varargout = sets(this, varargin)
+      % SETS usage: sets() : returns std.mapdoubledouble
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = class_wrapper(8, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function FunDouble.sets');
+    end
+
    function varargout = templatedMethodString(this, varargin)
      % TEMPLATEDMETHODSTRING usage: templatedMethodString(double d, string t) : returns Fun<double>
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'char')
-        varargout{1} = class_wrapper(8, this, varargin{:});
+        varargout{1} = class_wrapper(9, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function FunDouble.templatedMethodString');
@ -59,22 +70,22 @@ classdef FunDouble < handle
  end

  methods(Static = true)
-    function varargout = StaticMethodWithThis(varargin)
+    function varargout = staticMethodWithThis(varargin)
      % STATICMETHODWITHTHIS usage: staticMethodWithThis() : returns Fundouble
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        varargout{1} = class_wrapper(9, varargin{:});
+        varargout{1} = class_wrapper(10, varargin{:});
        return
      end

      error('Arguments do not match any overload of function FunDouble.staticMethodWithThis');
    end

-    function varargout = TemplatedStaticMethodInt(varargin)
+    function varargout = templatedStaticMethodInt(varargin)
      % TEMPLATEDSTATICMETHODINT usage: templatedStaticMethodInt(int m) : returns double
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = class_wrapper(10, varargin{:});
+        varargout{1} = class_wrapper(11, varargin{:});
        return
      end

--- a/wrap/tests/expected/matlab/FunRange.m
+++ b/wrap/tests/expected/matlab/FunRange.m
@ -52,7 +52,7 @@ classdef FunRange < handle
  end

  methods(Static = true)
-    function varargout = Create(varargin)
+    function varargout = create(varargin)
      % CREATE usage: create() : returns FunRange
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
--- a/wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m
+++ b/wrap/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(50, my_ptr);
+        class_wrapper(51, 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(51, obj.ptr_MultipleTemplatesIntDouble);
+      class_wrapper(52, obj.ptr_MultipleTemplatesIntDouble);
    end

    function display(obj), obj.print(''); end
--- a/wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m
+++ b/wrap/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(52, my_ptr);
+        class_wrapper(53, 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(53, obj.ptr_MultipleTemplatesIntFloat);
+      class_wrapper(54, obj.ptr_MultipleTemplatesIntFloat);
    end

    function display(obj), obj.print(''); end
--- a/wrap/tests/expected/matlab/MyFactorPosePoint2.m
+++ b/wrap/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(63, my_ptr);
+        class_wrapper(64, 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(64, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = class_wrapper(65, 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(65, obj.ptr_MyFactorPosePoint2);
+      class_wrapper(66, 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(66, this, varargin{:});
+        class_wrapper(67, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function MyFactorPosePoint2.print');
--- a/wrap/tests/expected/matlab/MyVector12.m
+++ b/wrap/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(47, my_ptr);
+        class_wrapper(48, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(48);
+        my_ptr = class_wrapper(49);
      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(49, obj.ptr_MyVector12);
+      class_wrapper(50, obj.ptr_MyVector12);
    end

    function display(obj), obj.print(''); end
--- a/wrap/tests/expected/matlab/MyVector3.m
+++ b/wrap/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(44, my_ptr);
+        class_wrapper(45, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(45);
+        my_ptr = class_wrapper(46);
      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(46, obj.ptr_MyVector3);
+      class_wrapper(47, obj.ptr_MyVector3);
    end

    function display(obj), obj.print(''); end
--- a/wrap/tests/expected/matlab/PrimitiveRefDouble.m
+++ b/wrap/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(40, my_ptr);
+        class_wrapper(41, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(41);
+        my_ptr = class_wrapper(42);
      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(42, obj.ptr_PrimitiveRefDouble);
+      class_wrapper(43, 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(43, varargin{:});
+        varargout{1} = class_wrapper(44, varargin{:});
        return
      end

--- a/wrap/tests/expected/matlab/ScopedTemplateResult.m
+++ b/wrap/tests/expected/matlab/ScopedTemplateResult.m
@ -0,0 +1,36 @@
+%class ScopedTemplateResult, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%ScopedTemplateResult(Result::Value arg)
+%
+classdef ScopedTemplateResult < handle
+  properties
+    ptr_ScopedTemplateResult = 0
+  end
+  methods
+    function obj = ScopedTemplateResult(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        template_wrapper(6, my_ptr);
+      elseif nargin == 1 && isa(varargin{1},'Result::Value')
+        my_ptr = template_wrapper(7, varargin{1});
+      else
+        error('Arguments do not match any overload of ScopedTemplateResult constructor');
+      end
+      obj.ptr_ScopedTemplateResult = my_ptr;
+    end
+
+    function delete(obj)
+      template_wrapper(8, obj.ptr_ScopedTemplateResult);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/TemplatedConstructor.m
+++ b/wrap/tests/expected/matlab/TemplatedConstructor.m
@ -0,0 +1,45 @@
+%class TemplatedConstructor, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%TemplatedConstructor()
+%TemplatedConstructor(string arg)
+%TemplatedConstructor(int arg)
+%TemplatedConstructor(double arg)
+%
+classdef TemplatedConstructor < handle
+  properties
+    ptr_TemplatedConstructor = 0
+  end
+  methods
+    function obj = TemplatedConstructor(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        template_wrapper(0, my_ptr);
+      elseif nargin == 0
+        my_ptr = template_wrapper(1);
+      elseif nargin == 1 && isa(varargin{1},'char')
+        my_ptr = template_wrapper(2, varargin{1});
+      elseif nargin == 1 && isa(varargin{1},'numeric')
+        my_ptr = template_wrapper(3, varargin{1});
+      elseif nargin == 1 && isa(varargin{1},'double')
+        my_ptr = template_wrapper(4, varargin{1});
+      else
+        error('Arguments do not match any overload of TemplatedConstructor constructor');
+      end
+      obj.ptr_TemplatedConstructor = my_ptr;
+    end
+
+    function delete(obj)
+      template_wrapper(5, obj.ptr_TemplatedConstructor);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end
--- a/wrap/tests/expected/matlab/Test.m
+++ b/wrap/tests/expected/matlab/Test.m
@ -40,11 +40,11 @@ classdef Test < handle
    function obj = Test(varargin)
      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
        my_ptr = varargin{2};
-        class_wrapper(11, my_ptr);
+        class_wrapper(12, my_ptr);
      elseif nargin == 0
-        my_ptr = class_wrapper(12);
+        my_ptr = class_wrapper(13);
      elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        my_ptr = class_wrapper(13, varargin{1}, varargin{2});
+        my_ptr = class_wrapper(14, varargin{1}, varargin{2});
      else
        error('Arguments do not match any overload of Test constructor');
      end
@ -52,7 +52,7 @@ classdef Test < handle
    end

    function delete(obj)
-      class_wrapper(14, obj.ptr_Test);
+      class_wrapper(15, obj.ptr_Test);
    end

    function display(obj), obj.print(''); end
@ -63,7 +63,7 @@ classdef Test < handle
      % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'double')
-        class_wrapper(15, this, varargin{:});
+        class_wrapper(16, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.arg_EigenConstRef');
@ -73,7 +73,7 @@ classdef Test < handle
      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test >
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.create_MixedPtrs');
@ -83,7 +83,7 @@ classdef Test < handle
      % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test >
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(18, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.create_ptrs');
@ -93,7 +93,7 @@ classdef Test < handle
      % GET_CONTAINER usage: get_container() : returns std.vectorTest
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        varargout{1} = class_wrapper(18, this, varargin{:});
+        varargout{1} = class_wrapper(19, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.get_container');
@ -103,7 +103,7 @@ classdef Test < handle
      % LAMBDA usage: lambda() : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        class_wrapper(19, this, varargin{:});
+        class_wrapper(20, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.lambda');
@ -113,7 +113,7 @@ classdef Test < handle
      % PRINT usage: print() : returns void
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 0
-        class_wrapper(20, this, varargin{:});
+        class_wrapper(21, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.print');
@ -123,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(21, this, varargin{:});
+        varargout{1} = class_wrapper(22, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_Point2Ptr');
@ -133,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(22, this, varargin{:});
+        varargout{1} = class_wrapper(23, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_Test');
@ -143,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(23, this, varargin{:});
+        varargout{1} = class_wrapper(24, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_TestPtr');
@ -153,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(24, this, varargin{:});
+        varargout{1} = class_wrapper(25, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_bool');
@ -163,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(25, this, varargin{:});
+        varargout{1} = class_wrapper(26, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_double');
@ -173,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(26, this, varargin{:});
+        varargout{1} = class_wrapper(27, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_field');
@ -183,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(27, this, varargin{:});
+        varargout{1} = class_wrapper(28, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_int');
@ -193,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(28, this, varargin{:});
+        varargout{1} = class_wrapper(29, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_matrix1');
@ -203,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(29, this, varargin{:});
+        varargout{1} = class_wrapper(30, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_matrix2');
@ -213,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(30, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(31, 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(31, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_pair');
@ -229,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(32, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(33, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_ptrs');
@ -239,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(33, this, varargin{:});
+        varargout{1} = class_wrapper(34, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_size_t');
@ -249,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(34, this, varargin{:});
+        varargout{1} = class_wrapper(35, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_string');
@ -259,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(35, this, varargin{:});
+        varargout{1} = class_wrapper(36, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function Test.return_vector1');
@ -269,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(36, this, varargin{:});
+        varargout{1} = class_wrapper(37, 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(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')
@ -294,6 +288,12 @@ classdef Test < handle
        class_wrapper(39, 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(40, this, varargin{:});
+        return
+      end
      error('Arguments do not match any overload of function Test.set_container');
    end

--- a/wrap/tests/expected/matlab/class_wrapper.cpp
+++ b/wrap/tests/expected/matlab/class_wrapper.cpp
@ -231,7 +231,14 @@ void FunDouble_multiTemplatedMethod_7(int nargout, mxArray *out[], int nargin, c
  out[0] = wrap_shared_ptr(boost::make_shared<Fun<double>>(obj->multiTemplatedMethod<string,size_t>(d,t,u)),"Fun<double>", false);
 }

-void FunDouble_templatedMethod_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void FunDouble_sets_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("sets",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<Fun<double>>(in[0], "ptr_FunDouble");
+  out[0] = wrap_shared_ptr(boost::make_shared<std::map<double,Fun<double>::double>>(obj->sets()),"std.mapdoubledouble", false);
+}
+
+void FunDouble_templatedMethod_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("templatedMethodString",nargout,nargin-1,2);
  auto obj = unwrap_shared_ptr<Fun<double>>(in[0], "ptr_FunDouble");
@ -240,20 +247,20 @@ void FunDouble_templatedMethod_8(int nargout, mxArray *out[], int nargin, const
  out[0] = wrap_shared_ptr(boost::make_shared<Fun<double>>(obj->templatedMethod<string>(d,t)),"Fun<double>", false);
 }

-void FunDouble_staticMethodWithThis_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void FunDouble_staticMethodWithThis_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("FunDouble.staticMethodWithThis",nargout,nargin,0);
+  checkArguments("Fun<double>.staticMethodWithThis",nargout,nargin,0);
  out[0] = wrap_shared_ptr(boost::make_shared<Fun<double>>(Fun<double>::staticMethodWithThis()),"Fundouble", false);
 }

-void FunDouble_templatedStaticMethodInt_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void FunDouble_templatedStaticMethodInt_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("FunDouble.templatedStaticMethodInt",nargout,nargin,1);
+  checkArguments("Fun<double>.templatedStaticMethodInt",nargout,nargin,1);
  int m = unwrap< int >(in[0]);
-  out[0] = wrap< double >(Fun<double>::templatedStaticMethodInt(m));
+  out[0] = wrap< double >(Fun<double>::templatedStaticMethod(m));
 }

-void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_collectorInsertAndMakeBase_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<Test> Shared;
@ -262,7 +269,7 @@ void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin,
  collector_Test.insert(self);
 }

-void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<Test> Shared;
@ -273,7 +280,7 @@ void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_constructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<Test> Shared;
@ -286,7 +293,7 @@ void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_deconstructor_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<Test> Shared;
  checkArguments("delete_Test",nargout,nargin,1);
@ -299,7 +306,7 @@ void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArra
  }
 }

-void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_arg_EigenConstRef_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -307,7 +314,7 @@ void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mx
  obj->arg_EigenConstRef(value);
 }

-void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_create_MixedPtrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("create_MixedPtrs",nargout,nargin-1,0);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -316,7 +323,7 @@ void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxA
  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }

-void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_create_ptrs_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("create_ptrs",nargout,nargin-1,0);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@ -325,28 +332,28 @@ void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }

-void Test_get_container_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_get_container_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("get_container",nargout,nargin-1,0);
  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
  out[0] = wrap_shared_ptr(boost::make_shared<std::vector<testing::Test>>(obj->get_container()),"std.vectorTest", false);
 }

-void Test_lambda_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_lambda_20(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_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_print_21(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_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Point2Ptr_22(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");
@ -357,7 +364,7 @@ void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxA
  }
 }

-void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Test_23(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");
@ -365,7 +372,7 @@ void Test_return_Test_22(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_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_TestPtr_24(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");
@ -373,7 +380,7 @@ void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
 }

-void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_bool_25(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");
@ -381,7 +388,7 @@ void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray
  out[0] = wrap< bool >(obj->return_bool(value));
 }

-void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_double_26(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");
@ -389,7 +396,7 @@ void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap< double >(obj->return_double(value));
 }

-void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_field_27(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");
@ -397,7 +404,7 @@ void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray
  out[0] = wrap< bool >(obj->return_field(t));
 }

-void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_int_28(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");
@ -405,7 +412,7 @@ void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *
  out[0] = wrap< int >(obj->return_int(value));
 }

-void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix1_29(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");
@ -413,7 +420,7 @@ void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Matrix >(obj->return_matrix1(value));
 }

-void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix2_30(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");
@ -421,7 +428,7 @@ void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Matrix >(obj->return_matrix2(value));
 }

-void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_31(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");
@ -432,7 +439,7 @@ void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap< Matrix >(pairResult.second);
 }

-void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_32(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");
@ -442,7 +449,7 @@ void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap< Matrix >(pairResult.second);
 }

-void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_ptrs_33(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");
@ -453,7 +460,7 @@ void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray
  out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }

-void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_size_t_34(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");
@ -461,7 +468,7 @@ void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap< size_t >(obj->return_size_t(value));
 }

-void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_string_35(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");
@ -469,7 +476,7 @@ void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArra
  out[0] = wrap< string >(obj->return_string(value));
 }

-void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector1_36(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");
@ -477,7 +484,7 @@ void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Vector >(obj->return_vector1(value));
 }

-void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_37(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");
@ -485,14 +492,6 @@ void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArr
  out[0] = wrap< Vector >(obj->return_vector2(value));
 }

-void Test_set_container_37(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_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  checkArguments("set_container",nargout,nargin-1,1);
@ -509,7 +508,15 @@ void Test_set_container_39(int nargout, mxArray *out[], int nargin, const mxArra
  obj->set_container(*container);
 }

-void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_set_container_40(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_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@ -518,7 +525,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[
  collector_PrimitiveRefDouble.insert(self);
 }

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

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

-void PrimitiveRefDouble_Brutal_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_Brutal_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1);
+  checkArguments("PrimitiveRef<double>.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_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyVector<3>> Shared;
@ -558,7 +565,7 @@ void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int na
  collector_MyVector3.insert(self);
 }

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

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

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

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

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

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

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

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

-void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
  checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1);
@ -659,7 +666,7 @@ void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int
  }
 }

-void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_collectorInsertAndMakeBase_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<ForwardKinematics> Shared;
@ -668,7 +675,7 @@ void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[]
  collector_ForwardKinematics.insert(self);
 }

-void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_constructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<ForwardKinematics> Shared;
@ -684,7 +691,7 @@ void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, c
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_deconstructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<ForwardKinematics> Shared;
  checkArguments("delete_ForwardKinematics",nargout,nargin,1);
@ -697,7 +704,7 @@ void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin,
  }
 }

-void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_collectorInsertAndMakeBase_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<TemplatedConstructor> Shared;
@ -706,7 +713,7 @@ void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *ou
  collector_TemplatedConstructor.insert(self);
 }

-void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<TemplatedConstructor> Shared;
@ -717,7 +724,7 @@ void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<TemplatedConstructor> Shared;
@ -729,7 +736,7 @@ void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<TemplatedConstructor> Shared;
@ -741,7 +748,7 @@ void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<TemplatedConstructor> Shared;
@ -753,7 +760,7 @@ void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin
  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }

-void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_deconstructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  typedef boost::shared_ptr<TemplatedConstructor> Shared;
  checkArguments("delete_TemplatedConstructor",nargout,nargin,1);
@ -766,7 +773,7 @@ void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int narg
  }
 }

-void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_collectorInsertAndMakeBase_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
  mexAtExit(&_deleteAllObjects);
  typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@ -775,7 +782,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[
  collector_MyFactorPosePoint2.insert(self);
 }

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

-void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_66(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);
@ -803,7 +810,7 @@ void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin
  }
 }

-void MyFactorPosePoint2_print_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_print_67(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");
@ -849,94 +856,94 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      FunDouble_multiTemplatedMethod_7(nargout, out, nargin-1, in+1);
      break;
    case 8:
-      FunDouble_templatedMethod_8(nargout, out, nargin-1, in+1);
+      FunDouble_sets_8(nargout, out, nargin-1, in+1);
      break;
    case 9:
-      FunDouble_staticMethodWithThis_9(nargout, out, nargin-1, in+1);
+      FunDouble_templatedMethod_9(nargout, out, nargin-1, in+1);
      break;
    case 10:
-      FunDouble_templatedStaticMethodInt_10(nargout, out, nargin-1, in+1);
+      FunDouble_staticMethodWithThis_10(nargout, out, nargin-1, in+1);
      break;
    case 11:
-      Test_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1);
+      FunDouble_templatedStaticMethodInt_11(nargout, out, nargin-1, in+1);
      break;
    case 12:
-      Test_constructor_12(nargout, out, nargin-1, in+1);
+      Test_collectorInsertAndMakeBase_12(nargout, out, nargin-1, in+1);
      break;
    case 13:
      Test_constructor_13(nargout, out, nargin-1, in+1);
      break;
    case 14:
-      Test_deconstructor_14(nargout, out, nargin-1, in+1);
+      Test_constructor_14(nargout, out, nargin-1, in+1);
      break;
    case 15:
-      Test_arg_EigenConstRef_15(nargout, out, nargin-1, in+1);
+      Test_deconstructor_15(nargout, out, nargin-1, in+1);
      break;
    case 16:
-      Test_create_MixedPtrs_16(nargout, out, nargin-1, in+1);
+      Test_arg_EigenConstRef_16(nargout, out, nargin-1, in+1);
      break;
    case 17:
-      Test_create_ptrs_17(nargout, out, nargin-1, in+1);
+      Test_create_MixedPtrs_17(nargout, out, nargin-1, in+1);
      break;
    case 18:
-      Test_get_container_18(nargout, out, nargin-1, in+1);
+      Test_create_ptrs_18(nargout, out, nargin-1, in+1);
      break;
    case 19:
-      Test_lambda_19(nargout, out, nargin-1, in+1);
+      Test_get_container_19(nargout, out, nargin-1, in+1);
      break;
    case 20:
-      Test_print_20(nargout, out, nargin-1, in+1);
+      Test_lambda_20(nargout, out, nargin-1, in+1);
      break;
    case 21:
-      Test_return_Point2Ptr_21(nargout, out, nargin-1, in+1);
+      Test_print_21(nargout, out, nargin-1, in+1);
      break;
    case 22:
-      Test_return_Test_22(nargout, out, nargin-1, in+1);
+      Test_return_Point2Ptr_22(nargout, out, nargin-1, in+1);
      break;
    case 23:
-      Test_return_TestPtr_23(nargout, out, nargin-1, in+1);
+      Test_return_Test_23(nargout, out, nargin-1, in+1);
      break;
    case 24:
-      Test_return_bool_24(nargout, out, nargin-1, in+1);
+      Test_return_TestPtr_24(nargout, out, nargin-1, in+1);
      break;
    case 25:
-      Test_return_double_25(nargout, out, nargin-1, in+1);
+      Test_return_bool_25(nargout, out, nargin-1, in+1);
      break;
    case 26:
-      Test_return_field_26(nargout, out, nargin-1, in+1);
+      Test_return_double_26(nargout, out, nargin-1, in+1);
      break;
    case 27:
-      Test_return_int_27(nargout, out, nargin-1, in+1);
+      Test_return_field_27(nargout, out, nargin-1, in+1);
      break;
    case 28:
-      Test_return_matrix1_28(nargout, out, nargin-1, in+1);
+      Test_return_int_28(nargout, out, nargin-1, in+1);
      break;
    case 29:
-      Test_return_matrix2_29(nargout, out, nargin-1, in+1);
+      Test_return_matrix1_29(nargout, out, nargin-1, in+1);
      break;
    case 30:
-      Test_return_pair_30(nargout, out, nargin-1, in+1);
+      Test_return_matrix2_30(nargout, out, nargin-1, in+1);
      break;
    case 31:
      Test_return_pair_31(nargout, out, nargin-1, in+1);
      break;
    case 32:
-      Test_return_ptrs_32(nargout, out, nargin-1, in+1);
+      Test_return_pair_32(nargout, out, nargin-1, in+1);
      break;
    case 33:
-      Test_return_size_t_33(nargout, out, nargin-1, in+1);
+      Test_return_ptrs_33(nargout, out, nargin-1, in+1);
      break;
    case 34:
-      Test_return_string_34(nargout, out, nargin-1, in+1);
+      Test_return_size_t_34(nargout, out, nargin-1, in+1);
      break;
    case 35:
-      Test_return_vector1_35(nargout, out, nargin-1, in+1);
+      Test_return_string_35(nargout, out, nargin-1, in+1);
      break;
    case 36:
-      Test_return_vector2_36(nargout, out, nargin-1, in+1);
+      Test_return_vector1_36(nargout, out, nargin-1, in+1);
      break;
    case 37:
-      Test_set_container_37(nargout, out, nargin-1, in+1);
+      Test_return_vector2_37(nargout, out, nargin-1, in+1);
      break;
    case 38:
      Test_set_container_38(nargout, out, nargin-1, in+1);
@ -945,61 +952,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      Test_set_container_39(nargout, out, nargin-1, in+1);
      break;
    case 40:
-      PrimitiveRefDouble_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1);
+      Test_set_container_40(nargout, out, nargin-1, in+1);
      break;
    case 41:
-      PrimitiveRefDouble_constructor_41(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_collectorInsertAndMakeBase_41(nargout, out, nargin-1, in+1);
      break;
    case 42:
-      PrimitiveRefDouble_deconstructor_42(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_constructor_42(nargout, out, nargin-1, in+1);
      break;
    case 43:
-      PrimitiveRefDouble_Brutal_43(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_deconstructor_43(nargout, out, nargin-1, in+1);
      break;
    case 44:
-      MyVector3_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_Brutal_44(nargout, out, nargin-1, in+1);
      break;
    case 45:
-      MyVector3_constructor_45(nargout, out, nargin-1, in+1);
+      MyVector3_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1);
      break;
    case 46:
-      MyVector3_deconstructor_46(nargout, out, nargin-1, in+1);
+      MyVector3_constructor_46(nargout, out, nargin-1, in+1);
      break;
    case 47:
-      MyVector12_collectorInsertAndMakeBase_47(nargout, out, nargin-1, in+1);
+      MyVector3_deconstructor_47(nargout, out, nargin-1, in+1);
      break;
    case 48:
-      MyVector12_constructor_48(nargout, out, nargin-1, in+1);
+      MyVector12_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1);
      break;
    case 49:
-      MyVector12_deconstructor_49(nargout, out, nargin-1, in+1);
+      MyVector12_constructor_49(nargout, out, nargin-1, in+1);
      break;
    case 50:
-      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1);
+      MyVector12_deconstructor_50(nargout, out, nargin-1, in+1);
      break;
    case 51:
-      MultipleTemplatesIntDouble_deconstructor_51(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
      break;
    case 52:
-      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_deconstructor_52(nargout, out, nargin-1, in+1);
      break;
    case 53:
-      MultipleTemplatesIntFloat_deconstructor_53(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1);
      break;
    case 54:
-      ForwardKinematics_collectorInsertAndMakeBase_54(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_deconstructor_54(nargout, out, nargin-1, in+1);
      break;
    case 55:
-      ForwardKinematics_constructor_55(nargout, out, nargin-1, in+1);
+      ForwardKinematics_collectorInsertAndMakeBase_55(nargout, out, nargin-1, in+1);
      break;
    case 56:
-      ForwardKinematics_deconstructor_56(nargout, out, nargin-1, in+1);
+      ForwardKinematics_constructor_56(nargout, out, nargin-1, in+1);
      break;
    case 57:
-      TemplatedConstructor_collectorInsertAndMakeBase_57(nargout, out, nargin-1, in+1);
+      ForwardKinematics_deconstructor_57(nargout, out, nargin-1, in+1);
      break;
    case 58:
-      TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_collectorInsertAndMakeBase_58(nargout, out, nargin-1, in+1);
      break;
    case 59:
      TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1);
@ -1011,19 +1018,22 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
      TemplatedConstructor_constructor_61(nargout, out, nargin-1, in+1);
      break;
    case 62:
-      TemplatedConstructor_deconstructor_62(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_constructor_62(nargout, out, nargin-1, in+1);
      break;
    case 63:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_63(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_deconstructor_63(nargout, out, nargin-1, in+1);
      break;
    case 64:
-      MyFactorPosePoint2_constructor_64(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_collectorInsertAndMakeBase_64(nargout, out, nargin-1, in+1);
      break;
    case 65:
-      MyFactorPosePoint2_deconstructor_65(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_constructor_65(nargout, out, nargin-1, in+1);
      break;
    case 66:
-      MyFactorPosePoint2_print_66(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_deconstructor_66(nargout, out, nargin-1, in+1);
+      break;
+    case 67:
+      MyFactorPosePoint2_print_67(nargout, out, nargin-1, in+1);
      break;
    }
  } catch(const std::exception& e) {
--- a/wrap/tests/expected/matlab/geometry_wrapper.cpp
+++ b/wrap/tests/expected/matlab/geometry_wrapper.cpp
@ -286,14 +286,14 @@ void gtsamPoint3_string_serialize_22(int nargout, mxArray *out[], int nargin, co
 }
 void gtsamPoint3_StaticFunctionRet_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1);
+  checkArguments("gtsam::Point3.StaticFunctionRet",nargout,nargin,1);
  double z = unwrap< double >(in[0]);
  out[0] = wrap< Point3 >(gtsam::Point3::StaticFunctionRet(z));
 }

 void gtsamPoint3_staticFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0);
+  checkArguments("gtsam::Point3.staticFunction",nargout,nargin,0);
  out[0] = wrap< double >(gtsam::Point3::staticFunction());
 }

--- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp
+++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp
@ -289,7 +289,7 @@ void MyTemplatePoint2_templatedMethod_17(int nargout, mxArray *out[], int nargin

 void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("MyTemplatePoint2.Level",nargout,nargin,1);
+  checkArguments("MyTemplate<gtsam::Point2>.Level",nargout,nargin,1);
  Point2 K = unwrap< Point2 >(in[0]);
  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Point2>>(MyTemplate<gtsam::Point2>::Level(K)),"MyTemplatePoint2", false);
 }
@ -457,7 +457,7 @@ void MyTemplateMatrix_templatedMethod_33(int nargout, mxArray *out[], int nargin

 void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("MyTemplateMatrix.Level",nargout,nargin,1);
+  checkArguments("MyTemplate<gtsam::Matrix>.Level",nargout,nargin,1);
  Matrix K = unwrap< Matrix >(in[0]);
  out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
 }
--- a/wrap/tests/expected/matlab/namespaces_wrapper.cpp
+++ b/wrap/tests/expected/matlab/namespaces_wrapper.cpp
@ -248,7 +248,7 @@ void ns2ClassA_nsReturn_12(int nargout, mxArray *out[], int nargin, const mxArra

 void ns2ClassA_afunction_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("ns2ClassA.afunction",nargout,nargin,0);
+  checkArguments("ns2::ClassA.afunction",nargout,nargin,0);
  out[0] = wrap< double >(ns2::ClassA::afunction());
 }

--- a/wrap/tests/expected/matlab/setPose.m
+++ b/wrap/tests/expected/matlab/setPose.m
@ -0,0 +1,6 @@
+function varargout = setPose(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Pose3')
+        functions_wrapper(13, varargin{:});
+      else
+        error('Arguments do not match any overload of function setPose');
+      end
--- a/wrap/tests/expected/python/class_pybind.cpp
+++ b/wrap/tests/expected/python/class_pybind.cpp
@ -31,6 +31,7 @@ PYBIND11_MODULE(class_py, m_) {
    py::class_<Fun<double>, std::shared_ptr<Fun<double>>>(m_, "FunDouble")
        .def("templatedMethodString",[](Fun<double>* self, double d, string t){return self->templatedMethod<string>(d, t);}, py::arg("d"), py::arg("t"))
        .def("multiTemplatedMethodStringSize_t",[](Fun<double>* self, double d, string t, size_t u){return self->multiTemplatedMethod<string,size_t>(d, t, u);}, py::arg("d"), py::arg("t"), py::arg("u"))
+        .def("sets",[](Fun<double>* self){return self->sets();})
        .def_static("staticMethodWithThis",[](){return Fun<double>::staticMethodWithThis();})
        .def_static("templatedStaticMethodInt",[](const int& m){return Fun<double>::templatedStaticMethod<int>(m);}, py::arg("m"));

--- a/wrap/tests/fixtures/class.i
+++ b/wrap/tests/fixtures/class.i
@ -18,6 +18,8 @@ class Fun {

  template<T={string}, U={size_t}>
  This multiTemplatedMethod(double d, T t, U u);
+
+  std::map<M, This::M> sets();
 };


--- a/wrap/tests/test_matlab_wrapper.py
+++ b/wrap/tests/test_matlab_wrapper.py
@ -92,10 +92,19 @@ class TestWrap(unittest.TestCase):
        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)

        files = [
-            'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m',
+            'functions_wrapper.cpp',
+            'aGlobalFunction.m',
+            'load2D.m',
            'MultiTemplatedFunctionDoubleSize_tDouble.m',
            'MultiTemplatedFunctionStringSize_tDouble.m',
-            'overloadedGlobalFunction.m', 'TemplatedFunctionRot3.m'
+            'overloadedGlobalFunction.m',
+            'TemplatedFunctionRot3.m',
+            'DefaultFuncInt.m',
+            'DefaultFuncObj.m',
+            'DefaultFuncString.m',
+            'DefaultFuncVector.m',
+            'DefaultFuncZero.m',
+            'setPose.m',
        ]

        for file in files:
@ -115,10 +124,17 @@ class TestWrap(unittest.TestCase):
        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)

        files = [
-            'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m',
-            'MultipleTemplatesIntDouble.m', 'MultipleTemplatesIntFloat.m',
-            'MyFactorPosePoint2.m', 'MyVector3.m', 'MyVector12.m',
-            'PrimitiveRefDouble.m', 'Test.m'
+            'class_wrapper.cpp',
+            'FunDouble.m',
+            'FunRange.m',
+            'MultipleTemplatesIntDouble.m',
+            'MultipleTemplatesIntFloat.m',
+            'MyFactorPosePoint2.m',
+            'MyVector3.m',
+            'MyVector12.m',
+            'PrimitiveRefDouble.m',
+            'Test.m',
+            'ForwardKinematics.m',
        ]

        for file in files:
@ -137,7 +153,10 @@ class TestWrap(unittest.TestCase):

        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)

-        files = ['template_wrapper.cpp']
+        files = [
+            'template_wrapper.cpp', 'ScopedTemplateResult.m',
+            'TemplatedConstructor.m'
+        ]

        for file in files:
            actual = osp.join(self.MATLAB_ACTUAL_DIR, file)
@ -155,8 +174,11 @@ class TestWrap(unittest.TestCase):
        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)

        files = [
-            'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m',
-            'MyTemplatePoint2.m'
+            'inheritance_wrapper.cpp',
+            'MyBase.m',
+            'MyTemplateMatrix.m',
+            'MyTemplatePoint2.m',
+            'ForwardKinematicsFactor.m',
        ]

        for file in files:
@ -178,10 +200,17 @@ class TestWrap(unittest.TestCase):
        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)

        files = [
-            'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m',
-            '+ns1/ClassA.m', '+ns1/ClassB.m', '+ns2/+ns3/ClassB.m',
-            '+ns2/aGlobalFunction.m', '+ns2/ClassA.m', '+ns2/ClassC.m',
-            '+ns2/overloadedGlobalFunction.m', 'ClassD.m'
+            'namespaces_wrapper.cpp',
+            '+ns1/aGlobalFunction.m',
+            '+ns1/ClassA.m',
+            '+ns1/ClassB.m',
+            '+ns2/+ns3/ClassB.m',
+            '+ns2/aGlobalFunction.m',
+            '+ns2/ClassA.m',
+            '+ns2/ClassC.m',
+            '+ns2/overloadedGlobalFunction.m',
+            'ClassD.m',
+            '+gtsam/Values.m',
        ]

        for file in files:
@ -203,8 +232,10 @@ class TestWrap(unittest.TestCase):

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

        for file in files: