1 #include <gtwrap/matlab.h> 
    4 #include <folder/path/to/Test.h> 
   45   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
   47   bool anyDeleted = 
false;
 
  129       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n" 
  130       "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" 
  131       "module, so that your recompiled module is used instead of the old one." << endl;
 
  132   std::cout.rdbuf(outbuf);
 
  136   const mxArray *alreadyCreated = mexGetVariablePtr(
"global", 
"gtsam_class_rttiRegistry_created");
 
  137   if(!alreadyCreated) {
 
  138     std::map<std::string, std::string> types;
 
  140     types.insert(std::make_pair(
typeid(HessianFactor).
name(), 
"HessianFactor"));
 
  143     mxArray *registry = mexGetVariable(
"global", 
"gtsamwrap_rttiRegistry");
 
  145       registry = mxCreateStructMatrix(1, 1, 0, 
NULL);
 
  146     typedef std::pair<std::string, std::string> StringPair;
 
  147     for(
const StringPair& rtti_matlab: types) {
 
  148       int fieldId = mxAddField(registry, rtti_matlab.first.c_str());
 
  150         mexErrMsgTxt(
"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
 
  152       mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str());
 
  153       mxSetFieldByNumber(registry, 0, fieldId, matlabName);
 
  155     if(mexPutVariable(
"global", 
"gtsamwrap_rttiRegistry", registry) != 0) {
 
  156       mexErrMsgTxt(
"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
 
  158     mxDestroyArray(registry);
 
  160     mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL);
 
  161     if(mexPutVariable(
"global", 
"gtsam_class_rttiRegistry_created", newAlreadyCreated) != 0) {
 
  162       mexErrMsgTxt(
"gtsam wrap:  Error indexing RTTI types, inheritance will not work correctly");
 
  164     mxDestroyArray(newAlreadyCreated);
 
  171   typedef std::shared_ptr<FunRange> Shared;
 
  173   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  180   typedef std::shared_ptr<FunRange> Shared;
 
  182   Shared *
self = 
new Shared(
new FunRange());
 
  185   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  190   typedef std::shared_ptr<FunRange> Shared;
 
  192   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  193   Collector_FunRange::iterator item;
 
  204   auto obj = unwrap_shared_ptr<FunRange>(in[0], 
"ptr_FunRange");
 
  218   typedef std::shared_ptr<Fun<double>> Shared;
 
  220   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  226   typedef std::shared_ptr<Fun<double>> Shared;
 
  228   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  229   Collector_FunDouble::iterator item;
 
  239   checkArguments(
"multiTemplatedMethodStringSize_t",nargout,nargin-1,3);
 
  240   auto obj = unwrap_shared_ptr<Fun<double>>(in[0], 
"ptr_FunDouble");
 
  250   auto obj = unwrap_shared_ptr<Fun<double>>(in[0], 
"ptr_FunDouble");
 
  257   auto obj = unwrap_shared_ptr<Fun<double>>(in[0], 
"ptr_FunDouble");
 
  265   checkArguments(
"Fun<double>.staticMethodWithThis",nargout,nargin,0);
 
  271   checkArguments(
"Fun<double>.templatedStaticMethodInt",nargout,nargin,1);
 
  279   typedef std::shared_ptr<Test> Shared;
 
  281   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  288   typedef std::shared_ptr<Test> Shared;
 
  290   Shared *
self = 
new Shared(
new Test());
 
  293   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  299   typedef std::shared_ptr<Test> Shared;
 
  302   Matrix b = unwrap< Matrix >(in[1]);
 
  303   Shared *
self = 
new Shared(
new Test(
a,
b));
 
  306   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  311   typedef std::shared_ptr<Test> Shared;
 
  313   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  314   Collector_Test::iterator item;
 
  325   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  327   obj->arg_EigenConstRef(
value);
 
  333   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  334   auto pairResult = obj->create_MixedPtrs();
 
  342   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  343   auto pairResult = obj->create_ptrs();
 
  351   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  358   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  365   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  366   gtsam::KeyFormatter& keyFormatter = *unwrap_shared_ptr< gtsam::KeyFormatter >(in[1], 
"ptr_gtsamKeyFormatter");
 
  373   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  380   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  387   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  390   std::shared_ptr<Point2> shared(obj->return_Point2Ptr(
value));
 
  398   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  399   std::shared_ptr<Test> 
value = unwrap_shared_ptr< Test >(in[1], 
"ptr_Test");
 
  406   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  407   std::shared_ptr<Test> 
value = unwrap_shared_ptr< Test >(in[1], 
"ptr_Test");
 
  414   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  422   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  430   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  431   Test& 
t = *unwrap_shared_ptr< Test >(in[1], 
"ptr_Test");
 
  438   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  446   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  448   out[0] = wrap< Matrix >(obj->return_matrix1(
value));
 
  454   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  456   out[0] = wrap< Matrix >(obj->return_matrix2(
value));
 
  462   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  463   Vector v = unwrap< Vector >(in[1]);
 
  464   Matrix A = unwrap< Matrix >(in[2]);
 
  465   auto pairResult = obj->return_pair(
v,
A);
 
  466   out[0] = wrap< Vector >(pairResult.first);
 
  467   out[1] = wrap< Matrix >(pairResult.second);
 
  473   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  474   Vector v = unwrap< Vector >(in[1]);
 
  475   auto pairResult = obj->return_pair(
v);
 
  476   out[0] = wrap< Vector >(pairResult.first);
 
  477   out[1] = wrap< Matrix >(pairResult.second);
 
  483   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  484   std::shared_ptr<Test> 
p1 = unwrap_shared_ptr< Test >(in[1], 
"ptr_Test");
 
  485   std::shared_ptr<Test> 
p2 = unwrap_shared_ptr< Test >(in[2], 
"ptr_Test");
 
  486   auto pairResult = obj->return_ptrs(
p1,
p2);
 
  494   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  502   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  510   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  512   out[0] = wrap< Vector >(obj->return_vector1(
value));
 
  518   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  520   out[0] = wrap< Vector >(obj->return_vector2(
value));
 
  526   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  527   std::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], 
"ptr_stdvectorTest");
 
  528   obj->set_container(*container);
 
  534   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  535   std::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], 
"ptr_stdvectorTest");
 
  536   obj->set_container(*container);
 
  542   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  543   std::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], 
