Wrap fixed-size Eigen values as MATLAB arrays

gtwrap/matlab_wrapper/mixins.py

@@ -1,5 +1,7 @@
 """Mixins for reducing the amount of boilerplate in the main wrapper class."""
 
+import re
+
 from typing import Any, Tuple, Union
 
 import gtwrap.interface_parser as parser
@@ -25,6 +27,9 @@ class CheckMixin:
     matrix_view_types: Tuple = ('ConstMatrixView', )
     # Eigen Ref types used as Jacobian output arguments (not inputs).
     eigen_ref_types: Tuple = ('MatrixXd', )
+    # Fixed-size GTSAM Eigen aliases, such as Matrix3, Matrix36, and Vector9.
+    fixed_size_eigen_pattern = re.compile(
+        r'^(?:Matrix\d+(?:x\d+)?|Vector\d+)$')
     # Methods that should be ignored
     ignore_methods: Tuple = ('pickle', )
     # Methods that should not be wrapped directly
@@ -47,6 +52,7 @@ def can_be_pointer(self, arg_type: parser.Type):
         """
         return (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)
                 and arg_type.typename.name != 'string')
 
@@ -70,9 +76,43 @@ def is_ref(self, arg_type: parser.Type):
         reference in the wrapper.
         """
         return arg_type.typename.name not in self.ignore_namespace and \
+               not self.is_fixed_size_eigen_value(arg_type) and \
                arg_type.typename.name not in self.not_ptr_type and \
                arg_type.is_ref
 
+    def is_fixed_size_eigen_value(self, arg_type: parser.Type):
+        """Check for a fixed-size MatrixN/VectorN value or reference alias."""
+        return (not arg_type.is_shared_ptr and not arg_type.is_ptr
+                and self.fixed_size_eigen_pattern.fullmatch(
+                    arg_type.typename.name) is not None)
+
+    def fixed_size_eigen_dimensions(self, arg_type: parser.Type):
+        """Infer dimensions when a GTSAM fixed-size alias is unambiguous."""
+        if not self.is_fixed_size_eigen_value(arg_type):
+            return None
+
+        name = arg_type.typename.name
+        vector = re.fullmatch(r'Vector(\d+)', name)
+        if vector:
+            return int(vector.group(1)), 1
+
+        explicit_matrix = re.fullmatch(r'Matrix(\d+)x(\d+)', name)
+        if explicit_matrix:
+            return int(explicit_matrix.group(1)), int(explicit_matrix.group(2))
+
+        # GTSAM defines MatrixN as NxN, and MatrixMN as MxN for M,N=1..9.
+        square_matrix = re.fullmatch(r'Matrix([1-9])', name)
+        if square_matrix:
+            dimension = int(square_matrix.group(1))
+            return dimension, dimension
+
+        rectangular_matrix = re.fullmatch(r'Matrix([1-9])([1-9])', name)
+        if rectangular_matrix:
+            return int(rectangular_matrix.group(1)), int(
+                rectangular_matrix.group(2))
+
+        return None
+
     def is_matrix_view(self, arg_type: parser.Type):
         """Check if `arg_type` should be unwrapped as a matrix view."""
         return arg_type.typename.name in self.matrix_view_types

gtwrap/matlab_wrapper/wrapper.py

