Wrap Update

matlab/gtsam_tests/testJacobianFactor.m

@@ -32,7 +32,7 @@
 % the RHS
 b2=[-1;1.5;2;-1];
 sigmas = [1;1;1;1];
-model4 = noiseModel.Diagonal.Sigmas(sigmas, true);
+model4 = noiseModel.Diagonal.Sigmas(sigmas);
 combined = JacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
 
 % eliminate the first variable (x2) in the combined factor, destructive !
@@ -74,7 +74,7 @@
 % the RHS
 b1= [0.0;0.894427];
 
-model2 = noiseModel.Diagonal.Sigmas([1;1], true);
+model2 = noiseModel.Diagonal.Sigmas([1;1]);
 expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
 
 % check if the result matches the combined (reduced) factor

matlab/gtsam_tests/testKalmanFilter.m

@@ -23,13 +23,13 @@
 F = eye(2,2);
 B = eye(2,2);
 u = [1.0; 0.0];
-modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
+modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1]);
 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], true);
+modelR = noiseModel.Diagonal.Sigmas([0.1;0.1]);
 R = 0.01*eye(2,2);
 
 %% Create the set of expected output TestValues

matlab/gtsam_tests/testLocalizationExample.m

@@ -17,7 +17,7 @@
 
 %% 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], true); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 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.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], true);
+model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
 for i=1:3
     graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
 end

matlab/gtsam_tests/testOdometryExample.m

@@ -17,12 +17,12 @@
 
 %% 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], true); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 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], true); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 

matlab/gtsam_tests/testPlanarSLAMExample.m

@@ -30,18 +30,18 @@
 
 %% Add prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 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], true);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 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], true);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 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));

matlab/gtsam_tests/testPose2SLAMExample.m

@@ -26,19 +26,19 @@
 %% 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], true);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 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], true);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 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], true);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 
 %% Initialize to noisy points

matlab/gtsam_tests/testPose3SLAMExample.m

@@ -21,7 +21,7 @@
 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], true);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
 fg.add(BetweenFactorPose3(0,1, delta, covariance));
 fg.add(BetweenFactorPose3(1,2, delta, covariance));
 fg.add(BetweenFactorPose3(2,3, delta, covariance));

matlab/gtsam_tests/testSFMExample.m

@@ -25,17 +25,17 @@
 graph = NonlinearFactorGraph;
 
 %% Add factors for all measurements
-measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma, true);
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
         j = data.J{i}{k};
         graph.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K));
     end
 end
 
-posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas, true);
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
 graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
-pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma, true);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
 graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
 
 %% Initial estimate

matlab/gtsam_tests/testSerialization.m

@@ -45,30 +45,30 @@
 
 % Prior factor
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 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], true);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 % Range Factors
-rNoise = noiseModel.Diagonal.Sigmas([0.2], true);
+rNoise = noiseModel.Diagonal.Sigmas([0.2]);
 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], true);
+bNoise = noiseModel.Diagonal.Sigmas([0.1]);
 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], true);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 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));

matlab/gtsam_tests/testStereoVOExample.m

@@ -33,7 +33,7 @@
 %% 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], true);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 
 %% Add measurements
 % pose 1

wrap/gtwrap/matlab_wrapper/wrapper.py

@@ -10,8 +10,10 @@
 import textwrap
 from functools import partial, reduce
 from typing import Dict, Iterable, List, Union
+import copy
 
 import gtwrap.interface_parser as parser
+from gtwrap.interface_parser.function import ArgumentList
 import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin
 from gtwrap.matlab_wrapper.templates import WrapperTemplate
@@ -137,6 +139,37 @@ def _insert_spaces(self, x, y):
         """
         return x + '\n' + ('' if y == '' else '  ') + y
 
+    @staticmethod
+    def _expand_default_arguments(method, save_backup=True):
+        """Recursively expand all possibilities for optional default arguments.
+        We create "overload" functions with fewer arguments, but since we have to "remember" what
+        the default arguments are for later, we make a backup.
+        """
+        def args_copy(args):
+            return ArgumentList([copy.copy(arg) for arg in args.list()])
+        def method_copy(method):
+            method2 = copy.copy(method)
+            method2.args = args_copy(method.args)
+            method2.args.backup = method.args.backup
+            return method2
+        if save_backup:
+            method.args.backup = args_copy(method.args)
+        method = method_copy(method)
+        for arg in reversed(method.args.list()):
+            if arg.default is not None:
+                arg.default = None
+                methodWithArg = method_copy(method)
+                method.args.list().remove(arg)
+                return [
+                    methodWithArg,
+                    *MatlabWrapper._expand_default_arguments(method, save_backup=False)
+                ]
+            break
+        assert all(arg.default is None for arg in method.args.list()), \
+            'In parsing method {:}: Arguments with default values cannot appear before ones ' \
+            'without default values.'.format(method.name)
+        return [method]
+
     def _group_methods(self, methods):
         """Group overloaded methods together"""
         method_map = {}
@@ -147,9 +180,9 @@ def _group_methods(self, methods):
 
             if method_index is None:
                 method_map[method.name] = len(method_out)
-                method_out.append([method])
+                method_out.append(MatlabWrapper._expand_default_arguments(method))
             else:
-                method_out[method_index].append(method)
+                method_out[method_index] += MatlabWrapper._expand_default_arguments(method)
 
         return method_out
 
@@ -301,13 +334,9 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False):
             ((a), Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test");),
             ((a), std::shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test");)
         """
