matlab.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
25 #include <gtsam/base/Vector.h>
26 #include <gtsam/base/Matrix.h>
27 #include <gtsam/geometry/Point2.h>
28 #include <gtsam/geometry/Point3.h>
29 #include <gtsam/base/utilities.h>
30 
31 using gtsam::Vector;
32 using gtsam::Matrix;
33 using gtsam::Point2;
34 using gtsam::Point3;
35 
36 extern "C" {
37 #include <mex.h>
38 }
39 
40 #include <list>
41 #include <set>
42 #include <sstream>
43 #include <streambuf>
44 #include <string>
45 #include <typeinfo>
46 
47 using namespace std;
48 
49 // start GTSAM Specifics /////////////////////////////////////////////////
50 // to enable Matrix and Vector constructor for SharedGaussian:
51 #define GTSAM_MAGIC_GAUSSIAN
52 // end GTSAM Specifics /////////////////////////////////////////////////
53 
54 #if defined(__LP64__) || defined(_WIN64)
55 // 64-bit
56 #define mxUINT32OR64_CLASS mxUINT64_CLASS
57 #else
58 #define mxUINT32OR64_CLASS mxUINT32_CLASS
59 #endif
60 
61 // "Unique" key to signal calling the matlab object constructor with a raw pointer
62 // to a shared pointer of the same C++ object type as the MATLAB type.
63 // Also present in utilities.h
65  (std::uint64_t('G') << 56) |
66  (std::uint64_t('T') << 48) |
67  (std::uint64_t('S') << 40) |
68  (std::uint64_t('A') << 32) |
69  (std::uint64_t('M') << 24) |
70  (std::uint64_t('p') << 16) |
71  (std::uint64_t('t') << 8) |
72  (std::uint64_t('r'));
73 
74 //*****************************************************************************
75 // Utilities
76 //*****************************************************************************
77 
78 void error(const char* str) {
79  mexErrMsgIdAndTxt("wrap:error", str);
80 }
81 
82 mxArray *scalar(mxClassID classid) {
83  mwSize dims[1]; dims[0]=1;
84  return mxCreateNumericArray(1, dims, classid, mxREAL);
85 }
86 
87 void checkScalar(const mxArray* array, const char* str) {
88  int m = mxGetM(array), n = mxGetN(array);
89  if (m!=1 || n!=1)
90  mexErrMsgIdAndTxt("wrap: not a scalar in ", str);
91 }
92 
93 // Replacement streambuf for cout that writes to the MATLAB console
94 // Thanks to http://stackoverflow.com/a/249008
95 class mstream : public std::streambuf {
96 protected:
97  virtual std::streamsize xsputn(const char *s, std::streamsize n) {
98  mexPrintf("%.*s",n,s);
99  return n;
100  }
101  virtual int overflow(int c = EOF) {
102  if (c != EOF) {
103  mexPrintf("%.1s",&c);
104  }
105  return 1;
106  }
107 };
108 
109 //*****************************************************************************
110 // Check arguments
111 //*****************************************************************************
112 
113 void checkArguments(const string& name, int nargout, int nargin, int expected) {
114  stringstream err;
115  err << name << " expects " << expected << " arguments, not " << nargin;
116  if (nargin!=expected)
117  error(err.str().c_str());
118 }
119 
120 //*****************************************************************************
121 // wrapping C++ basic types in MATLAB arrays
122 //*****************************************************************************
123 
124 // default wrapping throws an error: only basic types are allowed in wrap
125 template <typename Class>
126 mxArray* wrap(const Class& value) {
127  error("wrap internal error: attempted wrap of invalid type");
128  return 0;
129 }
130 
131 // specialization to string
132 // wraps into a character array
133 template<>
134 mxArray* wrap<string>(const string& value) {
135  return mxCreateString(value.c_str());
136 }
137 
138 // specialization to char
139 template<>
140 mxArray* wrap<char>(const char& value) {
141  mxArray *result = scalar(mxUINT32OR64_CLASS);
142  *(char*)mxGetData(result) = value;
143  return result;
144 }
145 
146 // specialization to unsigned char
147 template<>
148 mxArray* wrap<unsigned char>(const unsigned char& value) {
149  mxArray *result = scalar(mxUINT32OR64_CLASS);
150  *(unsigned char*)mxGetData(result) = value;
151  return result;
152 }
153 
154 // specialization to bool
155 template<>
156 mxArray* wrap<bool>(const bool& value) {
157  mxArray *result = scalar(mxUINT32OR64_CLASS);
158  *(bool*)mxGetData(result) = value;
159  return result;
160 }
161 
162 // specialization to size_t
163 template<>
164 mxArray* wrap<size_t>(const size_t& value) {
165  mxArray *result = scalar(mxUINT32OR64_CLASS);
166  *(size_t*)mxGetData(result) = value;
167  return result;
168 }
169 
170 // specialization to int
171 template<>
172 mxArray* wrap<int>(const int& value) {
173  mxArray *result = scalar(mxUINT32OR64_CLASS);
174  *(int*)mxGetData(result) = value;
175  return result;
176 }
177 
178 // specialization to double -> just double
179 template<>
180 mxArray* wrap<double>(const double& value) {
181  return mxCreateDoubleScalar(value);
182 }
183 
184 // wrap a const Eigen vector into a double vector
185 mxArray* wrap_Vector(const gtsam::Vector& v) {
186  int m = v.size();
187  mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL);
188  double *data = mxGetPr(result);
189  for (int i=0;i<m;i++) data[i]=v(i);
190  return result;
191 }
192 
193 // specialization to Eigen vector -> double vector
194 template<>
195 mxArray* wrap<gtsam::Vector >(const gtsam::Vector& v) {
196  return wrap_Vector(v);
197 }
198 
199 // specialization to Eigen vector -> double vector
200 template<>
201 mxArray* wrap<gtsam::Point2 >(const gtsam::Point2& v) {
202  return wrap_Vector(v);
203 }
204 
205 // specialization to Eigen vector -> double vector
206 template<>
207 mxArray* wrap<gtsam::Point3 >(const gtsam::Point3& v) {
208  return wrap_Vector(v);
209 }
210 
211 // wrap a const Eigen MATRIX into a double matrix
212 mxArray* wrap_Matrix(const gtsam::Matrix& A) {
213  int m = A.rows(), n = A.cols();
214 #ifdef DEBUG_WRAP
215  mexPrintf("wrap_Matrix called with A = \n", m,n);
216  gtsam::print(A);
217 #endif
218  mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL);
219  double *data = mxGetPr(result);
220  // converts from column-major to row-major
221  for (int j=0;j<n;j++) for (int i=0;i<m;i++,data++) *data = A(i,j);
222  return result;
223 }
224 
225 // specialization to Eigen MATRIX -> double matrix
226 template<>
227 mxArray* wrap<gtsam::Matrix >(const gtsam::Matrix& A) {
228  return wrap_Matrix(A);
229 }
230 
235 template <typename T>
236 mxArray* wrap_enum(const T x, const std::string& classname) {
237  // create double array to store value in
238  mxArray* a = mxCreateDoubleMatrix(1, 1, mxREAL);
239  double* data = mxGetPr(a);
240  data[0] = static_cast<double>(x);
241 
242  // convert to Matlab enumeration type
243  mxArray* result;
244  mexCallMATLAB(1, &result, 1, &a, classname.c_str());
245 
246  return result;
247 }
248 
249 //*****************************************************************************
250 // unwrapping MATLAB arrays into C++ basic types
251 //*****************************************************************************
252 
253 // default unwrapping throws an error
254 // as wrap only supports passing a reference or one of the basic types
255 template <typename T>
256 T unwrap(const mxArray* array) {
257  error("wrap internal error: attempted unwrap of invalid type");
258  return T();
259 }
260 
264 template <typename T>
265 T unwrap_enum(const mxArray* array) {
266  // Make duplicate to remove const-ness
267  mxArray* a = mxDuplicateArray(array);
268 
269  // convert void* to int32* array
270  mxArray* a_int32;
271  mexCallMATLAB(1, &a_int32, 1, &a, "int32");
272 
273  // Get the value in the input array
274  int32_T* value = (int32_T*)mxGetData(a_int32);
275  // cast int32 to enum type
276  return static_cast<T>(*value);
277 }
278 
279 // specialization to string
280 // expects a character array
281 // Warning: relies on mxChar==char
282 template<>
283 string unwrap<string>(const mxArray* array) {
284  char *data = mxArrayToString(array);
285  if (data==NULL) error("unwrap<string>: not a character array");
286  string str(data);
287  mxFree(data);
288  return str;
289 }
290 
291 // Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit
292 template <typename T>
293 T myGetScalar(const mxArray* array) {
294  switch (mxGetClassID(array)) {
295  case mxINT64_CLASS:
296  return (T) *(std::int64_t*) mxGetData(array);
297  case mxUINT64_CLASS:
298  return (T) *(std::uint64_t*) mxGetData(array);
299  default:
300  // hope for the best!
301  return (T) mxGetScalar(array);
302  }
303 }
304 
305 // specialization to bool
306 template<>
307 bool unwrap<bool>(const mxArray* array) {
308  checkScalar(array,"unwrap<bool>");
309  return myGetScalar<bool>(array);
310 }
311 
312 // specialization to char
313 template<>
314 char unwrap<char>(const mxArray* array) {
315  checkScalar(array,"unwrap<char>");
316  return myGetScalar<char>(array);
317 }
318 
319 // specialization to unsigned char
320 template<>
321 unsigned char unwrap<unsigned char>(const mxArray* array) {
322  checkScalar(array,"unwrap<unsigned char>");
323  return myGetScalar<unsigned char>(array);
324 }
325 
326 // specialization to int
327 template<>
328 int unwrap<int>(const mxArray* array) {
329  checkScalar(array,"unwrap<int>");
330  return myGetScalar<int>(array);
331 }
332 
333 // specialization to size_t
334 template<>
335 size_t unwrap<size_t>(const mxArray* array) {
336  checkScalar(array, "unwrap<size_t>");
337  return myGetScalar<size_t>(array);
338 }
339 
340 // specialization to double
341 template<>
342 double unwrap<double>(const mxArray* array) {
343  checkScalar(array,"unwrap<double>");
344  return myGetScalar<double>(array);
345 }
346 
347 // specialization to Eigen vector
348 template<>
350  int m = mxGetM(array), n = mxGetN(array);
351  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
352 #ifdef DEBUG_WRAP
353  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
354 #endif
355  double* data = (double*)mxGetData(array);
356  gtsam::Vector v(m);
357  for (int i=0;i<m;i++,data++) v(i) = *data;
358 #ifdef DEBUG_WRAP
359  gtsam::print(v);
360 #endif
361  return v;
362 }
363 
364 // specialization to Point2
365 template<>
367  int m = mxGetM(array), n = mxGetN(array);
368  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
369 #ifdef DEBUG_WRAP
370  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
371 #endif
372  double* data = (double*)mxGetData(array);
373  gtsam::Vector v(m);
374  for (int i=0;i<m;i++,data++) v(i) = *data;
375 #ifdef DEBUG_WRAP
376  gtsam::print(v);
377 #endif
378  return v;
379 }
380 
381 // specialization to Point3
382 template<>
384  int m = mxGetM(array), n = mxGetN(array);
385  if (mxIsDouble(array)==false || n!=1) error("unwrap<vector>: not a vector");
386 #ifdef DEBUG_WRAP
387  mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n);
388 #endif
389  double* data = (double*)mxGetData(array);
390  gtsam::Vector v(m);
391  for (int i=0;i<m;i++,data++) v(i) = *data;
392 #ifdef DEBUG_WRAP
393  gtsam::print(v);
394 #endif
395  return v;
396 }
397 
398 
399 // specialization to Eigen matrix
400 template<>
402  if (mxIsDouble(array)==false) error("unwrap<matrix>: not a matrix");
403  int m = mxGetM(array), n = mxGetN(array);
404 #ifdef DEBUG_WRAP
405  mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n);
406 #endif
407  double* data = (double*)mxGetData(array);
408  gtsam::Matrix A(m,n);
409  // converts from row-major to column-major
410  for (int j=0;j<n;j++) for (int i=0;i<m;i++,data++) A(i,j) = *data;
411 #ifdef DEBUG_WRAP
412  gtsam::print(A);
413 #endif
414  return A;
415 }
416 
417 /*
418  [create_object] creates a MATLAB proxy class object with a mexhandle
419  in the self property. Matlab does not allow the creation of matlab
420  objects from within mex files, hence we resort to an ugly trick: we
421  invoke the proxy class constructor by calling MATLAB with a special
422  uint64 value ptr_constructor_key and the pointer itself. MATLAB
423  allocates the object. Then, the special constructor in our wrap code
424  that is activated when the ptr_constructor_key is passed in passes
425  the pointer back into a C++ function to add the pointer to its
426  collector. We go through this extra "C++ to MATLAB to C++ step" in
427  order to be able to add to the collector could be in a different wrap
428  module.
429 */
430 mxArray* create_object(const std::string& classname, void *pointer, bool isVirtual, const char *rttiName) {
431  mxArray *result;
432  mxArray *input[3];
433  int nargin = 2;
434  // First input argument is pointer constructor key
435  input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL);
436  *reinterpret_cast<std::uint64_t*>(mxGetData(input[0])) = ptr_constructor_key;
437  // Second input argument is the pointer
438  input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
439  *reinterpret_cast<void**>(mxGetData(input[1])) = pointer;
440  // If the class is virtual, use the RTTI name to look up the derived matlab type
441  const char *derivedClassName;
442  if(isVirtual) {
443  const mxArray *rttiRegistry = mexGetVariablePtr("global", "gtsamwrap_rttiRegistry");
444  if(!rttiRegistry)
445  mexErrMsgTxt(
446  "gtsam wrap: RTTI registry is missing - it could have been cleared from the workspace."
447  " You can issue 'clear all' to completely clear the workspace, and next time a wrapped object is"
448  " created the RTTI registry will be recreated.");
449  const mxArray *derivedNameMx = mxGetField(rttiRegistry, 0, rttiName);
450  if(!derivedNameMx)
451  mexErrMsgTxt((
452  "gtsam wrap: The derived class type " + string(rttiName) + " was not found in the RTTI registry. "
453  "Try calling 'clear all' twice consecutively - we have seen things not get unloaded properly the "
454  "first time. If this does not work, this may indicate an inconsistency in your wrap interface file. "
455  "The most likely cause for this is that a base class was marked virtual in the wrap interface "
456  "definition header file for gtsam or for your module, but a derived type was returned by a C++ "
457  "function and that derived type was not marked virtual (or was not specified in the wrap interface "
458  "definition header at all).").c_str());
459  size_t strLen = mxGetN(derivedNameMx);
460  char *buf = new char[strLen+1];
461  if(mxGetString(derivedNameMx, buf, strLen+1))
462  mexErrMsgTxt("gtsam wrap: Internal error reading RTTI table, try 'clear all' to clear your workspace and reinitialize the toolbox.");
463  derivedClassName = buf;
464  input[2] = mxCreateString("void");
465  nargin = 3;
466  } else {
467  derivedClassName = classname.c_str();
468  }
469  // Call special pointer constructor, which sets 'self'
470  mexCallMATLAB(1,&result, nargin, input, derivedClassName);
471  // Deallocate our memory
472  mxDestroyArray(input[0]);
473  mxDestroyArray(input[1]);
474  if(isVirtual) {
475  mxDestroyArray(input[2]);
476  delete[] derivedClassName;
477  }
478  return result;
479 }
480 
481 /*
482  When the user calls a method that returns a shared pointer, we create
483  an ObjectHandle from the shared_pointer and return it as a proxy
484  class to matlab.
485 */
486 template <typename Class>
487 mxArray* wrap_shared_ptr(std::shared_ptr< Class > shared_ptr, const std::string& matlabName, bool isVirtual) {
488  // Create actual class object from out pointer
489  mxArray* result;
490  if(isVirtual) {
491  std::shared_ptr<void> void_ptr(shared_ptr);
492  result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name());
493  } else {
494  std::shared_ptr<Class> *heapPtr = new std::shared_ptr<Class>(shared_ptr);
495  result = create_object(matlabName, heapPtr, isVirtual, "");
496  }
497  return result;
498 }
499 
500 template <typename Class>
501 std::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& propertyName) {
502 
503  mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str());
504  if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh)
505  || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error(
506  "Parameter is not an Shared type.");
507 
508  std::shared_ptr<Class>* spp = *reinterpret_cast<std::shared_ptr<Class>**> (mxGetData(mxh));
509  return *spp;
510 }
511 
512 template <typename Class>
513 Class* unwrap_ptr(const mxArray* obj, const string& propertyName) {
514 
515  mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str());
516  Class* x = reinterpret_cast<Class*> (mxGetData(mxh));
517  return x;
518 }
519 
521 //template <>
522 //Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {
523 // bool unwrap_shared_ptr_Vector_attempted = false;
524 // static_assert(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer");
525 // return Vector();
526 //}
527 
529 //template <>
530 //Matrix unwrap_shared_ptr<Matrix>(const mxArray* obj, const string& propertyName) {
531 // bool unwrap_shared_ptr_Matrix_attempted = false;
532 // static_assert(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer");
533 // return Matrix();
534 //}
535 
int unwrap< int >(const mxArray *array)
Definition: matlab.h:328
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
int array[24]
mxArray * wrap_shared_ptr(std::shared_ptr< Class > shared_ptr, const std::string &matlabName, bool isVirtual)
Definition: matlab.h:487
gtsam::Vector unwrap< gtsam::Vector >(const mxArray *array)
Definition: matlab.h:349
Matrix expected
Definition: testMatrix.cpp:971
char unwrap< char >(const mxArray *array)
Definition: matlab.h:314
static const std::uint64_t ptr_constructor_key
Definition: matlab.h:64
void checkArguments(const string &name, int nargout, int nargin, int expected)
Definition: matlab.h:113
Vector2 Point2
Definition: Point2.h:32
Definition: numpy.h:680
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::Point3 unwrap< gtsam::Point3 >(const mxArray *array)
Definition: matlab.h:383
Definition: BFloat16.h:88
string unwrap< string >(const mxArray *array)
Definition: matlab.h:283
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
size_t unwrap< size_t >(const mxArray *array)
Definition: matlab.h:335
unsigned char unwrap< unsigned char >(const mxArray *array)
Definition: matlab.h:321
mxArray * wrap< size_t >(const size_t &value)
Definition: matlab.h:164
const char * c_str(Args &&...args)
Definition: internals.h:524
T unwrap(const mxArray *array)
Definition: matlab.h:256
T unwrap_enum(const mxArray *array)
Unwrap from matlab array to C++ enum type.
Definition: matlab.h:265
mxArray * create_object(const std::string &classname, void *pointer, bool isVirtual, const char *rttiName)
Definition: matlab.h:430
Eigen::VectorXd Vector
Definition: Vector.h:38
std::shared_ptr< Class > unwrap_shared_ptr(const mxArray *obj, const string &propertyName)
Definition: matlab.h:501
double unwrap< double >(const mxArray *array)
Definition: matlab.h:342
Values result
virtual int overflow(int c=EOF)
Definition: matlab.h:101
Definition: pytypes.h:1403
signed __int64 int64_t
Definition: ms_stdint.h:94
Array< int, Dynamic, 1 > v
mxArray * wrap_Vector(const gtsam::Vector &v)
Definition: matlab.h:185
unsigned __int64 uint64_t
Definition: ms_stdint.h:95
Eigen::Triplet< double > T
int data[]
mxArray * wrap< char >(const char &value)
Definition: matlab.h:140
virtual std::streamsize xsputn(const char *s, std::streamsize n)
Definition: matlab.h:97
RealScalar s
#define NULL
Definition: ccolamd.c:609
Class * unwrap_ptr(const mxArray *obj, const string &propertyName)
Definition: matlab.h:513
mxArray * wrap< unsigned char >(const unsigned char &value)
Definition: matlab.h:148
gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray *array)
Definition: matlab.h:401
mxArray * wrap< int >(const int &value)
Definition: matlab.h:172
typedef and functions to augment Eigen&#39;s VectorXd
mxArray * wrap< double >(const double &value)
Definition: matlab.h:180
#define mxUINT32OR64_CLASS
Definition: matlab.h:58
void checkScalar(const mxArray *array, const char *str)
Definition: matlab.h:87
mxArray * wrap(const Class &value)
Definition: matlab.h:126
mxArray * wrap< string >(const string &value)
Definition: matlab.h:134
3D Point
gtsam::Point2 unwrap< gtsam::Point2 >(const mxArray *array)
Definition: matlab.h:366
mxArray * wrap_enum(const T x, const std::string &classname)
Wrap the C++ enum to Matlab mxArray.
Definition: matlab.h:236
Annotation for function names.
Definition: attr.h:48
Vector3 Point3
Definition: Point3.h:38
mxArray * wrap< bool >(const bool &value)
Definition: matlab.h:156
bool unwrap< bool >(const mxArray *array)
Definition: matlab.h:307
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
2D Point
mxArray * scalar(mxClassID classid)
Definition: matlab.h:82
mxArray * wrap_Matrix(const gtsam::Matrix &A)
Definition: matlab.h:212
void error(const char *str)
Definition: matlab.h:78
Definition: matlab.h:95
std::ptrdiff_t j
T myGetScalar(const mxArray *array)
Definition: matlab.h:293


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:46