"ptr_stdvectorTest");
 
  544   obj->set_container(*container);
 
  550   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  557   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  558   std::shared_ptr<gtsam::noiseModel::Base> model_ptr = unwrap_shared_ptr< gtsam::noiseModel::Base >(in[1], 
"ptr_gtsamnoiseModelBase");
 
  559   obj->model_ptr = *model_ptr;
 
  565   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  572   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  580   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  587   auto obj = unwrap_shared_ptr<Test>(in[0], 
"ptr_Test");
 
  595   typedef std::shared_ptr<PrimitiveRef<double>> Shared;
 
  597   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  604   typedef std::shared_ptr<PrimitiveRef<double>> Shared;
 
  606   Shared *
self = 
new Shared(
new PrimitiveRef<double>());
 
  609   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  614   typedef std::shared_ptr<PrimitiveRef<double>> Shared;
 
  616   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  617   Collector_PrimitiveRefDouble::iterator item;
 
  635   typedef std::shared_ptr<MyVector<3>> Shared;
 
  637   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  644   typedef std::shared_ptr<MyVector<3>> Shared;
 
  646   Shared *
self = 
new Shared(
new MyVector<3>());
 
  649   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  654   typedef std::shared_ptr<MyVector<3>> Shared;
 
  656   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  657   Collector_MyVector3::iterator item;
 
  668   typedef std::shared_ptr<MyVector<12>> Shared;
 
  670   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  677   typedef std::shared_ptr<MyVector<12>> Shared;
 
  679   Shared *
