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);