Convert numeric STL containers to MATLAB values

gtwrap/matlab_wrapper/mixins.py

@@ -43,14 +43,30 @@ def _has_serialization(self, cls):
                 return True
         return False
 
+    @staticmethod
+    def is_numeric_container(ctype):
+        """Containers represented by MATLAB double vectors or containers.Map."""
+        if not isinstance(ctype, parser.TemplatedType):
+            return False
+        if ctype.is_ptr or ctype.is_shared_ptr:
+            return False
+        name = ctype.typename.qualified_name()
+        params = [p.typename.name for p in ctype.template_params]
+        if name == 'std::vector':
+            return params == ['double']
+        return (name == 'std::map' and len(params) == 2
+                and params[0] in ('string', 'double', 'Key', 'size_t', 'uint64_t')
+                and params[1] in ('double', 'int', 'size_t', 'DenseIndex'))
+
     def can_be_pointer(self, arg_type: parser.Type):
         """
         Determine if the `arg_type` can have a pointer to it.
 
         E.g. `Pose3` can have `Pose3*` but 
         `Matrix` should not have `Matrix*`.
         """
-        return (arg_type.typename.name not in self.not_ptr_type
+        return (not self.is_numeric_container(arg_type)
+                and arg_type.typename.name not in self.not_ptr_type
                 and arg_type.typename.name not in self.ignore_namespace
                 and not self.is_fixed_size_eigen_value(arg_type)
                 and not self.is_matrix_view(arg_type)

gtwrap/matlab_wrapper/wrapper.py

@@ -233,6 +233,11 @@ def _matlab_type_check(self,
                            variable,
                            wrap_datatypes=True):
         """Return the MATLAB predicate used to dispatch one argument."""
+        if self.is_numeric_container(ctype):
+            matlab_type = ('double' if ctype.typename.name == 'vector'
+                           else 'containers.Map')
+            return f"isa({variable},'{matlab_type}')"
+
         if self.is_optional(ctype):
             value_check = self._matlab_type_check(
                 self.optional_value_type(ctype), variable, wrap_datatypes)
@@ -360,6 +365,9 @@ def _unwrap_value_expression(self,
                                  value,
                                  instantiated_class=None):
         """Return a C++ expression that unwraps one MATLAB value."""
+        if self.is_numeric_container(ctype):
+            return f'unwrap_numeric_container<{ctype.get_typename()}>({value})'
+
         if self.is_optional(ctype):
             value_type = self.optional_value_type(ctype)
             cpp_type = value_type.to_cpp()
@@ -400,7 +408,11 @@ def _unwrap_argument(self, arg, arg_id=0, instantiated_class=None):
         ctype_camel = self._format_type_name(arg.ctype.typename, separator='')
         ctype_sep = self._format_type_name(arg.ctype.typename)
 
-        if self.is_optional(arg.ctype):
+        if self.is_numeric_container(arg.ctype):
+            arg_type = arg.ctype.get_typename()
+            unwrap = f'unwrap_numeric_container<{arg_type}>(in[{arg_id}]);'
+
+        elif self.is_optional(arg.ctype):
             arg_type = arg.ctype.get_typename()
             unwrap = self._unwrap_value_expression(
                 arg.ctype, f'in[{arg_id}]', instantiated_class) + ';'
@@ -1336,6 +1348,9 @@ def _collector_wrap_expression(self,
                                    ctype,
                                    instantiated_class=None):
         """Return the expression that converts one C++ value to mxArray*."""
+        if self.is_numeric_container(ctype):
+            return f'wrap_numeric_container({obj})'
+
         if self.is_optional(ctype):
             value_type = self.optional_value_type(ctype)
             cpp_type = value_type.to_cpp()
@@ -1395,7 +1410,9 @@ def wrap_collector_function_return_types(self, return_type, func_id):
         pair_value = 'first' if func_id == 0 else 'second'
         new_line = '\n' if func_id == 0 else ''
 
-        if self.is_fixed_size_eigen_value(return_type):
+        if self.is_numeric_container(return_type):
+            return_type_text += f'wrap_numeric_container(pairResult.{pair_value});{new_line}'
+        elif self.is_fixed_size_eigen_value(return_type):
             return_type_text += 'wrapFixedSizeEigen(pairResult.{0});{1}'.format(
                 pair_value, new_line)
         elif self.is_shared_ptr(return_type) or self.is_ptr(return_type) or \
@@ -1435,7 +1452,10 @@ def _collector_return(self,
         """Helper method to get the final statement before the return in the collector function."""
         expanded = ''
 
-        if self.is_optional(ctype):
+        if self.is_numeric_container(ctype):
+            expanded = f'  out[0] = wrap_numeric_container({obj});'
+
+        elif self.is_optional(ctype):
             expanded = '  out[0] = {wrapped};'.format(
                 wrapped=self._collector_wrap_expression(
                     obj, ctype, instantiated_class))

matlab.h

@@ -35,7 +35,11 @@ using gtsam::Point3;
 
 #include <mex.h>
 
+#include <algorithm>
+#include <cmath>
 #include <cstdint>
+#include <map>
+#include <vector>
 #include <limits>
 #include <list>
 #include <optional>
@@ -647,3 +651,75 @@ Class* unwrap_ptr(const mxArray* obj, const string& propertyName) {
 //  static_assert(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer");
 //  return Matrix();
 //}
+
+// Numeric STL containers use MATLAB values rather than opaque proxy objects.
+inline mxArray* wrap_numeric_container(const std::vector<double>& values) {
+  mxArray* result = mxCreateDoubleMatrix(values.size(), 1, mxREAL);
+  std::copy(values.begin(), values.end(), mxGetPr(result));
+  return result;
+}
+
+template <typename Key, typename Value>
+mxArray* wrap_numeric_container(const std::map<Key, Value>& values) {
+  mxArray* result = nullptr;
+  if (values.empty()) {
+    const char* keyType = std::is_same_v<Key, std::string> ? "char" :
+        (std::is_integral_v<Key> ? "uint64" : "double");
+    mxArray* args[] = {mxCreateString("KeyType"), mxCreateString(keyType),
+                      mxCreateString("ValueType"), mxCreateString("double")};
+    int status = mexCallMATLAB(1, &result, 4, args, "containers.Map");
+    for (auto arg : args) mxDestroyArray(arg);
+    if (status) error("Could not create containers.Map");
+  } else {
+    mxArray* args[] = {mxCreateCellMatrix(1, values.size()),
+                      mxCreateCellMatrix(1, values.size())};
+    mwIndex index = 0;
+    for (const auto& entry : values) {
+      mxSetCell(args[0], index, wrap<Key>(entry.first));
+      mxSetCell(args[1], index++, mxCreateDoubleScalar(entry.second));
+    }
+    int status = mexCallMATLAB(1, &result, 2, args, "containers.Map");
+    for (auto arg : args) mxDestroyArray(arg);
+    if (status) error("Could not create containers.Map");
+  }
+  return result;
+}
+
+template <typename Container>
+Container unwrap_numeric_container(const mxArray* array) {
+  if constexpr (std::is_same_v<Container, std::vector<double>>) {
+    if (!mxIsDouble(array) || mxIsComplex(array) || mxIsSparse(array) ||
+        (!mxIsEmpty(array) && mxGetM(array) != 1 && mxGetN(array) != 1))
+      error("Expected a real, full double vector");
+    if (mxIsEmpty(array)) return {};
+    const double* data = mxGetPr(array);
+    return Container(data, data + mxGetNumberOfElements(array));
+  } else {
+    if (!mxIsClass(array, "containers.Map")) error("Expected containers.Map");
+    mxArray* input = const_cast<mxArray*>(array);
+    mxArray *keys = nullptr, *values = nullptr;
+    if (mexCallMATLAB(1, &keys, 1, &input, "keys") ||
+        mexCallMATLAB(1, &values, 1, &input, "values"))
+      error("Could not read containers.Map");
+    Container result;
+    for (mwIndex i = 0; i < mxGetNumberOfElements(keys); ++i) {
+      using Key = typename Container::key_type;
+      using Value = typename Container::mapped_type;
+      const mxArray* value = mxGetCell(values, i);
+      if (!mxIsDouble(value) || mxIsComplex(value) ||
+          mxGetNumberOfElements(value) != 1)
+        error("Expected scalar double map values");
+      double number = mxGetScalar(value);
+      if constexpr (std::is_integral_v<Value>) {
+        if (!std::isfinite(number) || std::trunc(number) != number ||
+            static_cast<long double>(number) < std::numeric_limits<Value>::lowest() ||
+            static_cast<long double>(number) > std::numeric_limits<Value>::max())
+          error("Map value is outside the integer range");
+      }
+      result.emplace(unwrap<Key>(mxGetCell(keys, i)), static_cast<Value>(number));
+    }
+    mxDestroyArray(keys);
+    mxDestroyArray(values);
+    return result;
+  }
+}

tests/fixtures/numeric_containers.i

@@ -0,0 +1,11 @@
+namespace gtsam {
+class NumericContainers {
+  NumericContainers();
+  bool solve(const std::map<std::string, double>& params = std::map<std::string, double>());
+  std::vector<double> evrs() const;
+  const std::map<gtsam::Key, gtsam::DenseIndex>& dims() const;
+  std::vector<double> echo(std::vector<double> values) const;
+  std::optional<std::pair<std::vector<double>, std::map<double, double>>> pair() const;
+  std::optional<std::vector<double>> optional(std::optional<std::vector<double>> values) const;
+};
+}

tests/test_matlab_wrapper.py

@@ -333,6 +333,27 @@ def test_eigen_ref_jacobians(self):
         self.assertIn('out[1] = wrap< Eigen::MatrixXd >(Hxi);', cpp_content)
         self.assertIn('checkArguments("gtsam::Pose3.Expmap",nargout,nargin,1);', cpp_content)
 
+    def test_numeric_containers(self):
+        """Numeric STL containers use native MATLAB values in both directions."""
+        wrapper = MatlabWrapper(module_name='numeric_containers',
+                                top_module_namespace=['gtsam'])
+        wrapper.wrap([osp.join(self.INTERFACE_DIR, 'numeric_containers.i')],
+                     path=self.MATLAB_ACTUAL_DIR)
+        generated = Path(self.MATLAB_ACTUAL_DIR)
+        cpp = (generated / 'numeric_containers_wrapper.cpp').read_text()
+        matlab = (generated / '+gtsam' / 'NumericContainers.m').read_text()
+        self.assertIn("isa(varargin{1},'containers.Map')", matlab)
+        self.assertIn("isa(varargin{1},'double')", matlab)
+        self.assertNotIn("'std.map", matlab)
+        self.assertNotIn("'std.vector", matlab)
+        self.assertIn('unwrap_numeric_container<std::map<std::string, double>>(in[1])', cpp)
+        self.assertIn('unwrap_numeric_container<std::vector<double>>(in[1])', cpp)
+        self.assertIn('obj->echo(values)', cpp)
+        self.assertNotIn('obj->echo(*values)', cpp)
+        for result in ('obj->evrs()', 'obj->dims()', 'pairResult.first',
+                       'pairResult.second', 'value'):
+            self.assertIn(f'wrap_numeric_container({result})', cpp)
+
     def test_std_optional(self):
         """Test MATLAB [] <-> std::nullopt and engaged value conversion."""
         file = osp.join(self.INTERFACE_DIR, 'optionals.i')