self = 
new Shared(
new MyVector<12>());
 
  682   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  687   typedef std::shared_ptr<MyVector<12>> Shared;
 
  689   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  690   Collector_MyVector12::iterator item;
 
  701   typedef std::shared_ptr<MultipleTemplates<int, double>> Shared;
 
  703   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  709   typedef std::shared_ptr<MultipleTemplates<int, double>> Shared;
 
  710   checkArguments(
"delete_MultipleTemplatesIntDouble",nargout,nargin,1);
 
  711   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  712   Collector_MultipleTemplatesIntDouble::iterator item;
 
  723   typedef std::shared_ptr<MultipleTemplates<int, float>> Shared;
 
  725   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  731   typedef std::shared_ptr<MultipleTemplates<int, float>> Shared;
 
  732   checkArguments(
"delete_MultipleTemplatesIntFloat",nargout,nargin,1);
 
  733   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  734   Collector_MultipleTemplatesIntFloat::iterator item;
 
  745   typedef std::shared_ptr<ForwardKinematics> Shared;
 
  747   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  754   typedef std::shared_ptr<ForwardKinematics> Shared;
 
  756   gtdynamics::Robot& robot = *unwrap_shared_ptr< gtdynamics::Robot >(in[0], 
"ptr_gtdynamicsRobot");
 
  757   string& start_link_name = *unwrap_shared_ptr< string >(in[1], 
"ptr_string");
 
  758   string& end_link_name = *unwrap_shared_ptr< string >(in[2], 
"ptr_string");
 
  759   gtsam::Values& joint_angles = *unwrap_shared_ptr< gtsam::Values >(in[3], 
"ptr_gtsamValues");
 
  760   gtsam::Pose3& l2Tp = *unwrap_shared_ptr< gtsam::Pose3 >(in[4], 
"ptr_gtsamPose3");
 
  761   Shared *
self = 
new Shared(
new ForwardKinematics(robot,start_link_name,end_link_name,joint_angles,l2Tp));
 
  764   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  770   typedef std::shared_ptr<ForwardKinematics> Shared;
 
  772   gtdynamics::Robot& robot = *unwrap_shared_ptr< gtdynamics::Robot >(in[0], 
"ptr_gtdynamicsRobot");
 
  773   string& start_link_name = *unwrap_shared_ptr< string >(in[1], 
"ptr_string");
 
  774   string& end_link_name = *unwrap_shared_ptr< string >(in[2], 
"ptr_string");
 
  775   gtsam::Values& joint_angles = *unwrap_shared_ptr< gtsam::Values >(in[3], 
"ptr_gtsamValues");
 
  776   Shared *
self = 
new Shared(
new ForwardKinematics(robot,start_link_name,end_link_name,joint_angles,
gtsam::Pose3()));
 
  779   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  784   typedef std::shared_ptr<ForwardKinematics> Shared;
 
  786   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  787   Collector_ForwardKinematics::iterator item;
 
  798   typedef std::shared_ptr<TemplatedConstructor> Shared;
 
  800   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  807   typedef std::shared_ptr<TemplatedConstructor> Shared;
 
  809   Shared *
self = 
new Shared(
new TemplatedConstructor());
 
  812   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  818   typedef std::shared_ptr<TemplatedConstructor> Shared;
 
  820   string& 
arg = *unwrap_shared_ptr< string >(in[0], 
"ptr_string");
 
  821   Shared *
self = 
new Shared(
new TemplatedConstructor(
arg));
 
  824   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  830   typedef std::shared_ptr<TemplatedConstructor> Shared;
 
  833   Shared *
self = 
new Shared(
new TemplatedConstructor(
arg));
 
  836   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  842   typedef std::shared_ptr<TemplatedConstructor> Shared;
 
  845   Shared *
self = 
new Shared(
new TemplatedConstructor(
arg));
 
  848   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  853   typedef std::shared_ptr<TemplatedConstructor> Shared;
 
  855   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  856   Collector_TemplatedConstructor::iterator item;
 
  867   typedef std::shared_ptr<FastSet> Shared;
 
  869   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  876   typedef std::shared_ptr<FastSet> Shared;
 
  878   Shared *
