diff --git a/.cproject b/.cproject
index e15a51f96..860ea1cba 100644
--- a/.cproject
+++ b/.cproject
@@ -375,14 +375,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -409,6 +401,7 @@
make
+
tests/testBayesTree.run
true
false
@@ -416,6 +409,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -463,6 +457,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -470,6 +465,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -477,6 +473,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -492,11 +489,20 @@
make
+
tests/testBayesTree
true
false
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -523,7 +529,6 @@
make
-
testGraph.run
true
false
@@ -595,7 +600,6 @@
make
-
testInference.run
true
false
@@ -603,7 +607,6 @@
make
-
testGaussianFactor.run
true
false
@@ -611,7 +614,6 @@
make
-
testJunctionTree.run
true
false
@@ -619,7 +621,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -627,7 +628,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -721,7 +721,15 @@
true
true
-
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
make
-j2
check
@@ -729,6 +737,14 @@
true
true
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
make
-j2
@@ -769,6 +785,14 @@
true
true
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
make
-j2
@@ -825,14 +849,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
make
-j2
@@ -841,6 +857,14 @@
true
true
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
make
-j2
@@ -1083,6 +1107,7 @@
make
+
testErrors.run
true
false
@@ -1482,7 +1507,6 @@
make
-
testSimulated2DOriented.run
true
false
@@ -1522,7 +1546,6 @@
make
-
testSimulated2D.run
true
false
@@ -1530,7 +1553,6 @@
make
-
testSimulated3D.run
true
false
@@ -1722,7 +1744,6 @@
make
-
tests/testGaussianISAM2
true
false
@@ -1744,6 +1765,46 @@
true
true
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ dist
+ true
+ true
+ true
+
make
-j2
@@ -1840,54 +1901,6 @@
true
true
-
- make
- -j2
- install
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- dist
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
make
-j2
@@ -1928,6 +1941,14 @@
true
true
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
diff --git a/Makefile.am b/Makefile.am
index 46974a0a0..6c050e4f8 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -16,6 +16,10 @@ include aminclude.am
# All the sub-directories that need to be built
SUBDIRS = CppUnitLite gtsam tests examples
+if ENABLE_BUILD_TOOLBOX
+SUBDIRS += wrap
+endif
+
# Add these files to make sure they're in the distribution
EXTRA_DIST = autogen.sh configure.ac COPYING INSTALL LGPL LICENSE README THANKS USAGE doc
diff --git a/configure.ac b/configure.ac
index 2c6624a42..15073a4b9 100644
--- a/configure.ac
+++ b/configure.ac
@@ -7,6 +7,7 @@ AM_INIT_AUTOMAKE(gtsam, 0.9.3)
AC_CONFIG_MACRO_DIR([m4])
AC_CONFIG_HEADER([config.h])
AC_CONFIG_SRCDIR([CppUnitLite/Test.cpp])
+AC_CONFIG_SRCDIR([wrap/wrap.cpp])
AC_CONFIG_SRCDIR([gtsam/base/DSFVector.cpp])
AC_CONFIG_SRCDIR([gtsam/geometry/Cal3_S2.cpp])
AC_CONFIG_SRCDIR([gtsam/inference/SymbolicFactorGraph.cpp])
@@ -114,7 +115,26 @@ AC_CHECK_FUNCS([pow sqrt])
# Check for boost
AX_BOOST_BASE([1.40])
-# ask for toolbox directory
+# enable matlab toolbox generation
+AC_ARG_ENABLE([build_toolbox],
+ [ --enable-build-toolbox Enable building of the Matlab toolbox],
+ [case "${enableval}" in
+ yes) build_toolbox=true ;;
+ no) build_toolbox=false ;;
+ *) AC_MSG_ERROR([bad value ${enableval} for --enable-build-toolbox]) ;;
+ esac],[build_toolbox=false])
+
+AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue])
+
+# Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix
+AC_ARG_WITH([toolbox],
+ [AS_HELP_STRING([--with-toolbox],
+ [specify the matlab toolbox directory for installation])],
+ [toolbox=$withval],
+ [toolbox=$prefix])
+AC_SUBST([toolbox])
+
+#Original version
#AC_ARG_WITH([toolbox],
# [AS_HELP_STRING([--with-toolbox],
# [specify the matlab toolbox directory for installation (mandatory)])],
@@ -122,9 +142,9 @@ AX_BOOST_BASE([1.40])
# [AC_MSG_FAILURE(
# [--with-toolbox has to be specified])
# ])
-#AC_SUBST([toolbox])
AC_CONFIG_FILES([CppUnitLite/Makefile \
+wrap/Makefile \
gtsam/3rdparty/Makefile \
gtsam/base/Makefile \
gtsam/geometry/Makefile \
diff --git a/gtsam-broken.h b/gtsam-broken.h
new file mode 100644
index 000000000..9cf624ae0
--- /dev/null
+++ b/gtsam-broken.h
@@ -0,0 +1,270 @@
+// These are considered to be broken and will be added back as they start working
+// It's assumed that there have been interface changes that might break this
+
+class SharedGaussian {
+ SharedGaussian(Matrix covariance);
+ SharedGaussian(Vector sigmas);
+};
+
+class SharedDiagonal {
+ SharedDiagonal(Vector sigmas);
+};
+
+class Ordering {
+ Ordering();
+ void print(string s) const;
+ bool equals(const Ordering& ord, double tol) const;
+ void push_back(string s);
+};
+
+
+class VectorValues {
+ VectorValues();
+ VectorValues(size_t nVars, size_t varDim);
+ void print(string s) const;
+ bool equals(const VectorValues& expected, double tol) const;
+ size_t size() const;
+};
+
+class GaussianFactor {
+ void print(string s) const;
+ bool equals(const GaussianFactor& lf, double tol) const;
+ bool empty() const;
+ Vector getb() const;
+ double error(const VectorValues& c) const;
+};
+
+class GaussianFactorSet {
+ GaussianFactorSet();
+ void push_back(GaussianFactor* factor);
+};
+
+class GaussianConditional {
+ GaussianConditional();
+ void print(string s) const;
+ bool equals(const GaussianConditional &cg, double tol) const;
+ Vector solve(const VectorValues& x);
+};
+
+class GaussianBayesNet {
+ GaussianBayesNet();
+ void print(string s) const;
+ bool equals(const GaussianBayesNet& cbn, double tol) const;
+ void push_back(GaussianConditional* conditional);
+ void push_front(GaussianConditional* conditional);
+};
+
+class GaussianFactorGraph {
+ GaussianFactorGraph();
+ void print(string s) const;
+ bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
+
+ size_t size() const;
+ void push_back(GaussianFactor* ptr_f);
+ double error(const VectorValues& c) const;
+ double probPrime(const VectorValues& c) const;
+ void combine(const GaussianFactorGraph& lfg);
+};
+
+class Simulated2DValues {
+ Simulated2DValues();
+ void print(string s) const;
+ void insertPose(int i, const Point2& p);
+ void insertPoint(int j, const Point2& p);
+ int nrPoses() const;
+ int nrPoints() const;
+ Point2* pose(int i);
+ Point2* point(int j);
+};
+
+class Simulated2DOrientedValues {
+ Simulated2DOrientedValues();
+ void print(string s) const;
+ void insertPose(int i, const Pose2& p);
+ void insertPoint(int j, const Point2& p);
+ int nrPoses() const;
+ int nrPoints() const;
+ Pose2* pose(int i);
+ Point2* point(int j);
+};
+
+class Simulated2DPosePrior {
+ Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
+ void print(string s) const;
+ double error(const Simulated2DValues& c) const;
+};
+
+class Simulated2DOrientedPosePrior {
+ Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
+ void print(string s) const;
+ double error(const Simulated2DOrientedValues& c) const;
+};
+
+class Simulated2DPointPrior {
+ Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
+ void print(string s) const;
+ double error(const Simulated2DValues& c) const;
+};
+
+class Simulated2DOdometry {
+ Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
+ void print(string s) const;
+ double error(const Simulated2DValues& c) const;
+};
+
+class Simulated2DOrientedOdometry {
+ Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
+ void print(string s) const;
+ double error(const Simulated2DOrientedValues& c) const;
+};
+
+class Simulated2DMeasurement {
+ Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
+ void print(string s) const;
+ double error(const Simulated2DValues& c) const;
+};
+
+// These are currently broken
+// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class
+// We also have to solve the shared pointer mess to avoid duplicate methods
+
+class GaussianFactor {
+ GaussianFactor(string key1,
+ Matrix A1,
+ Vector b_in,
+ const SharedDiagonal& model);
+ GaussianFactor(string key1,
+ Matrix A1,
+ string key2,
+ Matrix A2,
+ Vector b_in,
+ const SharedDiagonal& model);
+ GaussianFactor(string key1,
+ Matrix A1,
+ string key2,
+ Matrix A2,
+ string key3,
+ Matrix A3,
+ Vector b_in,
+ const SharedDiagonal& model);
+ bool involves(string key) const;
+ Matrix getA(string key) const;
+ pair matrix(const Ordering& ordering) const;
+ pair eliminate(string key) const;
+};
+
+class GaussianConditional {
+ GaussianConditional(string key,
+ Vector d,
+ Matrix R,
+ Vector sigmas);
+ GaussianConditional(string key,
+ Vector d,
+ Matrix R,
+ string name1,
+ Matrix S,
+ Vector sigmas);
+ GaussianConditional(string key,
+ Vector d,
+ Matrix R,
+ string name1,
+ Matrix S,
+ string name2,
+ Matrix T,
+ Vector sigmas);
+ void add(string key, Matrix S);
+};
+
+class GaussianFactorGraph {
+ GaussianConditional* eliminateOne(string key);
+ GaussianBayesNet* eliminate_(const Ordering& ordering);
+ VectorValues* optimize_(const Ordering& ordering);
+ pair matrix(const Ordering& ordering) const;
+ Matrix sparse(const Ordering& ordering) const;
+ VectorValues* steepestDescent_(const VectorValues& x0) const;
+ VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
+};
+
+
+class Pose2Values{
+ Pose2Values();
+ Pose2 get(string key) const;
+ void insert(string name, const Pose2& val);
+ void print(string s) const;
+ void clear();
+ int size();
+};
+
+class Pose2Factor {
+ Pose2Factor(string key1, string key2,
+ const Pose2& measured, Matrix measurement_covariance);
+ void print(string name) const;
+ double error(const Pose2Values& c) const;
+ size_t size() const;
+ GaussianFactor* linearize(const Pose2Values& config) const;
+};
+
+class Pose2Graph{
+ Pose2Graph();
+ void print(string s) const;
+ GaussianFactorGraph* linearize_(const Pose2Values& config) const;
+ void push_back(Pose2Factor* factor);
+};
+
+class Ordering{
+ Ordering(string key);
+ Ordering subtract(const Ordering& keys) const;
+ void unique ();
+ void reverse ();
+};
+
+
+class SymbolicFactor{
+ SymbolicFactor(const Ordering& keys);
+ void print(string s) const;
+};
+
+
+class VectorValues {
+ void insert(string name, Vector val);
+ Vector get(string name) const;
+ bool contains(string name) const;
+};
+
+class Simulated2DPosePrior {
+ GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Simulated2DOrientedPosePrior {
+ GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
+};
+
+class Simulated2DPointPrior {
+ GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Simulated2DOdometry {
+ GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Simulated2DOrientedOdometry {
+ GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
+};
+
+class Simulated2DMeasurement {
+ GaussianFactor* linearize(const Simulated2DValues& config) const;
+};
+
+class Pose2SLAMOptimizer {
+ Pose2SLAMOptimizer(string dataset_name);
+ void print(string s) const;
+ void update(Vector x) const;
+ Vector optimize() const;
+ double error() const;
+ Matrix a1() const;
+ Matrix a2() const;
+ Vector b1() const;
+ Vector b2() const;
+};
+
+
diff --git a/gtsam.h b/gtsam.h
new file mode 100644
index 000000000..330b39f28
--- /dev/null
+++ b/gtsam.h
@@ -0,0 +1,37 @@
+class Point2 {
+ Point2();
+ Point2(double x, double y);
+ void print(string s) const;
+ double x();
+ double y();
+};
+
+class Point3 {
+ Point3();
+ Point3(double x, double y, double z);
+ Point3(Vector v);
+ void print(string s) const;
+ Vector vector() const;
+ double x();
+ double y();
+ double z();
+};
+
+class Pose2 {
+ Pose2();
+ Pose2(const Pose2& pose);
+ Pose2(double x, double y, double theta);
+ Pose2(double theta, const Point2& t);
+ Pose2(const Rot2& r, const Point2& t);
+ void print(string s) const;
+ bool equals(const Pose2& pose, double tol) const;
+ double x() const;
+ double y() const;
+ double theta() const;
+ size_t dim() const;
+ Pose2 expmap(const Vector& v) const;
+ Vector logmap(const Pose2& pose) const;
+ Point2 t() const;
+ Rot2 r() const;
+};
+
diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am
index 42db68d6f..42716f967 100644
--- a/gtsam/slam/Makefile.am
+++ b/gtsam/slam/Makefile.am
@@ -11,15 +11,26 @@ sources =
check_PROGRAMS =
# simulated2D example
-# headers += Simulated2DValues.h
headers += simulated2DConstraints.h
sources += simulated2D.cpp smallExample.cpp
check_PROGRAMS += tests/testSimulated2D
+# MATLAB Wrap headers for simulated2D
+headers += Simulated2DMeasurement.h
+headers += Simulated2DOdometry.h
+headers += Simulated2DPointPrior.h
+headers += Simulated2DPosePrior.h
+headers += Simulated2DValues.h
+
# simulated2DOriented example
sources += simulated2DOriented.cpp
check_PROGRAMS += tests/testSimulated2DOriented
+# MATLAB Wrap headers for simulated2DOriented
+headers += Simulated2DOrientedOdometry.h
+headers += Simulated2DOrientedPosePrior.h
+headers += Simulated2DOrientedValues.h
+
# simulated3D example
sources += Simulated3D.cpp
check_PROGRAMS += tests/testSimulated3D
diff --git a/gtsam/slam/Simulated2DMeasurement.h b/gtsam/slam/Simulated2DMeasurement.h
new file mode 100644
index 000000000..b85351fc1
--- /dev/null
+++ b/gtsam/slam/Simulated2DMeasurement.h
@@ -0,0 +1,29 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DMeasurement.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ typedef simulated2D::Measurement Simulated2DMeasurement;
+
+}
+
diff --git a/gtsam/slam/Simulated2DOdometry.h b/gtsam/slam/Simulated2DOdometry.h
new file mode 100644
index 000000000..498e23fcc
--- /dev/null
+++ b/gtsam/slam/Simulated2DOdometry.h
@@ -0,0 +1,29 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DOdometry.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ typedef simulated2D::Odometry Simulated2DOdometry;
+
+}
+
diff --git a/gtsam/slam/Simulated2DOrientedOdometry.h b/gtsam/slam/Simulated2DOrientedOdometry.h
new file mode 100644
index 000000000..dd5afac7b
--- /dev/null
+++ b/gtsam/slam/Simulated2DOrientedOdometry.h
@@ -0,0 +1,29 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DOrientedOdometry.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Kai Ni
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry;
+
+}
+
diff --git a/gtsam/slam/Simulated2DOrientedPosePrior.h b/gtsam/slam/Simulated2DOrientedPosePrior.h
new file mode 100644
index 000000000..be8407b6c
--- /dev/null
+++ b/gtsam/slam/Simulated2DOrientedPosePrior.h
@@ -0,0 +1,30 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DOrientedPosePrior.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Kai Ni
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ /** Create a prior on a pose Point2 with key 'x1' etc... */
+ typedef simulated2DOriented::GenericPosePrior Simulated2DOrientedPosePrior;
+
+}
+
diff --git a/gtsam/slam/Simulated2DOrientedValues.h b/gtsam/slam/Simulated2DOrientedValues.h
new file mode 100644
index 000000000..20bd7bffa
--- /dev/null
+++ b/gtsam/slam/Simulated2DOrientedValues.h
@@ -0,0 +1,60 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DValues.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include
+
+namespace gtsam {
+
+ class Simulated2DOrientedValues: public simulated2DOriented::Values {
+ public:
+ typedef boost::shared_ptr sharedPoint;
+ typedef boost::shared_ptr sharedPose;
+
+ Simulated2DOrientedValues() {
+ }
+
+ void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) {
+ insert(i, p);
+ }
+
+ void insertPoint(const simulated2DOriented::PointKey& j, const Point2& p) {
+ insert(j, p);
+ }
+
+ int nrPoses() const {
+ return this->first_.size();
+ }
+
+ int nrPoints() const {
+ return this->second_.size();
+ }
+
+ sharedPose pose(const simulated2DOriented::PoseKey& i) {
+ return sharedPose(new Pose2((*this)[i]));
+ }
+
+ sharedPoint point(const simulated2DOriented::PointKey& j) {
+ return sharedPoint(new Point2((*this)[j]));
+ }
+
+ };
+
+} // namespace gtsam
+
diff --git a/gtsam/slam/Simulated2DPointPrior.h b/gtsam/slam/Simulated2DPointPrior.h
new file mode 100644
index 000000000..3610f9c1b
--- /dev/null
+++ b/gtsam/slam/Simulated2DPointPrior.h
@@ -0,0 +1,30 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DPointPrior.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ /** Create a prior on a landmark Point2 with key 'l1' etc... */
+ typedef simulated2D::GenericPrior Simulated2DPointPrior;
+
+}
+
diff --git a/gtsam/slam/Simulated2DPosePrior.h b/gtsam/slam/Simulated2DPosePrior.h
new file mode 100644
index 000000000..a0b123243
--- /dev/null
+++ b/gtsam/slam/Simulated2DPosePrior.h
@@ -0,0 +1,30 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DPosePrior.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ /** Create a prior on a pose Point2 with key 'x1' etc... */
+ typedef simulated2D::GenericPrior Simulated2DPosePrior;
+
+}
+
diff --git a/gtsam/slam/Simulated2DValues.h b/gtsam/slam/Simulated2DValues.h
new file mode 100644
index 000000000..1d11cd4a4
--- /dev/null
+++ b/gtsam/slam/Simulated2DValues.h
@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Simulated2DValues.h
+ *
+ * Re-created on Feb 22, 2010 for compatibility with MATLAB
+ * Author: Frank Dellaert
+ */
+
+#pragma once
+
+#include
+
+namespace gtsam {
+
+ class Simulated2DValues: public simulated2D::Values {
+ public:
+ typedef boost::shared_ptr sharedPoint;
+
+ Simulated2DValues() {
+ }
+
+ void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
+ insert(i, p);
+ }
+
+ void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
+ insert(j, p);
+ }
+
+ int nrPoses() const {
+ return this->first_.size();
+ }
+
+ int nrPoints() const {
+ return this->second_.size();
+ }
+
+ sharedPoint pose(const simulated2D::PoseKey& i) {
+ return sharedPoint(new Point2((*this)[i]));
+ }
+
+ sharedPoint point(const simulated2D::PointKey& j) {
+ return sharedPoint(new Point2((*this)[j]));
+ }
+
+ };
+
+} // namespace gtsam
+
diff --git a/myconfigure.matlab b/myconfigure.matlab
new file mode 100755
index 000000000..d9bcd2bb7
--- /dev/null
+++ b/myconfigure.matlab
@@ -0,0 +1 @@
+../configure --prefix=$HOME --with-toolbox=$HOME/borg/gtsam/toolbox --enable-build-toolbox CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install
diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp
new file mode 100644
index 000000000..e44b3033c
--- /dev/null
+++ b/wrap/Argument.cpp
@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Argument.ccp
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+#include
+#include
+
+#include "Argument.h"
+
+using namespace std;
+
+/* ************************************************************************* */
+void Argument::matlab_unwrap(ofstream& ofs,
+ const string& matlabName)
+{
+ // the templated unwrap function returns a pointer
+ // example: double tol = unwrap< double >(in[2]);
+ ofs << " ";
+
+ if (is_ptr)
+ ofs << "shared_ptr<" << type << "> " << name << " = unwrap_shared_ptr< ";
+ else if (is_ref)
+ ofs << type << "& " << name << " = *unwrap_shared_ptr< ";
+ else
+ ofs << type << " " << name << " = unwrap< ";
+
+ ofs << type << " >(" << matlabName;
+ if (is_ptr || is_ref) ofs << ", \"" << type << "\"";
+ ofs << ");" << endl;
+}
+
+/* ************************************************************************* */
+string ArgumentList::types() {
+ string str;
+ bool first=true;
+ BOOST_FOREACH(Argument arg, *this) {
+ if (!first) str += ","; str += arg.type; first=false;
+ }
+ return str;
+}
+
+/* ************************************************************************* */
+string ArgumentList::signature() {
+ string str;
+ BOOST_FOREACH(Argument arg, *this)
+ str += arg.type[0];
+ return str;
+}
+
+/* ************************************************************************* */
+string ArgumentList::names() {
+ string str;
+ bool first=true;
+ BOOST_FOREACH(Argument arg, *this) {
+ if (!first) str += ","; str += arg.name; first=false;
+ }
+ return str;
+}
+
+/* ************************************************************************* */
+void ArgumentList::matlab_unwrap(ofstream& ofs, int start) {
+ int index = start;
+ BOOST_FOREACH(Argument arg, *this) {
+ stringstream buf;
+ buf << "in[" << index << "]";
+ arg.matlab_unwrap(ofs,buf.str());
+ index++;
+ }
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Argument.h b/wrap/Argument.h
new file mode 100644
index 000000000..73c3ca12f
--- /dev/null
+++ b/wrap/Argument.h
@@ -0,0 +1,51 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Argument.h
+ * brief: arguments to constructors and methods
+ * Author: Frank Dellaert
+ **/
+
+#pragma once
+
+#include
+#include
+
+// Argument class
+struct Argument {
+ bool is_const, is_ref, is_ptr;
+ std::string type;
+ std::string name;
+Argument() : is_const(false), is_ref(false), is_ptr(false) {}
+
+ // MATLAB code generation:
+ void matlab_unwrap(std::ofstream& ofs,
+ const std::string& matlabName); // MATLAB to C++
+};
+
+// Argument list
+struct ArgumentList : public std::list {
+ std::list args;
+ std::string types ();
+ std::string signature();
+ std::string names ();
+
+ // MATLAB code generation:
+
+ /**
+ * emit code to unwrap arguments
+ * @param ofs output stream
+ * @param start initial index for input array, set to 1 for method
+ */
+ void matlab_unwrap(std::ofstream& ofs, int start=0); // MATLAB to C++
+};
+
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
new file mode 100644
index 000000000..de1620733
--- /dev/null
+++ b/wrap/Class.cpp
@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Class.ccp
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+
+#include
+
+#include "Class.h"
+#include "utilities.h"
+
+using namespace std;
+
+/* ************************************************************************* */
+void Class::matlab_proxy(const string& classFile) {
+ // open destination classFile
+ ofstream ofs(classFile.c_str());
+ if(!ofs) throw CantOpenFile(classFile);
+ if(verbose_) cerr << "generating " << classFile << endl;
+
+ // emit class proxy code
+ emit_header_comment(ofs,"%");
+ ofs << "classdef " << name << endl;
+ ofs << " properties" << endl;
+ ofs << " self = 0" << endl;
+ ofs << " end" << endl;
+ ofs << " methods" << endl;
+ ofs << " function obj = " << name << "(varargin)" << endl;
+ BOOST_FOREACH(Constructor c, constructors)
+ c.matlab_proxy_fragment(ofs,name);
+ ofs << " if nargin ~= 13 && obj.self == 0, error('" << name << " constructor failed'); end" << endl;
+ ofs << " end" << endl;
+ ofs << " function display(obj), obj.print(''); end" << endl;
+ ofs << " function disp(obj), obj.display; end" << endl;
+ ofs << " end" << endl;
+ ofs << "end" << endl;
+
+ // close file
+ ofs.close();
+}
+
+/* ************************************************************************* */
+void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) {
+ BOOST_FOREACH(Constructor c, constructors) {
+ c.matlab_mfile (toolboxPath, name);
+ c.matlab_wrapper(toolboxPath, name, nameSpace);
+ }
+}
+
+/* ************************************************************************* */
+void Class::matlab_methods(const string& classPath, const string& nameSpace) {
+ BOOST_FOREACH(Method m, methods) {
+ m.matlab_mfile (classPath);
+ m.matlab_wrapper(classPath, name, nameSpace);
+ }
+}
+
+/* ************************************************************************* */
+void Class::matlab_make_fragment(ofstream& ofs,
+ const string& toolboxPath,
+ const string& mexFlags)
+{
+ string mex = "mex " + mexFlags + " ";
+ BOOST_FOREACH(Constructor c, constructors)
+ ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl;
+ ofs << endl << "cd @" << name << endl;
+ BOOST_FOREACH(Method m, methods)
+ ofs << mex << m.name << ".cpp" << endl;
+ ofs << endl;
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Class.h b/wrap/Class.h
new file mode 100644
index 000000000..f00812b95
--- /dev/null
+++ b/wrap/Class.h
@@ -0,0 +1,45 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Class.h
+ * brief: describe the C++ class that is being wrapped
+ * Author: Frank Dellaert
+ **/
+
+#pragma once
+
+#include
+#include
+
+#include "Constructor.h"
+#include "Method.h"
+
+// Class has name, constructors, methods
+struct Class {
+ std::string name;
+ std::list constructors;
+ std::list methods;
+ bool verbose_;
+
+ Class(bool verbose=true) : verbose_(verbose) {}
+
+ // MATLAB code generation:
+ void matlab_proxy(const std::string& classFile); // proxy class
+ void matlab_constructors(const std::string& toolboxPath,
+ const std::string& nameSpace); // constructor wrappers
+ void matlab_methods(const std::string& classPath,
+ const std::string& nameSpace); // method wrappers
+ void matlab_make_fragment(std::ofstream& ofs,
+ const std::string& toolboxPath,
+ const std::string& mexFlags); // make fragment
+};
+
diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp
new file mode 100644
index 000000000..d26e16a07
--- /dev/null
+++ b/wrap/Constructor.cpp
@@ -0,0 +1,100 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Constructor.ccp
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+
+#include
+
+#include "utilities.h"
+#include "Constructor.h"
+
+using namespace std;
+
+/* ************************************************************************* */
+string Constructor::matlab_wrapper_name(const string& className) {
+ string str = "new_" + className + "_" + args.signature();
+ return str;
+}
+
+/* ************************************************************************* */
+void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) {
+ ofs << " if nargin == " << args.size() << ", obj.self = "
+ << matlab_wrapper_name(className) << "(";
+ bool first = true;
+ for(size_t i=0;i" << endl;
+ ofs << "#include <" << className << ".h>" << endl;
+ if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl;
+ ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
+ ofs << "{" << endl;
+ ofs << " checkArguments(\"" << name << "\",nargout,nargin," << args.size() << ");" << endl;
+ args.matlab_unwrap(ofs); // unwrap arguments
+ ofs << " " << className << "* self = new " << className << "(" << args.names() << ");" << endl;
+ ofs << " out[0] = wrap_constructed(self,\"" << className << "\");" << endl;
+ ofs << "}" << endl;
+
+ // close file
+ ofs.close();
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Constructor.h b/wrap/Constructor.h
new file mode 100644
index 000000000..0a15c06d1
--- /dev/null
+++ b/wrap/Constructor.h
@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Constructor.h
+ * brief: class describing a constructor + code generation
+ * Author: Frank Dellaert
+ **/
+
+#pragma once
+
+#include
+#include
+
+#include "Argument.h"
+
+// Constructor class
+struct Constructor {
+ ArgumentList args;
+ bool verbose_;
+
+ Constructor(bool verbose=true) : verbose_(verbose) {}
+
+ // MATLAB code generation
+ // toolboxPath is main toolbox directory, e.g., ../matlab
+ // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
+
+ std::string matlab_wrapper_name(const std::string& className); // wrapper name
+ void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className); // proxy class fragment
+ void matlab_mfile (const std::string& toolboxPath, const std::string& className); // m-file
+ void matlab_wrapper(const std::string& toolboxPath,
+ const std::string& className,
+ const std::string& nameSpace); // wrapper
+};
+
diff --git a/wrap/Makefile.am b/wrap/Makefile.am
new file mode 100644
index 000000000..1aefe38d1
--- /dev/null
+++ b/wrap/Makefile.am
@@ -0,0 +1,70 @@
+#----------------------------------------------------------------------------------------------------
+# GTSAM Matlab wrap toolset
+#----------------------------------------------------------------------------------------------------
+
+# use nostdinc to turn off -I. and -I.., we do not need them because
+# header files are qualified so they can be included in external projects.
+AUTOMAKE_OPTIONS = nostdinc
+AM_DEFAULT_SOURCE_EXT = .cpp
+
+headers =
+sources =
+check_PROGRAMS =
+
+# disable all of matlab toolbox build by default
+if ENABLE_BUILD_TOOLBOX
+
+# Build a library from the core sources
+sources += utilities.cpp Argument.cpp Constructor.cpp Method.cpp Class.cpp Module.cpp
+check_PROGRAMS += tests/testSpirit tests/testWrap
+noinst_PROGRAMS = wrap
+
+#----------------------------------------------------------------------------------------------------
+# Create a libtool library that is not installed
+# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
+# The headers are installed in $(includedir)/gtsam:
+#----------------------------------------------------------------------------------------------------
+headers += $(sources:.cpp=.h) matlab.h
+wrapdir = $(pkgincludedir)/wrap
+wrap_HEADERS = $(headers)
+noinst_LTLIBRARIES = libwrap.la
+libwrap_la_SOURCES = $(sources)
+AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -DTOPSRCDIR="\"$(top_srcdir)\""
+AM_LDFLAGS = $(BOOST_LDFLAGS)
+
+#----------------------------------------------------------------------------------------------------
+# rules to build local programs
+#----------------------------------------------------------------------------------------------------
+TESTS = $(check_PROGRAMS)
+AM_LDFLAGS += $(boost_serialization)
+LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a
+
+# rule to run an executable
+%.run: % $(LDADD)
+ ./$^
+
+# rule to run executable with valgrind
+%.valgrind: % $(LDADD)
+ valgrind ./$^
+
+# generate local toolbox dir
+interfacePath = $(top_srcdir)
+moduleName = gtsam
+toolboxpath = ../toolbox
+nameSpace = "gtsam"
+mexFlags = "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/wrap -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam"
+all:
+ ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags}
+
+# install the headers and matlab toolbox
+install-exec-hook: all
+ install -d ${toolbox}/gtsam && \
+ cp -rf ../toolbox/* ${toolbox}/gtsam
+
+# clean local toolbox dir
+clean:
+ @test -z "wrap" || rm -f wrap
+ @test -z "../toolbox" || rm -rf ../toolbox
+
+endif
+#----------------------------------------------------------------------------------------------------
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
new file mode 100644
index 000000000..cec6391bb
--- /dev/null
+++ b/wrap/Method.cpp
@@ -0,0 +1,139 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Method.ccp
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+
+#include
+
+#include "Method.h"
+#include "utilities.h"
+
+using namespace std;
+
+/* ************************************************************************* */
+// auxiliary function to wrap an argument into a shared_ptr template
+/* ************************************************************************* */
+string maybe_shared_ptr(bool add, const string& type) {
+ string str = add? "shared_ptr<" : "";
+ str += type;
+ if (add) str += ">";
+ return str;
+}
+
+/* ************************************************************************* */
+string Method::return_type(bool add_ptr, pairing p) {
+ if (p==pair && returns_pair) {
+ string str = "pair< " +
+ maybe_shared_ptr(add_ptr && returns_ptr, returns ) + ", " +
+ maybe_shared_ptr(add_ptr && returns_ptr, returns2) + " >";
+ return str;
+ } else
+ return maybe_shared_ptr(add_ptr && returns_ptr, (p==arg2)? returns2 : returns);
+}
+
+/* ************************************************************************* */
+void Method::matlab_mfile(const string& classPath) {
+
+ // open destination m-file
+ string wrapperFile = classPath + "/" + name + ".m";
+ ofstream ofs(wrapperFile.c_str());
+ if(!ofs) throw CantOpenFile(wrapperFile);
+ if(verbose_) cerr << "generating " << wrapperFile << endl;
+
+ // generate code
+ emit_header_comment(ofs, "%");
+ ofs << "% usage: obj." << name << "(" << args.names() << ")" << endl;
+ string returnType = returns_pair? "[first,second]" : "result";
+ ofs << "function " << returnType << " = " << name << "(obj";
+ if (args.size()) ofs << "," << args.names();
+ ofs << ")" << endl;
+ ofs << " error('need to compile " << name << ".cpp');" << endl;
+ ofs << "end" << endl;
+
+ // close file
+ ofs.close();
+}
+
+/* ************************************************************************* */
+void Method::matlab_wrapper(const string& classPath,
+ const string& className,
+ const string& nameSpace)
+{
+ // open destination wrapperFile
+ string wrapperFile = classPath + "/" + name + ".cpp";
+ ofstream ofs(wrapperFile.c_str());
+ if(!ofs) throw CantOpenFile(wrapperFile);
+ if(verbose_) cerr << "generating " << wrapperFile << endl;
+
+ // generate code
+
+ // header
+ emit_header_comment(ofs, "//");
+ ofs << "#include \n";
+ ofs << "#include <" << className << ".h>\n";
+ if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl;
+
+ // call
+ ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
+ // start
+ ofs << "{\n";
+
+ // check arguments
+ // extra argument obj -> nargin-1 is passed !
+ // example: checkArguments("equals",nargout,nargin-1,2);
+ ofs << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n";
+
+ // get class pointer
+ // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test");
+ ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className
+ << " >(in[0],\"" << className << "\");" << endl;
+
+ // unwrap arguments, see Argument.cpp
+ args.matlab_unwrap(ofs,1);
+
+ // call method
+ // example: bool result = self->return_field(t);
+ ofs << " ";
+ if (returns!="void")
+ ofs << return_type(true,pair) << " result = ";
+ ofs << "self->" << name << "(" << args.names() << ");\n";
+
+ // wrap result
+ // example: out[0]=wrap(result);
+ if (returns_pair) {
+ if (returns_ptr)
+ ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns << "\");\n";
+ else
+ ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n";
+ if (returns_ptr2)
+ ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2 << "\");\n";
+ else
+ ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
+ }
+ else if (returns_ptr)
+ ofs << " out[0] = wrap_shared_ptr(result,\"" << returns << "\");\n";
+ else if (returns!="void")
+ ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
+
+ // finish
+ ofs << "}\n";
+
+ // close file
+ ofs.close();
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Method.h b/wrap/Method.h
new file mode 100644
index 000000000..8666f74ac
--- /dev/null
+++ b/wrap/Method.h
@@ -0,0 +1,46 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Method.h
+ * brief: describes and generates code for methods
+ * Author: Frank Dellaert
+ **/
+
+#pragma once
+
+#include
+#include
+
+#include "Argument.h"
+
+// Method class
+struct Method {
+ bool is_const;
+ ArgumentList args;
+ std::string returns, returns2, name;
+ bool returns_ptr, returns_ptr2, returns_pair;
+ bool verbose_;
+
+ Method(bool verbose=true) : returns_ptr(false), returns_ptr2(false), returns_pair(false), verbose_(verbose) {}
+
+ enum pairing {arg1, arg2, pair};
+ std::string return_type(bool add_ptr, pairing p);
+
+ // MATLAB code generation
+ // classPath is class directory, e.g., ../matlab/@Point2
+
+ void matlab_mfile (const std::string& classPath); // m-file
+ void matlab_wrapper(const std::string& classPath,
+ const std::string& className,
+ const std::string& nameSpace); // wrapper
+};
+
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
new file mode 100644
index 000000000..82977e336
--- /dev/null
+++ b/wrap/Module.cpp
@@ -0,0 +1,228 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Module.ccp
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+
+//#define BOOST_SPIRIT_DEBUG
+#include
+#include
+
+#include "Module.h"
+#include "utilities.h"
+
+using namespace std;
+using namespace BOOST_SPIRIT_CLASSIC_NS;
+
+typedef rule Rule;
+
+/* ************************************************************************* */
+// We parse an interface file into a Module object.
+// The grammar is defined using the boost/spirit combinatorial parser.
+// For example, str_p("const") parses the string "const", and the >>
+// operator creates a sequence parser. The grammar below, composed of rules
+// and with start rule [class_p], doubles as the specs for our interface files.
+/* ************************************************************************* */
+
+Module::Module(const string& interfacePath,
+ const string& moduleName, bool verbose) : name(moduleName), verbose_(verbose)
+{
+ // these variables will be imperatively updated to gradually build [cls]
+ // The one with postfix 0 are used to reset the variables after parse.
+ Argument arg0, arg;
+ ArgumentList args0, args;
+ Constructor constructor0(verbose), constructor(verbose);
+ Method method0(verbose), method(verbose);
+ Class cls0(verbose),cls(verbose);
+
+ //----------------------------------------------------------------------------
+ // Grammar with actions that build the Class object. Actions are
+ // defined within the square brackets [] and are executed whenever a
+ // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug.
+ // The grammar is allows a very restricted C++ header:
+ // - No comments allowed.
+ // -Only types allowed are string, bool, size_t int, double, Vector, and Matrix
+ // as well as class names that start with an uppercase letter
+ // - The types unsigned int and bool should be specified as int.
+ // ----------------------------------------------------------------------------
+
+ // lexeme_d turns off white space skipping
+ // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
+
+ Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')];
+
+ Rule classPtr_p =
+ className_p [assign_a(arg.type)] >>
+ ch_p('*') [assign_a(arg.is_ptr,true)];
+
+ Rule classRef_p =
+ !str_p("const") [assign_a(arg.is_const,true)] >>
+ className_p [assign_a(arg.type)] >>
+ ch_p('&') [assign_a(arg.is_ref,true)];
+
+ Rule basisType_p =
+ (str_p("string") | "bool" | "size_t" | "int" | "double");
+
+ Rule ublasType =
+ (str_p("Vector") | "Matrix")[assign_a(arg.type)] >>
+ !ch_p('*')[assign_a(arg.is_ptr,true)];
+
+ Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
+
+ Rule argument_p =
+ ((basisType_p[assign_a(arg.type)] | ublasType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)])
+ [push_back_a(args, arg)]
+ [assign_a(arg,arg0)];
+
+ Rule argumentList_p = !argument_p >> * (',' >> argument_p);
+
+ Rule constructor_p =
+ (className_p >> '(' >> argumentList_p >> ')' >> ';')
+ [assign_a(constructor.args,args)]
+ [assign_a(args,args0)]
+ [push_back_a(cls.constructors, constructor)]
+ [assign_a(constructor,constructor0)];
+
+ Rule returnType1_p =
+ basisType_p[assign_a(method.returns)] |
+ ((className_p | "Vector" | "Matrix")[assign_a(method.returns)] >>
+ !ch_p('*') [assign_a(method.returns_ptr,true)]);
+
+ Rule returnType2_p =
+ basisType_p[assign_a(method.returns2)] |
+ ((className_p | "Vector" | "Matrix")[assign_a(method.returns2)] >>
+ !ch_p('*') [assign_a(method.returns_ptr2,true)]);
+
+ Rule pair_p =
+ (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>')
+ [assign_a(method.returns_pair,true)];
+
+ Rule void_p = str_p("void")[assign_a(method.returns)];
+
+ Rule returnType_p = void_p | returnType1_p | pair_p;
+
+ Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')];
+
+ Rule method_p =
+ (returnType_p >> methodName_p[assign_a(method.name)] >>
+ '(' >> argumentList_p >> ')' >>
+ !str_p("const")[assign_a(method.is_const,true)] >> ';')
+ [assign_a(method.args,args)]
+ [assign_a(args,args0)]
+ [push_back_a(cls.methods, method)]
+ [assign_a(method,method0)];
+
+ Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >>
+ *constructor_p >>
+ *method_p >>
+ '}' >> ";";
+
+ Rule module_p = +class_p
+ [push_back_a(classes,cls)]
+ [assign_a(cls,cls0)]
+ >> !end_p;
+
+ //----------------------------------------------------------------------------
+ // for debugging, define BOOST_SPIRIT_DEBUG
+# ifdef BOOST_SPIRIT_DEBUG
+ BOOST_SPIRIT_DEBUG_NODE(className_p);
+ BOOST_SPIRIT_DEBUG_NODE(classPtr_p);
+ BOOST_SPIRIT_DEBUG_NODE(classRef_p);
+ BOOST_SPIRIT_DEBUG_NODE(basisType_p);
+ BOOST_SPIRIT_DEBUG_NODE(name_p);
+ BOOST_SPIRIT_DEBUG_NODE(argument_p);
+ BOOST_SPIRIT_DEBUG_NODE(argumentList_p);
+ BOOST_SPIRIT_DEBUG_NODE(constructor_p);
+ BOOST_SPIRIT_DEBUG_NODE(returnType1_p);
+ BOOST_SPIRIT_DEBUG_NODE(returnType2_p);
+ BOOST_SPIRIT_DEBUG_NODE(pair_p);
+ BOOST_SPIRIT_DEBUG_NODE(void_p);
+ BOOST_SPIRIT_DEBUG_NODE(returnType_p);
+ BOOST_SPIRIT_DEBUG_NODE(methodName_p);
+ BOOST_SPIRIT_DEBUG_NODE(method_p);
+ BOOST_SPIRIT_DEBUG_NODE(class_p);
+ BOOST_SPIRIT_DEBUG_NODE(module_p);
+# endif
+ //----------------------------------------------------------------------------
+
+ // read interface file
+ string interfaceFile = interfacePath + "/" + moduleName + ".h";
+ string contents = file_contents(interfaceFile);
+
+ // Comment parser : does not work for some reason
+ rule<> comment_p = str_p("/*") >> +anychar_p >> "*/";
+ rule<> skip_p = space_p; // | comment_p;
+
+ // and parse contents
+ parse_info info = parse(contents.c_str(), module_p, skip_p);
+ if(!info.full) {
+ printf("parsing stopped at \n%.20s\n",info.stop);
+ throw ParseFailed(info.length);
+ }
+}
+
+/* ************************************************************************* */
+void Module::matlab_code(const string& toolboxPath,
+ const string& nameSpace,
+ const string& mexFlags)
+{
+ try {
+ string installCmd = "install -d " + toolboxPath;
+ system(installCmd.c_str());
+
+ // create make m-file
+ string makeFile = toolboxPath + "/make_" + name + ".m";
+ ofstream ofs(makeFile.c_str());
+ if(!ofs) throw CantOpenFile(makeFile);
+
+ if (verbose_) cerr << "generating " << makeFile << endl;
+ emit_header_comment(ofs,"%");
+ ofs << "echo on" << endl << endl;
+ ofs << "toolboxpath = pwd" << endl;
+ ofs << "addpath(toolboxpath);" << endl << endl;
+
+ // generate proxy classes and wrappers
+ BOOST_FOREACH(Class cls, classes) {
+ // create directory if needed
+ string classPath = toolboxPath + "/@" + cls.name;
+ string installCmd = "install -d " + classPath;
+ system(installCmd.c_str());
+
+ // create proxy class
+ string classFile = classPath + "/" + cls.name + ".m";
+ cls.matlab_proxy(classFile);
+
+ // create constructor and method wrappers
+ cls.matlab_constructors(toolboxPath,nameSpace);
+ cls.matlab_methods(classPath,nameSpace);
+
+ // add lines to make m-file
+ ofs << "cd(toolboxpath)" << endl;
+ cls.matlab_make_fragment(ofs, toolboxPath, mexFlags);
+ }
+
+ // finish make m-file
+ ofs << "cd(toolboxpath)" << endl << endl;
+ ofs << "echo off" << endl;
+ ofs.close();
+ }
+ catch(exception &e) {
+ cerr << "generate_matlab_toolbox failed because " << e.what() << endl;
+ }
+
+}
+
+/* ************************************************************************* */
diff --git a/wrap/Module.h b/wrap/Module.h
new file mode 100644
index 000000000..5ee1248ca
--- /dev/null
+++ b/wrap/Module.h
@@ -0,0 +1,45 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: Module.h
+ * brief: describes module to be wrapped
+ * Author: Frank Dellaert
+ **/
+
+#pragma once
+
+#include
+#include
+
+#include "Class.h"
+
+// A module has classes
+struct Module {
+ std::string name;
+ std::list classes;
+ bool verbose_;
+
+ /**
+ * constructor that parses interface file
+ */
+ Module(const std::string& interfacePath,
+ const std::string& moduleName,
+ bool verbose=true);
+
+ /**
+ * MATLAB code generation:
+ */
+ void matlab_code(const std::string& path,
+ const std::string& nameSpace,
+ const std::string& mexFlags);
+};
+
diff --git a/wrap/geometry.h b/wrap/geometry.h
new file mode 100644
index 000000000..9b1d80b00
--- /dev/null
+++ b/wrap/geometry.h
@@ -0,0 +1,39 @@
+
+class Point2 {
+ Point2();
+ Point2(double x, double y);
+ double x();
+ double y();
+ int dim() const;
+};
+
+class Point3 {
+ Point3(double x, double y, double z);
+ double norm() const;
+};
+
+class Test {
+ Test();
+
+ bool return_bool (bool value);
+ size_t return_size_t (size_t value);
+ int return_int (int value);
+ double return_double (double value);
+
+ string return_string (string value);
+ Vector return_vector1(Vector value);
+ Matrix return_matrix1(Matrix value);
+ Vector return_vector2(Vector value);
+ Matrix return_matrix2(Matrix value);
+
+ pair return_pair (Vector v, Matrix A);
+
+ bool return_field(const Test& t) const;
+
+ Test* return_TestPtr(Test* value);
+
+ pair create_ptrs ();
+ pair return_ptrs (Test* p1, Test* p2);
+
+ void print();
+};
diff --git a/wrap/matlab.h b/wrap/matlab.h
new file mode 100644
index 000000000..6f203be9b
--- /dev/null
+++ b/wrap/matlab.h
@@ -0,0 +1,438 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+// header file to be included in MATLAB wrappers
+// Copyright (c) 2008 Frank Dellaert, All Rights reserved
+// wrapping and unwrapping is done using specialized templates, see
+// http://www.cplusplus.com/doc/tutorial/templates.html
+
+extern "C" {
+#include
+}
+
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+using namespace boost; // not usual, but for conciseness of generated code
+
+// start GTSAM Specifics /////////////////////////////////////////////////
+typedef Eigen::VectorXd Vector; // NOTE: outside of gtsam namespace
+typedef Eigen::MatrixXd Matrix;
+
+// to make keys be constructed from strings:
+#define GTSAM_MAGIC_KEY
+// to enable Matrix and Vector constructor for SharedGaussian:
+#define GTSAM_MAGIC_GAUSSIAN
+// end GTSAM Specifics /////////////////////////////////////////////////
+
+#ifdef __LP64__
+// 64-bit Mac
+#define mxUINT32OR64_CLASS mxUINT64_CLASS
+#else
+#define mxUINT32OR64_CLASS mxUINT32_CLASS
+#endif
+
+//*****************************************************************************
+// Utilities
+//*****************************************************************************
+
+void error(const char* str) {
+ mexErrMsgIdAndTxt("wrap:error", str);
+}
+
+mxArray *scalar(mxClassID classid) {
+ mwSize dims[1]; dims[0]=1;
+ return mxCreateNumericArray(1, dims, classid, mxREAL);
+}
+
+void checkScalar(const mxArray* array, const char* str) {
+ int m = mxGetM(array), n = mxGetN(array);
+ if (m!=1 || n!=1)
+ mexErrMsgIdAndTxt("wrap: not a scalar in ", str);
+}
+
+//*****************************************************************************
+// Check arguments
+//*****************************************************************************
+
+void checkArguments(const string& name, int nargout, int nargin, int expected) {
+ stringstream err;
+ err << name << " expects " << expected << " arguments, not " << nargin;
+ if (nargin!=expected)
+ error(err.str().c_str());
+}
+
+//*****************************************************************************
+// wrapping C++ basis types in MATLAB arrays
+//*****************************************************************************
+
+// default wrapping throws an error: only basis types are allowed in wrap
+template
+mxArray* wrap(Class& value) {
+ error("wrap internal error: attempted wrap of invalid type");
+}
+
+// specialization to string
+// wraps into a character array
+template<>
+mxArray* wrap(string& value) {
+ return mxCreateString(value.c_str());
+}
+
+// specialization to bool
+template<>
+mxArray* wrap(bool& value) {
+ mxArray *result = scalar(mxUINT32OR64_CLASS);
+ *(bool*)mxGetData(result) = value;
+ return result;
+}
+
+// specialization to size_t
+template<>
+mxArray* wrap(size_t& value) {
+ mxArray *result = scalar(mxUINT32OR64_CLASS);
+ *(size_t*)mxGetData(result) = value;
+ return result;
+}
+
+// specialization to int
+template<>
+mxArray* wrap(int& value) {
+ mxArray *result = scalar(mxUINT32OR64_CLASS);
+ *(int*)mxGetData(result) = value;
+ return result;
+}
+
+// specialization to double -> just double
+template<>
+mxArray* wrap(double& value) {
+ return mxCreateDoubleScalar(value);
+}
+
+// wrap a const BOOST vector into a double vector
+mxArray* wrap_Vector(const Vector& v) {
+ int m = v.size();
+ mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL);
+ double *data = mxGetPr(result);
+ for (int i=0;i double vector
+template<>
+mxArray* wrap(Vector& v) {
+ return wrap_Vector(v);
+}
+
+// const version
+template<>
+mxArray* wrap(const Vector& v) {
+ return wrap_Vector(v);
+}
+
+// wrap a const BOOST MATRIX into a double matrix
+mxArray* wrap_Matrix(const Matrix& A) {
+ int m = A.size1(), n = A.size2();
+ mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL);
+ double *data = mxGetPr(result);
+ // converts from column-major to row-major
+ for (int j=0;j double matrix
+template<>
+mxArray* wrap(Matrix& A) {
+ return wrap_Matrix(A);
+}
+
+// const version
+template<>
+mxArray* wrap(const Matrix& A) {
+ return wrap_Matrix(A);
+}
+
+//*****************************************************************************
+// unwrapping MATLAB arrays into C++ basis types
+//*****************************************************************************
+
+// default unwrapping throws an error
+// as wrap only supports passing a reference or one of the basic types
+template
+T unwrap(const mxArray* array) {
+ error("wrap internal error: attempted unwrap of invalid type");
+}
+
+// specialization to string
+// expects a character array
+// Warning: relies on mxChar==char
+template<>
+string unwrap(const mxArray* array) {
+ char *data = mxArrayToString(array);
+ if (data==NULL) error("unwrap: not a character array");
+ string str(data);
+ mxFree(data);
+ return str;
+}
+
+// specialization to bool
+template<>
+bool unwrap(const mxArray* array) {
+ checkScalar(array,"unwrap");
+ return mxGetScalar(array) != 0.0;
+}
+
+// specialization to size_t
+template<>
+size_t unwrap(const mxArray* array) {
+ checkScalar(array,"unwrap");
+ return (size_t)mxGetScalar(array);
+}
+
+// specialization to int
+template<>
+int unwrap(const mxArray* array) {
+ checkScalar(array,"unwrap");
+ return (int)mxGetScalar(array);
+}
+
+// specialization to double
+template<>
+double unwrap(const mxArray* array) {
+ checkScalar(array,"unwrap");
+ return (double)mxGetScalar(array);
+}
+
+// specialization to BOOST vector
+template<>
+Vector unwrap< Vector >(const mxArray* array) {
+ int m = mxGetM(array), n = mxGetN(array);
+ if (mxIsDouble(array)==false || n!=1) error("unwrap: not a vector");
+ double* data = (double*)mxGetData(array);
+ Vector v(m);
+ copy(data,data+m,v.begin());
+ return v;
+}
+
+// specialization to BOOST matrix
+template<>
+Matrix unwrap< Matrix >(const mxArray* array) {
+ if (mxIsDouble(array)==false) error("unwrap: not a matrix");
+ int m = mxGetM(array), n = mxGetN(array);
+ double* data = (double*)mxGetData(array);
+ Matrix A(m,n);
+ // converts from row-major to column-major
+ for (int j=0;j class Collector;
+
+template
+class ObjectHandle {
+private:
+ ObjectHandle* signature; // use 'this' as a unique object signature
+ const std::type_info* type; // type checking information
+ shared_ptr t; // object pointer
+
+public:
+ // Constructor for free-store allocated objects.
+ // Creates shared pointer, will delete if is last one to hold pointer
+ ObjectHandle(T* ptr) :
+ type(&typeid(T)), t(shared_ptr (ptr)) {
+ signature = this;
+ Collector::register_handle(this);
+ }
+
+ // Constructor for shared pointers
+ // Creates shared pointer, will delete if is last one to hold pointer
+ ObjectHandle(shared_ptr ptr) :
+ type(&typeid(T)), t(ptr) {
+ signature = this;
+ }
+
+ ~ObjectHandle() {
+ // object is in shared_ptr, will be automatically deleted
+ signature = 0; // destroy signature
+ }
+
+ // Get the actual object contained by handle
+ shared_ptr get_object() const {
+ return t;
+ }
+
+ // Print the mexhandle for debugging
+ void print(const char* str) {
+ mexPrintf("mexhandle %s:\n", str);
+ mexPrintf(" signature = %d:\n", signature);
+ mexPrintf(" pointer = %d:\n", t.get());
+ }
+
+ // Convert ObjectHandle to a mxArray handle (to pass back from mex-function).
+ // Create a numeric array as handle for an ObjectHandle.
+ // We ASSUME we can store object pointer in the mxUINT32 element of mxArray.
+ mxArray* to_mex_handle() {
+ mxArray* handle = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+ *reinterpret_cast**> (mxGetPr(handle)) = this;
+ return handle;
+ }
+
+ string type_name() const {
+ return type->name();
+ }
+
+ // Convert mxArray (passed to mex-function) to an ObjectHandle.
+ // Import a handle from MatLab as a mxArray of UINT32. Check that
+ // it is actually a pointer to an ObjectHandle.
+ static ObjectHandle* from_mex_handle(const mxArray* handle) {
+ if (mxGetClassID(handle) != mxUINT32OR64_CLASS || mxIsComplex(handle)
+ || mxGetM(handle) != 1 || mxGetN(handle) != 1) error(
+ "Parameter is not an ObjectHandle type.");
+
+ // We *assume* we can store ObjectHandle pointer in the mxUINT32 of handle
+ ObjectHandle* obj = *reinterpret_cast (mxGetPr(handle));
+
+ if (!obj) // gross check to see we don't have an invalid pointer
+ error("Parameter is NULL. It does not represent an ObjectHandle object.");
+ // TODO: change this for max-min check for pointer values
+
+ if (obj->signature != obj) // check memory has correct signature
+ error("Parameter does not represent an ObjectHandle object.");
+
+ /*
+ if (*(obj->type) != typeid(T)) { // check type
+ mexPrintf("Given: <%s>, Required: <%s>.\n", obj->type_name(), typeid(T).name());
+ error("Given ObjectHandle does not represent the correct type.");
+ }
+ */
+
+ return obj;
+ }
+
+ friend class Collector ; // allow Collector access to signature
+};
+
+// ---------------------------------------------------------
+// ------------------ Garbage Collection -------------------
+// ---------------------------------------------------------
+
+// Garbage collection singleton (one collector object for each type T).
+// Ensures that registered handles are deleted when the dll is released (they
+// may also be deleted previously without problem).
+// The Collector provides protection against resource leaks in the case
+// where 'clear all' is called in MatLab. (This is because MatLab will call
+// the destructors of statically allocated objects but not free-store allocated
+// objects.)
+template
+class Collector {
+ typedef ObjectHandle Handle;
+ typedef std::list< Handle* > ObjList;
+ typedef typename ObjList::iterator iterator;
+ ObjList objlist;
+public:
+ ~Collector() {
+ for (iterator i= objlist.begin(); i!=objlist.end(); ++i) {
+ if ((*i)->signature == *i) // check for valid signature
+ delete *i;
+ }
+ }
+
+ static void register_handle (Handle* obj) {
+ static Collector singleton;
+ singleton.objlist.push_back(obj);
+ }
+
+private: // prevent construction
+ Collector() {}
+ Collector(const Collector&);
+};
+
+//*****************************************************************************
+// wrapping C++ objects in a MATLAB proxy class
+//*****************************************************************************
+
+/*
+ For every C++ class Class, a matlab proxy class @Class/Class.m object
+ is created. Its constructor will check which of the C++ constructors
+ needs to be called, based on nr of arguments. It then calls the
+ corresponding mex function new_Class_signature, which will create a
+ C++ object using new, and pass the pointer to wrap_constructed
+ (below). This creates a mexhandle and returns it to the proxy class
+ constructor, which assigns it to self. Matlab owns this handle now.
+*/
+template
+mxArray* wrap_constructed(Class* pointer, const char *classname) {
+ ObjectHandle* handle = new ObjectHandle(pointer);
+ return handle->to_mex_handle();
+}
+
+/*
+ [create_object] creates a MATLAB proxy class object with a mexhandle
+ in the self property. Matlab does not allow the creation of matlab
+ objects from within mex files, hence we resort to an ugly trick: we
+ invoke the proxy class constructor by calling MATLAB, and pass 13
+ dummy arguments to let the constructor know we want an object without
+ the self property initialized. We then assign the mexhandle to self.
+*/
+// TODO: think about memory
+mxArray* create_object(const char *classname, mxArray* h) {
+ mxArray *result;
+ mxArray* dummy[13] = {h,h,h,h,h, h,h,h,h,h, h,h,h};
+ mexCallMATLAB(1,&result,13,dummy,classname);
+ mxSetProperty(result, 0, "self", h);
+ return result;
+}
+
+/*
+ When the user calls a method that returns a shared pointer, we create
+ an ObjectHandle from the shared_pointer and return it as a proxy
+ class to matlab.
+*/
+template
+mxArray* wrap_shared_ptr(shared_ptr< Class > shared_ptr, const char *classname) {
+ ObjectHandle* handle = new ObjectHandle(shared_ptr);
+ return create_object(classname,handle->to_mex_handle());
+}
+
+//*****************************************************************************
+// unwrapping a MATLAB proxy class to a C++ object reference
+//*****************************************************************************
+
+/*
+ Besides the basis types, the only other argument type allowed is a
+ shared pointer to a C++ object. In this case, matlab needs to pass a
+ proxy class object to the mex function. [unwrap_shared_ptr] extracts
+ the ObjectHandle from the self property, and returns a shared pointer
+ to the object.
+*/
+template
+shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) {
+ bool isClass = mxIsClass(obj, className.c_str());
+ if (!isClass) {
+ mexPrintf("Expected %s, got %s\n", className.c_str(), mxGetClassName(obj));
+ error("Argument has wrong type.");
+ }
+ mxArray* mxh = mxGetProperty(obj,0,"self");
+ if (mxh==NULL) error("unwrap_reference: invalid wrap object");
+ ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh);
+ return handle->get_object();
+}
+
+//*****************************************************************************
diff --git a/wrap/tests/expected/@Point2/Point2.m b/wrap/tests/expected/@Point2/Point2.m
new file mode 100644
index 000000000..9cdfd6fd5
--- /dev/null
+++ b/wrap/tests/expected/@Point2/Point2.m
@@ -0,0 +1,15 @@
+% automatically generated by wrap on 2010-Feb-23
+classdef Point2
+ properties
+ self = 0
+ end
+ methods
+ function obj = Point2(varargin)
+ if nargin == 0, obj.self = new_Point2_(); end
+ if nargin == 2, obj.self = new_Point2_dd(varargin{1},varargin{2}); end
+ if nargin ~= 13 && obj.self == 0, error('Point2 constructor failed'); end
+ end
+ function display(obj), obj.print(''); end
+ function disp(obj), obj.display; end
+ end
+end
diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp
new file mode 100644
index 000000000..2955d58c3
--- /dev/null
+++ b/wrap/tests/expected/@Point2/dim.cpp
@@ -0,0 +1,10 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("dim",nargout,nargin-1,0);
+ shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
+ int result = self->dim();
+ out[0] = wrap< int >(result);
+}
diff --git a/wrap/tests/expected/@Point2/dim.m b/wrap/tests/expected/@Point2/dim.m
new file mode 100644
index 000000000..93caab12f
--- /dev/null
+++ b/wrap/tests/expected/@Point2/dim.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.dim()
+function result = dim(obj)
+ error('need to compile dim.cpp');
+end
diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp
new file mode 100644
index 000000000..5f98d17c6
--- /dev/null
+++ b/wrap/tests/expected/@Point2/x.cpp
@@ -0,0 +1,10 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("x",nargout,nargin-1,0);
+ shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
+ double result = self->x();
+ out[0] = wrap< double >(result);
+}
diff --git a/wrap/tests/expected/@Point2/x.m b/wrap/tests/expected/@Point2/x.m
new file mode 100644
index 000000000..4dae83bf5
--- /dev/null
+++ b/wrap/tests/expected/@Point2/x.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.x()
+function result = x(obj)
+ error('need to compile x.cpp');
+end
diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp
new file mode 100644
index 000000000..835aa8a1e
--- /dev/null
+++ b/wrap/tests/expected/@Point2/y.cpp
@@ -0,0 +1,10 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("y",nargout,nargin-1,0);
+ shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
+ double result = self->y();
+ out[0] = wrap< double >(result);
+}
diff --git a/wrap/tests/expected/@Point2/y.m b/wrap/tests/expected/@Point2/y.m
new file mode 100644
index 000000000..9e2897114
--- /dev/null
+++ b/wrap/tests/expected/@Point2/y.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.y()
+function result = y(obj)
+ error('need to compile y.cpp');
+end
diff --git a/wrap/tests/expected/@Point3/Point3.m b/wrap/tests/expected/@Point3/Point3.m
new file mode 100644
index 000000000..139c7d079
--- /dev/null
+++ b/wrap/tests/expected/@Point3/Point3.m
@@ -0,0 +1,14 @@
+% automatically generated by wrap on 2010-Feb-23
+classdef Point3
+ properties
+ self = 0
+ end
+ methods
+ function obj = Point3(varargin)
+ if nargin == 3, obj.self = new_Point3_ddd(varargin{1},varargin{2},varargin{3}); end
+ if nargin ~= 13 && obj.self == 0, error('Point3 constructor failed'); end
+ end
+ function display(obj), obj.print(''); end
+ function disp(obj), obj.display; end
+ end
+end
diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp
new file mode 100644
index 000000000..37ad329ce
--- /dev/null
+++ b/wrap/tests/expected/@Point3/norm.cpp
@@ -0,0 +1,10 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("norm",nargout,nargin-1,0);
+ shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3");
+ double result = self->norm();
+ out[0] = wrap< double >(result);
+}
diff --git a/wrap/tests/expected/@Point3/norm.m b/wrap/tests/expected/@Point3/norm.m
new file mode 100644
index 000000000..02943303f
--- /dev/null
+++ b/wrap/tests/expected/@Point3/norm.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.norm()
+function result = norm(obj)
+ error('need to compile norm.cpp');
+end
diff --git a/wrap/tests/expected/@Test/Test.m b/wrap/tests/expected/@Test/Test.m
new file mode 100644
index 000000000..31e5e35b2
--- /dev/null
+++ b/wrap/tests/expected/@Test/Test.m
@@ -0,0 +1,14 @@
+% automatically generated by wrap on 2010-Feb-23
+classdef Test
+ properties
+ self = 0
+ end
+ methods
+ function obj = Test(varargin)
+ if nargin == 0, obj.self = new_Test_(); end
+ if nargin ~= 13 && obj.self == 0, error('Test constructor failed'); end
+ end
+ function display(obj), obj.print(''); end
+ function disp(obj), obj.display; end
+ end
+end
diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp
new file mode 100644
index 000000000..9a7215a60
--- /dev/null
+++ b/wrap/tests/expected/@Test/create_ptrs.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("create_ptrs",nargout,nargin-1,0);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ pair< shared_ptr, shared_ptr > result = self->create_ptrs();
+ out[0] = wrap_shared_ptr(result.first,"Test");
+ out[1] = wrap_shared_ptr(result.second,"Test");
+}
diff --git a/wrap/tests/expected/@Test/create_ptrs.m b/wrap/tests/expected/@Test/create_ptrs.m
new file mode 100644
index 000000000..0fbbd9be5
--- /dev/null
+++ b/wrap/tests/expected/@Test/create_ptrs.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.create_ptrs()
+function [first,second] = create_ptrs(obj)
+ error('need to compile create_ptrs.cpp');
+end
diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp
new file mode 100644
index 000000000..5db55477c
--- /dev/null
+++ b/wrap/tests/expected/@Test/print.cpp
@@ -0,0 +1,9 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("print",nargout,nargin-1,0);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ self->print();
+}
diff --git a/wrap/tests/expected/@Test/print.m b/wrap/tests/expected/@Test/print.m
new file mode 100644
index 000000000..91e8ff31a
--- /dev/null
+++ b/wrap/tests/expected/@Test/print.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.print()
+function result = print(obj)
+ error('need to compile print.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp
new file mode 100644
index 000000000..0c22621a6
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_TestPtr.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_TestPtr",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test");
+ shared_ptr result = self->return_TestPtr(value);
+ out[0] = wrap_shared_ptr(result,"Test");
+}
diff --git a/wrap/tests/expected/@Test/return_TestPtr.m b/wrap/tests/expected/@Test/return_TestPtr.m
new file mode 100644
index 000000000..0753c283b
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_TestPtr.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_TestPtr(value)
+function result = return_TestPtr(obj,value)
+ error('need to compile return_TestPtr.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp
new file mode 100644
index 000000000..d05f5c3b7
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_bool.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_bool",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ bool value = unwrap< bool >(in[1]);
+ bool result = self->return_bool(value);
+ out[0] = wrap< bool >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_bool.m b/wrap/tests/expected/@Test/return_bool.m
new file mode 100644
index 000000000..3ef97ad8f
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_bool.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_bool(value)
+function result = return_bool(obj,value)
+ error('need to compile return_bool.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp
new file mode 100644
index 000000000..edbffb889
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_double.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_double",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ double value = unwrap< double >(in[1]);
+ double result = self->return_double(value);
+ out[0] = wrap< double >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_double.m b/wrap/tests/expected/@Test/return_double.m
new file mode 100644
index 000000000..87e2929da
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_double.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_double(value)
+function result = return_double(obj,value)
+ error('need to compile return_double.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp
new file mode 100644
index 000000000..727f6faa0
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_field.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_field",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ Test& t = *unwrap_shared_ptr< Test >(in[1], "Test");
+ bool result = self->return_field(t);
+ out[0] = wrap< bool >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_field.m b/wrap/tests/expected/@Test/return_field.m
new file mode 100644
index 000000000..7e63e32e2
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_field.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_field(t)
+function result = return_field(obj,t)
+ error('need to compile return_field.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp
new file mode 100644
index 000000000..61b53f4ce
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_int.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_int",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ int value = unwrap< int >(in[1]);
+ int result = self->return_int(value);
+ out[0] = wrap< int >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_int.m b/wrap/tests/expected/@Test/return_int.m
new file mode 100644
index 000000000..df5502203
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_int.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_int(value)
+function result = return_int(obj,value)
+ error('need to compile return_int.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp
new file mode 100644
index 000000000..c7b12ddad
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_matrix1.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_matrix1",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ Matrix value = unwrap< Matrix >(in[1]);
+ Matrix result = self->return_matrix1(value);
+ out[0] = wrap< Matrix >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_matrix1.m b/wrap/tests/expected/@Test/return_matrix1.m
new file mode 100644
index 000000000..bb71670db
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_matrix1.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_matrix1(value)
+function result = return_matrix1(obj,value)
+ error('need to compile return_matrix1.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp
new file mode 100644
index 000000000..e41cf5f96
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_matrix2.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_matrix2",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ Matrix value = unwrap< Matrix >(in[1]);
+ Matrix result = self->return_matrix2(value);
+ out[0] = wrap< Matrix >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_matrix2.m b/wrap/tests/expected/@Test/return_matrix2.m
new file mode 100644
index 000000000..b8b74dd03
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_matrix2.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_matrix2(value)
+function result = return_matrix2(obj,value)
+ error('need to compile return_matrix2.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp
new file mode 100644
index 000000000..0e274049e
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_pair.cpp
@@ -0,0 +1,13 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_pair",nargout,nargin-1,2);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ Vector v = unwrap< Vector >(in[1]);
+ Matrix A = unwrap< Matrix >(in[2]);
+ pair< Vector, Matrix > result = self->return_pair(v,A);
+ out[0] = wrap< Vector >(result.first);
+ out[1] = wrap< Matrix >(result.second);
+}
diff --git a/wrap/tests/expected/@Test/return_pair.m b/wrap/tests/expected/@Test/return_pair.m
new file mode 100644
index 000000000..35348fd9b
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_pair.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_pair(v,A)
+function [first,second] = return_pair(obj,v,A)
+ error('need to compile return_pair.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp
new file mode 100644
index 000000000..4ad4c4fd4
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_ptrs.cpp
@@ -0,0 +1,13 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_ptrs",nargout,nargin-1,2);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "Test");
+ shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "Test");
+ pair< shared_ptr, shared_ptr > result = self->return_ptrs(p1,p2);
+ out[0] = wrap_shared_ptr(result.first,"Test");
+ out[1] = wrap_shared_ptr(result.second,"Test");
+}
diff --git a/wrap/tests/expected/@Test/return_ptrs.m b/wrap/tests/expected/@Test/return_ptrs.m
new file mode 100644
index 000000000..ba1a16659
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_ptrs.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_ptrs(p1,p2)
+function [first,second] = return_ptrs(obj,p1,p2)
+ error('need to compile return_ptrs.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp
new file mode 100644
index 000000000..ce5a1a9e4
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_size_t.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_size_t",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ size_t value = unwrap< size_t >(in[1]);
+ size_t result = self->return_size_t(value);
+ out[0] = wrap< size_t >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_size_t.m b/wrap/tests/expected/@Test/return_size_t.m
new file mode 100644
index 000000000..ad4500fd1
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_size_t.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_size_t(value)
+function result = return_size_t(obj,value)
+ error('need to compile return_size_t.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp
new file mode 100644
index 000000000..d09e8fb31
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_string.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_string",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ string value = unwrap< string >(in[1]);
+ string result = self->return_string(value);
+ out[0] = wrap< string >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_string.m b/wrap/tests/expected/@Test/return_string.m
new file mode 100644
index 000000000..28a02b0d1
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_string.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_string(value)
+function result = return_string(obj,value)
+ error('need to compile return_string.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp
new file mode 100644
index 000000000..079ebff09
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_vector1.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_vector1",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ Vector value = unwrap< Vector >(in[1]);
+ Vector result = self->return_vector1(value);
+ out[0] = wrap< Vector >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_vector1.m b/wrap/tests/expected/@Test/return_vector1.m
new file mode 100644
index 000000000..266017c86
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_vector1.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_vector1(value)
+function result = return_vector1(obj,value)
+ error('need to compile return_vector1.cpp');
+end
diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp
new file mode 100644
index 000000000..693d3d06b
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_vector2.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("return_vector2",nargout,nargin-1,1);
+ shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test");
+ Vector value = unwrap< Vector >(in[1]);
+ Vector result = self->return_vector2(value);
+ out[0] = wrap< Vector >(result);
+}
diff --git a/wrap/tests/expected/@Test/return_vector2.m b/wrap/tests/expected/@Test/return_vector2.m
new file mode 100644
index 000000000..f237ce228
--- /dev/null
+++ b/wrap/tests/expected/@Test/return_vector2.m
@@ -0,0 +1,5 @@
+% automatically generated by wrap on 2010-Feb-23
+% usage: obj.return_vector2(value)
+function result = return_vector2(obj,value)
+ error('need to compile return_vector2.cpp');
+end
diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m
new file mode 100644
index 000000000..a266b2d6d
--- /dev/null
+++ b/wrap/tests/expected/make_geometry.m
@@ -0,0 +1,44 @@
+% automatically generated by wrap on 2010-Feb-23
+echo on
+
+toolboxpath = pwd
+addpath(toolboxpath);
+
+cd(toolboxpath)
+mex -O5 new_Point2_.cpp
+mex -O5 new_Point2_dd.cpp
+
+cd @Point2
+mex -O5 x.cpp
+mex -O5 y.cpp
+mex -O5 dim.cpp
+
+cd(toolboxpath)
+mex -O5 new_Point3_ddd.cpp
+
+cd @Point3
+mex -O5 norm.cpp
+
+cd(toolboxpath)
+mex -O5 new_Test_.cpp
+
+cd @Test
+mex -O5 return_bool.cpp
+mex -O5 return_size_t.cpp
+mex -O5 return_int.cpp
+mex -O5 return_double.cpp
+mex -O5 return_string.cpp
+mex -O5 return_vector1.cpp
+mex -O5 return_matrix1.cpp
+mex -O5 return_vector2.cpp
+mex -O5 return_matrix2.cpp
+mex -O5 return_pair.cpp
+mex -O5 return_field.cpp
+mex -O5 return_TestPtr.cpp
+mex -O5 create_ptrs.cpp
+mex -O5 return_ptrs.cpp
+mex -O5 print.cpp
+
+cd(toolboxpath)
+
+echo off
diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp
new file mode 100644
index 000000000..6a78d318d
--- /dev/null
+++ b/wrap/tests/expected/new_Point2_.cpp
@@ -0,0 +1,9 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("new_Point2_",nargout,nargin,0);
+ Point2* self = new Point2();
+ out[0] = wrap_constructed(self,"Point2");
+}
diff --git a/wrap/tests/expected/new_Point2_.m b/wrap/tests/expected/new_Point2_.m
new file mode 100644
index 000000000..b5cab71ab
--- /dev/null
+++ b/wrap/tests/expected/new_Point2_.m
@@ -0,0 +1,4 @@
+% automatically generated by wrap on 2010-Feb-23
+function result = new_Point2_(obj)
+ error('need to compile new_Point2_.cpp');
+end
diff --git a/wrap/tests/expected/new_Point2_dd.cpp b/wrap/tests/expected/new_Point2_dd.cpp
new file mode 100644
index 000000000..15a8a190d
--- /dev/null
+++ b/wrap/tests/expected/new_Point2_dd.cpp
@@ -0,0 +1,11 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("new_Point2_dd",nargout,nargin,2);
+ double x = unwrap< double >(in[0]);
+ double y = unwrap< double >(in[1]);
+ Point2* self = new Point2(x,y);
+ out[0] = wrap_constructed(self,"Point2");
+}
diff --git a/wrap/tests/expected/new_Point2_dd.m b/wrap/tests/expected/new_Point2_dd.m
new file mode 100644
index 000000000..bdd42b766
--- /dev/null
+++ b/wrap/tests/expected/new_Point2_dd.m
@@ -0,0 +1,4 @@
+% automatically generated by wrap on 2010-Feb-23
+function result = new_Point2_dd(obj,x,y)
+ error('need to compile new_Point2_dd.cpp');
+end
diff --git a/wrap/tests/expected/new_Point3_ddd.cpp b/wrap/tests/expected/new_Point3_ddd.cpp
new file mode 100644
index 000000000..82c95bdc6
--- /dev/null
+++ b/wrap/tests/expected/new_Point3_ddd.cpp
@@ -0,0 +1,12 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("new_Point3_ddd",nargout,nargin,3);
+ double x = unwrap< double >(in[0]);
+ double y = unwrap< double >(in[1]);
+ double z = unwrap< double >(in[2]);
+ Point3* self = new Point3(x,y,z);
+ out[0] = wrap_constructed(self,"Point3");
+}
diff --git a/wrap/tests/expected/new_Point3_ddd.m b/wrap/tests/expected/new_Point3_ddd.m
new file mode 100644
index 000000000..229b9658c
--- /dev/null
+++ b/wrap/tests/expected/new_Point3_ddd.m
@@ -0,0 +1,4 @@
+% automatically generated by wrap on 2010-Feb-23
+function result = new_Point3_ddd(obj,x,y,z)
+ error('need to compile new_Point3_ddd.cpp');
+end
diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp
new file mode 100644
index 000000000..ab2171762
--- /dev/null
+++ b/wrap/tests/expected/new_Test_.cpp
@@ -0,0 +1,9 @@
+// automatically generated by wrap on 2010-Feb-23
+#include
+#include
+void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+ checkArguments("new_Test_",nargout,nargin,0);
+ Test* self = new Test();
+ out[0] = wrap_constructed(self,"Test");
+}
diff --git a/wrap/tests/expected/new_Test_.m b/wrap/tests/expected/new_Test_.m
new file mode 100644
index 000000000..1aae9b6f5
--- /dev/null
+++ b/wrap/tests/expected/new_Test_.m
@@ -0,0 +1,4 @@
+% automatically generated by wrap on 2010-Feb-23
+function result = new_Test_(obj)
+ error('need to compile new_Test_.cpp');
+end
diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp
new file mode 100644
index 000000000..120e5561f
--- /dev/null
+++ b/wrap/tests/testSpirit.cpp
@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * Unit test for Boost's awesome Spirit parser
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+#include
+
+using namespace std;
+using namespace BOOST_SPIRIT_CLASSIC_NS;
+
+typedef rule Rule;
+
+/* ************************************************************************* */
+// lexeme_d turns off white space skipping
+// http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
+Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
+Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')];
+Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')];
+
+Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double" | "Vector" | "Matrix");
+
+/* ************************************************************************* */
+TEST( spirit, real ) {
+ // check if we can parse 8.99 as a real
+ CHECK(parse("8.99", real_p, space_p).full);
+ // make sure parsing fails on this one
+ CHECK(!parse("zztop", real_p, space_p).full);
+}
+
+/* ************************************************************************* */
+TEST( spirit, string ) {
+ // check if we can parse a string
+ CHECK(parse("double", str_p("double"), space_p).full);
+}
+
+/* ************************************************************************* */
+TEST( spirit, sequence ) {
+ // check that we skip white space
+ CHECK(parse("int int", str_p("int") >> *str_p("int"), space_p).full);
+ CHECK(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full);
+ CHECK(parse("const \t string", str_p("const") >> str_p("string"), space_p).full);
+
+ // not that (see spirit FAQ) the vanilla rule<> does not deal with whitespace
+ rule<>vanilla_p = str_p("const") >> str_p("string");
+ CHECK(!parse("const \t string", vanilla_p, space_p).full);
+
+ // to fix it, we need to use
+ rulephrase_level_p = str_p("const") >> str_p("string");
+ CHECK(parse("const \t string", phrase_level_p, space_p).full);
+}
+
+/* ************************************************************************* */
+// parser for interface files
+
+// const string reference reference
+Rule constStringRef_p =
+ str_p("const") >> "string" >> '&';
+
+// class reference
+Rule classRef_p = className_p >> '&';
+
+// const class reference
+Rule constClassRef_p = str_p("const") >> classRef_p;
+
+// method parsers
+Rule constMethod_p = basisType_p >> methodName_p >> '(' >> ')' >> "const" >> ';';
+
+/* ************************************************************************* */
+TEST( spirit, basisType_p ) {
+ CHECK(!parse("Point3", basisType_p, space_p).full);
+ CHECK(parse("string", basisType_p, space_p).full);
+}
+
+/* ************************************************************************* */
+TEST( spirit, className_p ) {
+ CHECK(parse("Point3", className_p, space_p).full);
+}
+
+/* ************************************************************************* */
+TEST( spirit, classRef_p ) {
+ CHECK(parse("Point3 &", classRef_p, space_p).full);
+ CHECK(parse("Point3&", classRef_p, space_p).full);
+}
+
+/* ************************************************************************* */
+TEST( spirit, constMethod_p ) {
+ CHECK(parse("double norm() const;", constMethod_p, space_p).full);
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp
new file mode 100644
index 000000000..2e664c6c3
--- /dev/null
+++ b/wrap/tests/testWrap.cpp
@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * Unit test for wrap.c
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+using namespace std;
+static bool verbose = false;
+#ifdef TOPSRCDIR
+static string topdir = TOPSRCDIR;
+#else
+static string topdir = "penguin";
+#endif
+
+/* ************************************************************************* */
+TEST( wrap, ArgumentList ) {
+ ArgumentList args;
+ Argument arg; arg.type = "double"; arg.name = "x";
+ args.push_back(arg);
+ args.push_back(arg);
+ args.push_back(arg);
+ CHECK(args.signature()=="ddd");
+ EXPECT(args.types()=="double,double,double");
+ EXPECT(args.names()=="x,x,x");
+}
+
+/* ************************************************************************* */
+TEST( wrap, check_exception ) {
+ THROWS_EXCEPTION(Module("/notarealpath", "geometry",verbose));
+ CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",verbose), CantOpenFile);
+}
+
+/* ************************************************************************* */
+TEST( wrap, parse ) {
+ string path = topdir + "/wrap";
+
+ Module module(path.c_str(), "geometry",verbose);
+ EXPECT(module.classes.size()==3);
+
+ // check second class, Point3
+ Class cls = *(++module.classes.begin());
+ EXPECT(cls.name=="Point3");
+ EXPECT(cls.constructors.size()==1);
+ EXPECT(cls.methods.size()==1);
+
+ // first constructor takes 3 doubles
+ Constructor c1 = cls.constructors.front();
+ EXPECT(c1.args.size()==3);
+
+ // check first double argument
+ Argument a1 = c1.args.front();
+ EXPECT(!a1.is_const);
+ EXPECT(a1.type=="double");
+ EXPECT(!a1.is_ref);
+ EXPECT(a1.name=="x");
+
+ // check method
+ Method m1 = cls.methods.front();
+ EXPECT(m1.returns=="double");
+ EXPECT(m1.name=="norm");
+ EXPECT(m1.args.size()==0);
+ EXPECT(m1.is_const);
+}
+
+/* ************************************************************************* */
+TEST( wrap, matlab_code ) {
+ // Parse into class object
+ string path = topdir + "/wrap";
+ Module module(path,"geometry",verbose);
+
+ // emit MATLAB code
+ // make_geometry will not compile, use make testwrap to generate real make
+ module.matlab_code("actual", "", "-O5");
+
+ EXPECT(files_equal(path + "/tests/expected/@Point2/Point2.m" , "actual/@Point2/Point2.m" ));
+ EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" ));
+ EXPECT(files_equal(path + "/tests/expected/@Point3/Point3.m" , "actual/@Point3/Point3.m" ));
+ EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.m" , "actual/new_Point3_ddd.m" ));
+ EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.cpp", "actual/new_Point3_ddd.cpp"));
+ EXPECT(files_equal(path + "/tests/expected/@Point3/norm.m" , "actual/@Point3/norm.m" ));
+ EXPECT(files_equal(path + "/tests/expected/@Point3/norm.cpp" , "actual/@Point3/norm.cpp" ));
+
+ EXPECT(files_equal(path + "/tests/expected/new_Test_.cpp" , "actual/new_Test_.cpp" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/Test.m" , "actual/@Test/Test.m" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/return_string.cpp" , "actual/@Test/return_string.cpp" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/return_pair.cpp" , "actual/@Test/return_pair.cpp" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/return_field.cpp" , "actual/@Test/return_field.cpp" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/return_TestPtr.cpp", "actual/@Test/return_TestPtr.cpp"));
+ EXPECT(files_equal(path + "/tests/expected/@Test/return_ptrs.cpp" , "actual/@Test/return_ptrs.cpp" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/print.m" , "actual/@Test/print.m" ));
+ EXPECT(files_equal(path + "/tests/expected/@Test/print.cpp" , "actual/@Test/print.cpp" ));
+
+ EXPECT(files_equal(path + "/tests/expected/make_geometry.m" , "actual/make_geometry.m" ));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp
new file mode 100644
index 000000000..f7376390f
--- /dev/null
+++ b/wrap/utilities.cpp
@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: utilities.ccp
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+
+#include
+
+#include "utilities.h"
+
+using namespace std;
+using namespace boost::gregorian;
+
+/* ************************************************************************* */
+string file_contents(const string& filename, bool skipheader) {
+ ifstream ifs(filename.c_str());
+ if(!ifs) throw CantOpenFile(filename);
+
+ // read file into stringstream
+ stringstream ss;
+ if (skipheader) ifs.ignore(256,'\n');
+ ss << ifs.rdbuf();
+ ifs.close();
+
+ // return string
+ return ss.str();
+}
+
+/* ************************************************************************* */
+bool files_equal(const string& expected, const string& actual, bool skipheader) {
+ try {
+ string expected_contents = file_contents(expected, skipheader);
+ string actual_contents = file_contents(actual, skipheader);
+ bool equal = actual_contents == expected_contents;
+ if (!equal) {
+ stringstream command;
+ command << "diff " << actual << " " << expected << endl;
+ system(command.str().c_str());
+ }
+ return equal;
+ }
+ catch (const string& reason) {
+ cerr << "expection: " << reason << endl;
+ return false;
+ }
+ return true;
+}
+
+/* ************************************************************************* */
+void emit_header_comment(ofstream& ofs, const string& delimiter) {
+ date today = day_clock::local_day();
+ ofs << delimiter << " automatically generated by wrap on " << today << endl;
+}
+
+/* ************************************************************************* */
diff --git a/wrap/utilities.h b/wrap/utilities.h
new file mode 100644
index 000000000..e75c44025
--- /dev/null
+++ b/wrap/utilities.h
@@ -0,0 +1,60 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: utilities.ccp
+ * Author: Frank Dellaert
+ **/
+
+#pragma once
+
+#include
+#include
+
+class CantOpenFile : public std::exception {
+ private:
+ std::string filename_;
+ public:
+ CantOpenFile(const std::string& filename) : filename_(filename) {}
+ ~CantOpenFile() throw() {}
+ virtual const char* what() const throw() {
+ return ("Can't open file " + filename_).c_str();
+ }
+};
+
+class ParseFailed : public std::exception {
+ private:
+ int length_;
+ public:
+ ParseFailed(int length) : length_(length) {}
+ ~ParseFailed() throw() {}
+ virtual const char* what() const throw() {
+ std::stringstream buf;
+ buf << "Parse failed at character " << (length_+1);
+ return buf.str().c_str();
+ }
+};
+
+/**
+ * read contents of a file into a std::string
+ */
+std::string file_contents(const std::string& filename, bool skipheader=false);
+
+/**
+ * Check whether two files are equal
+ * By default, skips the first line of actual so header is not generated
+ */
+bool files_equal(const std::string& expected, const std::string& actual, bool skipheader=true);
+
+/**
+ * emit a header at the top of generated files
+ */
+void emit_header_comment(std::ofstream& ofs, const std::string& delimiter);
diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp
new file mode 100644
index 000000000..1a52da6bd
--- /dev/null
+++ b/wrap/wrap.cpp
@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * file: wrap.ccp
+ * brief: wraps functions
+ * Author: Frank Dellaert
+ **/
+
+#include
+#include
+
+#include "Module.h"
+
+using namespace std;
+
+/* ************************************************************************* */
+/**
+ * main function to wrap a module
+ */
+void generate_matlab_toolbox(const string& interfacePath,
+ const string& moduleName,
+ const string& toolboxPath,
+ const string& nameSpace,
+ const string& mexFlags)
+{
+ // Parse into class object
+ Module module(interfacePath, moduleName, false);
+
+ // emit MATLAB code
+ module.matlab_code(toolboxPath,nameSpace,mexFlags);
+}
+
+/* ************************************************************************* */
+
+int main(int argc, const char* argv[]) {
+ if (argc<5 || argc>6) {
+ cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl;
+ cerr << "usage: wrap interfacePath moduleName toolboxPath" << endl;
+ cerr << " interfacePath : *absolute* path to directory of module interface file" << endl;
+ cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl;
+ cerr << " toolboxPath : the directory in which to generate the wrappers" << endl;
+ cerr << " nameSpace : namespace to use, pass empty string if none" << endl;
+ cerr << " [mexFlags] : extra flags for the mex command" << endl;
+ }
+ else
+ generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argc==5 ? " " : argv[5]);
+}
+
+/* ************************************************************************* */