@@ -245,7 +245,8 @@ def _matlab_type_check(self,
         if name in self.not_check_type:
             return ''
 
-        check_type = self.data_type_param.get(name)
+        check_type = ('double' if self.is_fixed_size_eigen_value(ctype) else
+                      self.data_type_param.get(name))
         if self.data_type.get(check_type):
             check_type = self.data_type[check_type]
 
@@ -266,6 +267,12 @@ def _matlab_type_check(self,
             checks.append(f'size({variable},1)==3')
             checks.append(f'size({variable},2)==1')
 
+        fixed_dimensions = self.fixed_size_eigen_dimensions(ctype)
+        if fixed_dimensions:
+            rows, cols = fixed_dimensions
+            checks.append(f'size({variable},1)=={rows}')
+            checks.append(f'size({variable},2)=={cols}')
+
         return ' && '.join(checks)
 
     def _wrap_variable_arguments(self, args, wrap_datatypes=True):
@@ -372,6 +379,9 @@ def _unwrap_value_expression(self,
         if self.is_matrix_view(ctype):
             return f'unwrapMatrixView< {ctype_sep} >({value})'
 
+        if self.is_fixed_size_eigen_value(ctype):
+            return f'unwrapFixedSizeEigen< {ctype_sep} >({value})'
+
         if self.is_ptr(ctype) and ctype.typename.name not in self.ignore_namespace:
             return ('unwrap_ptr< {ctype} >({value}, "ptr_{camel}")'.format(
                 ctype=ctype_sep, value=value, camel=ctype_camel))
@@ -412,6 +422,11 @@ def _unwrap_argument(self, arg, arg_id=0, instantiated_class=None):
             arg_type = "Eigen::MatrixXd"
             unwrap = 'Eigen::MatrixXd();'
 
+        elif self.is_fixed_size_eigen_value(arg.ctype):
+            arg_type = ctype_sep
+            unwrap = 'unwrapFixedSizeEigen< {ctype} >(in[{id}]);'.format(
+                ctype=ctype_sep, id=arg_id)
+
         elif self.is_ref(arg.ctype):  # and not constructor:
             arg_type = "{ctype}&".format(ctype=ctype_sep)
             unwrap = '*unwrap_shared_ptr< {ctype} >(in[{id}], "ptr_{ctype_camel}");'.format(
@@ -1343,6 +1358,9 @@ def _collector_wrap_expression(self,
             return f'wrap_enum({obj},"{class_name}{ctype.typename.name}")'
 
         ctype_cpp = self._format_type_name(ctype.typename)
+        if self.is_fixed_size_eigen_value(ctype):
+            return f'wrapFixedSizeEigen({obj})'
+
         if ((self.is_shared_ptr(ctype) or self.is_ptr(ctype))
                 and ctype.typename.name in self.ignore_namespace):
             return f'wrap< {ctype_cpp} >(*{obj})'
@@ -1377,7 +1395,10 @@ 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_shared_ptr(return_type) or self.is_ptr(return_type) or \
+        if 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 \
             self.can_be_pointer(return_type):
             shared_obj = 'pairResult.' + pair_value
 
@@ -1436,6 +1457,9 @@ def _collector_return(self,
             expanded = textwrap.indent(
                 f'out[0] = wrap_enum({obj},\"{enum_type}\");', prefix='  ')
 
+        elif self.is_fixed_size_eigen_value(ctype):
+            expanded += '  out[0] = wrapFixedSizeEigen({0});'.format(obj)
+
         elif self.is_shared_ptr(ctype) or self.is_ptr(ctype) or \
             self.can_be_pointer(ctype):
             sep_method_name = partial(self._format_type_name,

matlab.h

@@ -211,6 +211,24 @@ mxArray* wrap<double>(const double& value) {
   return mxCreateDoubleScalar(value);
 }
 
+// Wrap a fixed-size Eigen matrix or vector as a MATLAB double array.
+template <typename Derived>
+mxArray* wrapFixedSizeEigen(const Eigen::MatrixBase<Derived>& value) {
+  static_assert(std::is_same<typename Derived::Scalar, double>::value,
+                "MATLAB wrappers only support double-valued Eigen aliases");
+  const mwSize rows = static_cast<mwSize>(value.rows());
+  const mwSize cols = static_cast<mwSize>(value.cols());
+  mxArray* result = mxCreateDoubleMatrix(rows, cols, mxREAL);
+  double* data = mxGetPr(result);
+  for (mwSize j = 0; j < cols; ++j) {
+    for (mwSize i = 0; i < rows; ++i, ++data) {
+      *data = value(static_cast<Eigen::Index>(i),
+                    static_cast<Eigen::Index>(j));
+    }
+  }
+  return result;
+}
+
 // wrap a const Eigen vector into a double vector
 mxArray* wrap_Vector(const gtsam::Vector& v) {
   int m = v.size();
@@ -388,6 +406,36 @@ double unwrap<double>(const mxArray* array) {
   return myGetScalar<double>(array);
 }
 
+// Unwrap a MATLAB double array into a fixed-size Eigen matrix or vector.
+template <typename EigenType>
+EigenType unwrapFixedSizeEigen(const mxArray* array) {
+  static_assert(std::is_same<typename EigenType::Scalar, double>::value,
+                "MATLAB wrappers only support double-valued Eigen aliases");
+  static_assert(EigenType::RowsAtCompileTime != Eigen::Dynamic &&
+                    EigenType::ColsAtCompileTime != Eigen::Dynamic,
+                "unwrapFixedSizeEigen requires a fixed-size Eigen type");
+  if (!mxIsDouble(array) || mxIsComplex(array) || mxIsSparse(array)) {
+    error("unwrapFixedSizeEigen: not a full real double matrix");
+  }
+
+  const mwSize rows = mxGetM(array);
+  const mwSize cols = mxGetN(array);
+  if (rows != static_cast<mwSize>(EigenType::RowsAtCompileTime) ||
+      cols != static_cast<mwSize>(EigenType::ColsAtCompileTime)) {
+    error("unwrapFixedSizeEigen: matrix dimensions do not match C++ type");
+  }
+
+  const double* data = static_cast<const double*>(mxGetData(array));
+  EigenType result;
+  for (mwSize j = 0; j < cols; ++j) {
+    for (mwSize i = 0; i < rows; ++i, ++data) {
+      result(static_cast<Eigen::Index>(i), static_cast<Eigen::Index>(j)) =
+          *data;
+    }
+  }
+  return result;
+}
+
 // specialization to Eigen vector
 template<>
 gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) {

tests/fixtures/fixed_size_eigen.i

@@ -0,0 +1,19 @@
+#include <folder/path/to/FixedSizeEigenFixture.h>
+
+namespace gtsam {
+
+class FixedSizeEigenFixture {
+  FixedSizeEigenFixture(const gtsam::Matrix3& matrix,
+                        const gtsam::Vector10& vector);
+
+  gtsam::Matrix3 matrix(const gtsam::Matrix3& value) const;
+  gtsam::Matrix36 rectangular(const gtsam::Matrix36& value) const;
+  gtsam::Vector10 vector(const gtsam::Vector10& value) const;
+  pair<gtsam::Matrix3, gtsam::Vector10> pairValues() const;
+  std::optional<gtsam::Matrix3> optionalMatrix() const;
+
+  gtsam::Matrix3 matrixProperty;
+  gtsam::Vector10 vectorProperty;
+};
+
+}

tests/test_matlab_wrapper.py

@@ -122,6 +122,67 @@ def test_matrix_view_arguments(self):
         self.assertIn('Eigen::Index m', header_content)
         self.assertIn('Stride(m, 1)', header_content)
 
+    def test_fixed_size_eigen_values(self):
+        """MatrixN/VectorN aliases are MATLAB double arrays, not handles."""
+        file = osp.join(self.INTERFACE_DIR, 'fixed_size_eigen.i')
+
+        wrapper = MatlabWrapper(module_name='fixed_size_eigen',
+                                top_module_namespace=['gtsam'],
+                                ignore_classes=[''])
+        wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
+
+        cpp_file = osp.join(self.MATLAB_ACTUAL_DIR,
+                            'fixed_size_eigen_wrapper.cpp')
+        with open(cpp_file, 'r', encoding='UTF-8') as f:
+            cpp_content = f.read()
+
+        self.assertIn(
+            'gtsam::Matrix3 matrix = unwrapFixedSizeEigen< gtsam::Matrix3 >(in[0]);',
+            cpp_content)
+        self.assertIn(
+            'gtsam::Vector10 vector = unwrapFixedSizeEigen< gtsam::Vector10 >(in[1]);',
+            cpp_content)
+        self.assertIn('out[0] = wrapFixedSizeEigen(obj->matrix(value));',
+                      cpp_content)
+        self.assertIn(
+            'out[0] = wrapFixedSizeEigen(obj->rectangular(value));',
+            cpp_content)
+        self.assertIn('out[0] = wrapFixedSizeEigen(obj->vector(value));',
+                      cpp_content)
+        self.assertIn(
+            'out[0] = wrapFixedSizeEigen(pairResult.first);', cpp_content)
+        self.assertIn(
+            'out[1] = wrapFixedSizeEigen(pairResult.second);', cpp_content)
+        self.assertIn(
+            'return wrapFixedSizeEigen(value);', cpp_content)
+        self.assertIn(
+            'out[0] = wrapFixedSizeEigen(obj->matrixProperty);', cpp_content)
+        self.assertNotIn('wrap_shared_ptr(std::make_shared<gtsam::Matrix3>',
+                         cpp_content)
+        self.assertNotIn('unwrap_shared_ptr< gtsam::Vector10 >', cpp_content)
+
+        m_file = osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam',
+                          'FixedSizeEigenFixture.m')
+        with open(m_file, 'r', encoding='UTF-8') as f:
+            matlab_content = f.read()
+
+        self.assertNotIn("isa(varargin{1},'gtsam.Matrix3')", matlab_content)
+        self.assertNotIn("isa(varargin{1},'gtsam.Vector10')", matlab_content)
+        self.assertIn(
+            "isa(varargin{1},'double') && size(varargin{1},1)==3 && size(varargin{1},2)==3",
+            matlab_content)
+        self.assertIn(
+            "isa(varargin{2},'double') && size(varargin{2},1)==10 && size(varargin{2},2)==1",
+            matlab_content)
+
+        matlab_header = osp.join(self.TEST_DIR, '..', 'matlab.h')
+        with open(matlab_header, 'r', encoding='UTF-8') as f:
+            header_content = f.read()
+
+        self.assertIn('mxArray* wrapFixedSizeEigen(', header_content)
+        self.assertIn('EigenType unwrapFixedSizeEigen(', header_content)
+        self.assertIn('RowsAtCompileTime', header_content)
+
     def test_pybind_lambda_annotation_is_ignored(self):
         """Pybind-only annotations do not alter generated MATLAB files."""
         source = Path(self.INTERFACE_DIR) / 'pybind_lambda_adapters.i'