self = 
new Shared(
new FastSet());
 
  881   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  886   typedef std::shared_ptr<FastSet> Shared;
 
  888   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  889   Collector_FastSet::iterator item;
 
  900   typedef std::shared_ptr<HessianFactor> Shared;
 
  902   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  905   typedef std::shared_ptr<gtsam::GaussianFactor> SharedBase;
 
  907   *
reinterpret_cast<SharedBase**
>(mxGetData(
out[0])) = 
new SharedBase(*
self);
 
  912   typedef std::shared_ptr<HessianFactor> Shared;
 
  913   std::shared_ptr<void> *asVoid = *
reinterpret_cast<std::shared_ptr<void>**
> (mxGetData(in[0]));
 
  915   Shared *
self = 
new Shared(std::static_pointer_cast<HessianFactor>(*asVoid));
 
  916   *
reinterpret_cast<Shared**
>(mxGetData(
out[0])) = 
self;
 
  922   typedef std::shared_ptr<HessianFactor> Shared;
 
  924   gtsam::KeyVector& js = *unwrap_shared_ptr< gtsam::KeyVector >(in[0], 
"ptr_gtsamKeyVector");
 
  925   std::vector<Matrix>& Gs = *unwrap_shared_ptr< std::vector<Matrix> >(in[1], 
"ptr_stdvectorMatrix");
 
  926   std::vector<Vector>& gs = *unwrap_shared_ptr< std::vector<Vector> >(in[2], 
"ptr_stdvectorVector");
 
  928   Shared *
self = 
new Shared(
new HessianFactor(js,Gs,gs,
f));
 
  931   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  933   typedef std::shared_ptr<gtsam::GaussianFactor> SharedBase;
 
  935   *
reinterpret_cast<SharedBase**
>(mxGetData(
out[1])) = 
new SharedBase(*
self);
 
  940   typedef std::shared_ptr<HessianFactor> Shared;
 
  942   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  943   Collector_HessianFactor::iterator item;
 
  954   typedef std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
 
  956   Shared *
self = *
reinterpret_cast<Shared**
> (mxGetData(in[0]));
 
  963   typedef std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
 
  968   std::shared_ptr<gtsam::noiseModel::Base> noiseModel = unwrap_shared_ptr< gtsam::noiseModel::Base >(in[3], 
"ptr_gtsamnoiseModelBase");
 
  972   *
reinterpret_cast<Shared**
> (mxGetData(
out[0])) = 
self;
 
  977   typedef std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
 
  979   Shared *
self = *
reinterpret_cast<Shared**
>(mxGetData(in[0]));
 
  980   Collector_MyFactorPosePoint2::iterator item;
 
  991   auto obj = unwrap_shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>(in[0], 
"ptr_MyFactorPosePoint2");
 
  992   string& 
s = *unwrap_shared_ptr< string >(in[1], 
"ptr_string");
 
  993   gtsam::KeyFormatter& keyFormatter = *unwrap_shared_ptr< gtsam::KeyFormatter >(in[2], 
"ptr_gtsamKeyFormatter");
 
  994   obj->print(
s,keyFormatter);
 
 1000   auto obj = unwrap_shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>(in[0], 
"ptr_MyFactorPosePoint2");
 
 1001   string& 
s = *unwrap_shared_ptr< string >(in[1], 
"ptr_string");
 
 1008   auto obj = unwrap_shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>(in[0], 
"ptr_MyFactorPosePoint2");
 
 1016   std::streambuf *outbuf = std::cout.rdbuf(&mout);
 
 1283   } 
catch(
const std::exception& 
e) {
 
 1284     mexErrMsgTxt((
"Exception from gtsam:\n" + std::string(
e.what()) + 
"\n").c_str());
 
 1287   std::cout.rdbuf(outbuf);