-        params = ''
         body_args = ''
 
         for arg in args.list():
-            if params != '':
-                params += ','
-
             if self.is_ref(arg.ctype):  # and not constructor:
                 ctype_camel = self._format_type_name(arg.ctype.typename,
                                                      separator='')
@@ -336,8 +365,6 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False):
                            name=arg.name,
                            id=arg_id)),
                                              prefix='  ')
-                if call_type == "":
-                    params += "*"
 
             else:
                 body_args += textwrap.indent(textwrap.dedent('''\
@@ -347,10 +374,29 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False):
                            id=arg_id)),
                                              prefix='  ')
 
-            params += arg.name
-
             arg_id += 1
 
+        params = ''
+        explicit_arg_names = [arg.name for arg in args.list()]
+        # when returning the params list, we need to re-include the default args.
+        for arg in args.backup.list():
+            if params != '':
+                params += ','
+
+            if (arg.default is not None) and (arg.name not in explicit_arg_names):
+                params += arg.default
+                continue
+
+            if (not self.is_ref(arg.ctype)) and (self.is_shared_ptr(arg.ctype)) and (self.is_ptr(
+                    arg.ctype)) and (arg.ctype.typename.name not in self.ignore_namespace):
+                if arg.ctype.is_shared_ptr:
+                    call_type = arg.ctype.is_shared_ptr
+                else:
+                    call_type = arg.ctype.is_ptr
+                if call_type == "":
+                    params += "*"
+            params += arg.name
+
         return params, body_args
 
     @staticmethod
@@ -555,6 +601,8 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name,
         if not isinstance(ctors, Iterable):
             ctors = [ctors]
 
+        ctors = sum((MatlabWrapper._expand_default_arguments(ctor) for ctor in ctors), [])
+
         methods_wrap = textwrap.indent(textwrap.dedent("""\
             methods
               function obj = {class_name}(varargin)
@@ -674,20 +722,7 @@ def wrap_class_display(self):
 
     def _group_class_methods(self, methods):
         """Group overloaded methods together"""
-        method_map = {}
-        method_out = []
-
-        for method in methods:
-            method_index = method_map.get(method.name)
-
-            if method_index is None:
-                method_map[method.name] = len(method_out)
-                method_out.append([method])
-            else:
-                # print("[_group_methods] Merging {} with {}".format(method_index, method.name))
-                method_out[method_index].append(method)
-
-        return method_out
+        return self._group_methods(methods)
 
     @classmethod
     def _format_varargout(cls, return_type, return_type_formatted):

wrap/tests/actual/.gitignore

@@ -0,0 +1,2 @@
+./*
+!.gitignore

wrap/tests/expected/matlab/DefaultFuncInt.m

@@ -1,6 +1,10 @@
 function varargout = DefaultFuncInt(varargin)
       if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric')
         functions_wrapper(8, varargin{:});
+      elseif length(varargin) == 1 && isa(varargin{1},'numeric')
+        functions_wrapper(9, varargin{:});
+      elseif length(varargin) == 0
+        functions_wrapper(10, varargin{:});
       else
         error('Arguments do not match any overload of function DefaultFuncInt');
       end

wrap/tests/expected/matlab/DefaultFuncObj.m

@@ -1,6 +1,8 @@
 function varargout = DefaultFuncObj(varargin)
       if length(varargin) == 1 && isa(varargin{1},'gtsam.KeyFormatter')
-        functions_wrapper(10, varargin{:});
+        functions_wrapper(14, varargin{:});
+      elseif length(varargin) == 0
+        functions_wrapper(15, varargin{:});
       else
         error('Arguments do not match any overload of function DefaultFuncObj');
       end

wrap/tests/expected/matlab/DefaultFuncString.m

@@ -1,6 +1,10 @@
 function varargout = DefaultFuncString(varargin)
       if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'char')
-        functions_wrapper(9, varargin{:});
+        functions_wrapper(11, varargin{:});
+      elseif length(varargin) == 1 && isa(varargin{1},'char')
+        functions_wrapper(12, varargin{:});
+      elseif length(varargin) == 0
+        functions_wrapper(13, varargin{:});
       else
         error('Arguments do not match any overload of function DefaultFuncString');
       end

wrap/tests/expected/matlab/DefaultFuncVector.m

@@ -1,6 +1,10 @@
 function varargout = DefaultFuncVector(varargin)
       if length(varargin) == 2 && isa(varargin{1},'std.vectornumeric') && isa(varargin{2},'std.vectorchar')
-        functions_wrapper(12, varargin{:});
+        functions_wrapper(20, varargin{:});
+      elseif length(varargin) == 1 && isa(varargin{1},'std.vectornumeric')
+        functions_wrapper(21, varargin{:});
+      elseif length(varargin) == 0
+        functions_wrapper(22, varargin{:});
       else
         error('Arguments do not match any overload of function DefaultFuncVector');
       end

wrap/tests/expected/matlab/DefaultFuncZero.m

@@ -1,6 +1,12 @@
 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{:});
+      if length(varargin) == 5 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'numeric') && isa(varargin{5},'logical')
+        functions_wrapper(16, varargin{:});
+      elseif length(varargin) == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'numeric')
+        functions_wrapper(17, varargin{:});
+      elseif length(varargin) == 3 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double')
+        functions_wrapper(18, varargin{:});
+      elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric')
+        functions_wrapper(19, varargin{:});
       else
         error('Arguments do not match any overload of function DefaultFuncZero');
       end