diff --git a/.cproject b/.cproject index a35bd2fed..35f7946a4 100644 --- a/.cproject +++ b/.cproject @@ -36,6 +36,9 @@ @@ -681,6 +684,22 @@ true true + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + make -j2 @@ -2029,6 +2048,22 @@ true true + + make + -j5 + check + true + false + true + + + make + -j5 + install + true + false + true + make -j2 @@ -2045,6 +2080,38 @@ true true + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + diff --git a/configure.ac b/configure.ac index 6b057ba46..5929b22ee 100644 --- a/configure.ac +++ b/configure.ac @@ -30,8 +30,11 @@ DX_PS_FEATURE(OFF) DX_INIT_DOXYGEN(gtsam) # Check for OS -AC_CANONICAL_HOST # needs to be called at some point earlier -AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) +# needs to be called at some point earlier +AC_CANONICAL_HOST +AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) +AM_CONDITIONAL([LINUX], [case $host_os in linux*) true;; *) false;; esac]) +AM_CONDITIONAL([IS_64BIT], [case $host_cpu in *x86_64*) true;; *) false;; esac]) # enable debug variable AC_ARG_ENABLE([debug], @@ -44,20 +47,6 @@ AC_ARG_ENABLE([debug], AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) - -AC_CANONICAL_HOST -# We need to determine what os we are on to determine if we need to do -# special things because we are on a mac -case $host_os in - darwin* ) - # Do something specific for mac - ISMAC=true - ;; - *) - ISMAC=false - ;; -esac - # enable profiling AC_ARG_ENABLE([profiling], [ --enable-profiling Enable profiling], @@ -91,6 +80,77 @@ AC_ARG_ENABLE([install_cppunitlite], AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue]) +# 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]) + +# enable installation of matlab tests +AC_ARG_ENABLE([install_matlab_tests], + [ --enable-install-matlab-tests Enable installation of tests for the Matlab toolbox], + [case "${enableval}" in + yes) install_matlab_tests=true ;; + no) install_matlab_tests=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-tests]) ;; + esac],[install_matlab_tests=true]) + +AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue]) + +# enable installation of matlab examples +AC_ARG_ENABLE([install_matlab_examples], + [ --enable-install-matlab-examples Enable installation of examples for the Matlab toolbox], + [case "${enableval}" in + yes) install_matlab_examples=true ;; + no) install_matlab_examples=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-examples]) ;; + esac],[install_matlab_examples=true]) + +AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_EXAMPLES], [test x$install_matlab_examples = 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]) + +# enable installation of the wrap utility +AC_ARG_ENABLE([install_wrap], + [ --enable-install-wrap Enable installation of the wrap tool for generating matlab interfaces], + [case "${enableval}" in + yes) install_wrap=true ;; + no) install_wrap=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-wrap]) ;; + esac],[install_wrap=false]) + +AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue]) + +# enable unsafe mode for wrap +AC_ARG_ENABLE([unsafe_wrap], + [ --enable-unsafe-wrap Enable using unsafe mode in wrap], + [case "${enableval}" in + yes) unsafe_wrap=true ;; + no) unsafe_wrap=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-unsafe-wrap]) ;; + esac],[unsafe_wrap=false]) + +AM_CONDITIONAL([ENABLE_UNSAFE_WRAP], [test x$unsafe_wrap = xtrue]) + +# wrap install path: optional flag to change location of wrap, defaults to install prefix/bin +AC_ARG_WITH([wrap], + [AS_HELP_STRING([--with-wrap], + [specify the wrap directory for installation])], + [wrap=$withval], + [wrap=$prefix/bin]) +AC_SUBST([wrap]) + # Checks for programs. AC_PROG_CXX AC_PROG_CC @@ -115,25 +175,6 @@ AC_CHECK_FUNCS([pow sqrt]) # Check for boost AX_BOOST_BASE([1.40]) -# 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]) - AC_CONFIG_FILES([CppUnitLite/Makefile \ wrap/Makefile \ gtsam/3rdparty/Makefile \ diff --git a/examples/Data/calib.txt b/examples/Data/calib.txt index 8eeb7e656..cb8b77384 100644 --- a/examples/Data/calib.txt +++ b/examples/Data/calib.txt @@ -1 +1 @@ -800 600 -1119.61507797 1119.61507797 399.5 299.5 +800 600 1119.61507797 1119.61507797 399.5 299.5 diff --git a/examples/Data/measurementsISAM.txt b/examples/Data/measurementsISAM.txt deleted file mode 100644 index 7325c7dd9..000000000 --- a/examples/Data/measurementsISAM.txt +++ /dev/null @@ -1,12 +0,0 @@ -10 -ttpy10.feat -ttpy20.feat -ttpy30.feat -ttpy40.feat -ttpy50.feat -ttpy60.feat -ttpy70.feat -ttpy80.feat -ttpy90.feat -ttpy100.feat - diff --git a/examples/Data/posesISAM.txt b/examples/Data/posesISAM.txt deleted file mode 100644 index ebe39c6f2..000000000 --- a/examples/Data/posesISAM.txt +++ /dev/null @@ -1,12 +0,0 @@ -10 -ttpy10.pose -ttpy20.pose -ttpy30.pose -ttpy40.pose -ttpy50.pose -ttpy60.pose -ttpy70.pose -ttpy80.pose -ttpy90.pose -ttpy100.pose - diff --git a/examples/Data/ttpy10.pose b/examples/Data/ttpy10.pose index ee0f6231a..b9226c723 100644 --- a/examples/Data/ttpy10.pose +++ b/examples/Data/ttpy10.pose @@ -1,4 +1,4 @@ -0.939693 0.34202 0 0 --0.241845 0.664463 -0.707107 0 --0.241845 0.664463 0.707107 0 -34.202 -93.9693 100 1 +0.93969 0.24185 -0.24185 34.202 +0.34202 -0.66446 0.66446 -93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy100.pose b/examples/Data/ttpy100.pose index 4225e3d4e..cbd21233f 100644 --- a/examples/Data/ttpy100.pose +++ b/examples/Data/ttpy100.pose @@ -1,4 +1,4 @@ --0.939693 -0.34202 0 0 -0.241845 -0.664463 -0.707107 0 -0.241845 -0.664463 0.707107 0 --34.202 93.9693 100 1 +-0.93969 -0.24185 0.24185 -34.202 +-0.34202 0.66446 -0.66446 93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy20.pose b/examples/Data/ttpy20.pose index f8b252855..bba2606d0 100644 --- a/examples/Data/ttpy20.pose +++ b/examples/Data/ttpy20.pose @@ -1,4 +1,4 @@ -0.766044 0.642788 0 0 --0.454519 0.541675 -0.707107 0 --0.454519 0.541675 0.707107 0 -64.2788 -76.6044 100 1 +0.76604 0.45452 -0.45452 64.279 +0.64279 -0.54168 0.54168 -76.604 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy30.pose b/examples/Data/ttpy30.pose index f7ad9f674..737e391f2 100644 --- a/examples/Data/ttpy30.pose +++ b/examples/Data/ttpy30.pose @@ -1,4 +1,4 @@ -0.5 0.866025 0 0 --0.612372 0.353553 -0.707107 0 --0.612372 0.353553 0.707107 0 -86.6025 -50 100 1 +0.5 0.61237 -0.61237 86.603 +0.86603 -0.35355 0.35355 -50 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy40.pose b/examples/Data/ttpy40.pose index e9c9cd251..4e241c130 100644 --- a/examples/Data/ttpy40.pose +++ b/examples/Data/ttpy40.pose @@ -1,4 +1,4 @@ -0.173648 0.984808 0 0 --0.696364 0.122788 -0.707107 0 --0.696364 0.122788 0.707107 0 -98.4808 -17.3648 100 1 +0.17365 0.69636 -0.69636 98.481 +0.98481 -0.12279 0.12279 -17.365 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy50.pose b/examples/Data/ttpy50.pose index 9c84b54f3..5a57b10ef 100644 --- a/examples/Data/ttpy50.pose +++ b/examples/Data/ttpy50.pose @@ -1,4 +1,4 @@ --0.173648 0.984808 0 0 --0.696364 -0.122788 -0.707107 0 --0.696364 -0.122788 0.707107 0 -98.4808 17.3648 100 1 +-0.17365 0.69636 -0.69636 98.481 +0.98481 0.12279 -0.12279 17.365 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy60.pose b/examples/Data/ttpy60.pose index 9c0a8c942..4e5455a0d 100644 --- a/examples/Data/ttpy60.pose +++ b/examples/Data/ttpy60.pose @@ -1,4 +1,4 @@ --0.5 0.866025 0 0 --0.612372 -0.353553 -0.707107 0 --0.612372 -0.353553 0.707107 0 -86.6025 50 100 1 +-0.5 0.61237 -0.61237 86.603 +0.86603 0.35355 -0.35355 50 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy70.pose b/examples/Data/ttpy70.pose index 0c2254de6..608e9ff7c 100644 --- a/examples/Data/ttpy70.pose +++ b/examples/Data/ttpy70.pose @@ -1,4 +1,4 @@ --0.766044 0.642788 0 0 --0.454519 -0.541675 -0.707107 0 --0.454519 -0.541675 0.707107 0 -64.2788 76.6044 100 1 +-0.76604 0.45452 -0.45452 64.279 +0.64279 0.54168 -0.54168 76.604 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy80.pose b/examples/Data/ttpy80.pose index 9d6491304..8a6ae4cd1 100644 --- a/examples/Data/ttpy80.pose +++ b/examples/Data/ttpy80.pose @@ -1,4 +1,4 @@ --0.939693 0.34202 0 0 --0.241845 -0.664463 -0.707107 0 --0.241845 -0.664463 0.707107 0 -34.202 93.9693 100 1 +-0.93969 0.24185 -0.24185 34.202 +0.34202 0.66446 -0.66446 93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy90.pose b/examples/Data/ttpy90.pose index b51c1a58f..863f71fea 100644 --- a/examples/Data/ttpy90.pose +++ b/examples/Data/ttpy90.pose @@ -1,4 +1,4 @@ --1 0 0 0 -0 -0.707107 -0.707107 0 -0 -0.707107 0.707107 0 -0 100 100 1 +-1 -0 0 0 +0 0.70711 -0.70711 100 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index ed3a68ba8..bd35e3636 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace boost; // Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h @@ -40,8 +41,8 @@ using namespace boost; /* ************************************************************************* */ #define CALIB_FILE "calib.txt" #define LANDMARKS_FILE "landmarks.txt" -#define POSES_FILE "posesISAM.txt" -#define MEASUREMENTS_FILE "measurementsISAM.txt" +#define POSES_FILE "poses.txt" +#define MEASUREMENTS_FILE "measurements.txt" // Base data folder string g_dataFolder; @@ -49,8 +50,8 @@ string g_dataFolder; // Store groundtruth values, read from files shared_ptrK g_calib; map g_landmarks; // map: -std::vector g_poses; // prior camera poses at each frame -std::vector > g_measurements; // feature sets detected at each frame +map g_poses; // map: +std::map > g_measurements; // feature sets detected at each frame // Noise models SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); @@ -69,7 +70,7 @@ void readAllDataISAM() { g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE); // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes. - g_poses = readPosesISAM(g_dataFolder, POSES_FILE); + g_poses = readPoses(g_dataFolder, POSES_FILE); // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark. g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE); @@ -80,7 +81,7 @@ void readAllDataISAM() { * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, - int pose_id, Pose3& pose, std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { + int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements newFactors = shared_ptr (new Graph()); @@ -121,16 +122,19 @@ int main(int argc, char* argv[]) { g_dataFolder = string(argv[1]) + "/"; readAllDataISAM(); - // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates + // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); - // At each frame i with new camera pose and new set of measurements associated with it, + // At each frame (poseId) with new camera pose and set of associated measurements, // create a graph of new factors and update ISAM - for (size_t i = 0; i < g_measurements.size(); i++) { + typedef std::map > FeatureMap; + BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { + const int poseId = features.first; shared_ptr newFactors; shared_ptr initialValues; - createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib); + createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], + features.second, measurementSigma, g_calib); isam.update(*newFactors, *initialValues); visualSLAM::Values currentEstimate = isam.estimate(); diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp index ae9042f2a..1c3912f2d 100644 --- a/examples/vSLAMexample/vSLAMutils.cpp +++ b/examples/vSLAMexample/vSLAMutils.cpp @@ -46,10 +46,6 @@ std::map readLandMarks(const std::string& landmarkFile) { } /* ************************************************************************* */ -/** - * Read pose from file, output by Panda3D. - * Warning: row major!!! - */ gtsam::Pose3 readPose(const char* Fn) { ifstream poseFile(Fn); if (!poseFile) { @@ -62,18 +58,7 @@ gtsam::Pose3 readPose(const char* Fn) { poseFile >> v[i]; poseFile.close(); - // Because panda3d's camera is z-up, y-view, - // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule - //... similar to OpenGL's camera - for (int i = 0; i<3; i++) { - float t = v[4+i]; - v[4+i] = v[8+i]; - v[8+i] = -t; - } - - ::Vector vec = Vector_(16, v); - - Matrix T = Matrix_(4,4, vec); // column order !!! + Matrix T = Matrix_(4,4, v); // row order !!! Pose3 pose(T); return pose; @@ -166,28 +151,7 @@ std::vector readAllMeasurements(const std::string& baseFolder, const } /* ************************************************************************* */ -std::vector readPosesISAM(const std::string& baseFolder, const std::string& posesFn) { - ifstream posesFile((baseFolder+posesFn).c_str()); - if (!posesFile) { - cout << "Cannot read all pose ISAM file: " << posesFn << endl; - exit(0); - } - int numPoses; - posesFile >> numPoses; - vector poses; - for (int i = 0; i> poseFileName; - - Pose3 pose = readPose((baseFolder+poseFileName).c_str()); - poses.push_back(pose); - } - - return poses; -} - -/* ************************************************************************* */ -std::vector > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) { +std::map > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) { ifstream measurementsFile((baseFolder+measurementsFn).c_str()); if (!measurementsFile) { cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl; @@ -196,13 +160,16 @@ std::vector > readAllMeasurementsISAM(const std::string& int numPoses; measurementsFile >> numPoses; - std::vector > allFeatures; + std::map > allFeatures; for (int i = 0; i> poseId; + string featureFileName; measurementsFile >> featureFileName; - vector features = readFeatures(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM - allFeatures.push_back(features); + vector features = readFeatures(poseId, (baseFolder+featureFileName).c_str()); + allFeatures[poseId] = features; } return allFeatures; diff --git a/examples/vSLAMexample/vSLAMutils.h b/examples/vSLAMexample/vSLAMutils.h index f1483cd70..52d492224 100644 --- a/examples/vSLAMexample/vSLAMutils.h +++ b/examples/vSLAMexample/vSLAMutils.h @@ -29,11 +29,9 @@ std::map readLandMarks(const std::string& landmarkFile); gtsam::Pose3 readPose(const char* poseFn); std::map readPoses(const std::string& baseFolder, const std::string& posesFN); -std::vector readPosesISAM(const std::string& baseFolder, const std::string& posesFN); - gtsam::shared_ptrK readCalibData(const std::string& calibFn); std::vector readFeatureFile(const char* filename); std::vector readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn); -std::vector< std::vector > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); +std::map > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); diff --git a/gtsam-broken.h b/gtsam-broken.h deleted file mode 100644 index 09113a695..000000000 --- a/gtsam-broken.h +++ /dev/null @@ -1,182 +0,0 @@ -// 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 Ordering{ - Ordering(string key); - void print(string s) const; - bool equals(const Ordering& ord, double tol) const; - Ordering subtract(const Ordering& keys) const; - void unique (); - void reverse (); - void push_back(string s); -}; - -class GaussianFactorSet { - GaussianFactorSet(); - void push_back(GaussianFactor* factor); -}; - -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 GaussianFactorGraph { - GaussianConditional* eliminateOne(string key); - GaussianBayesNet* eliminate_(const Ordering& ordering); - VectorValues* optimize_(const Ordering& ordering); - pair matrix(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 SymbolicFactor{ - SymbolicFactor(const Ordering& keys); - void print(string s) 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 index 331bdd09a..071ababc8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1,29 +1,139 @@ +/** + * GTSAM Wrap Module Definition + * + * These are the current classes available through the matlab toolbox interface, + * add more functions/classes as they are available. + * + * Requirements: + * Classes must start with an uppercase letter + * Only one Method/Constructor per line + * Methods can return + * - Eigen types: Matrix, Vector + * - C/C++ basic types: string, bool, size_t, int, double + * - void + * - Any class with which be copied with boost::make_shared() + * - boost::shared_ptr of any object type + * Limitations on methods + * - Parsing does not support overloading + * - There can only be one method with a given name + * Arguments to functions any of + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference + * - C/C++ basic types: string, bool, size_t, int, double + * - Any class with which be copied with boost::make_shared() (except Eigen) + * - boost::shared_ptr of any object type (except Eigen) + * Comments can use either C++ or C style, with multiple lines + * Namespace definitions + * - Names of namespaces must start with a lowercase letter + * - start a namespace with "namespace {" + * - end a namespace with exactly "}///\namespace [namespace_name]", optionally adding the name of the namespace + * - This ending is not C++ standard, and must contain "}///\namespace" to parse + * - Namespaces can be nested + * Namespace usage + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * Methods must start with a lowercase letter + * Static methods must start with a letter (upper or lowercase) and use the "static" keyword + * Includes in C++ wrappers + * - By default, the include will be <[classname].h> + * - To override, add a full include statement inside the class definition + */ + +/** + * Status: + * - TODO: global functions + * - TODO: default values for arguments + * - TODO: overloaded functions + * - TODO: Handle Rot3M conversions to quaternions + */ + +// Everything is in the gtsam namespace, so we avoid copying everything in +using namespace gtsam; + class Point2 { Point2(); Point2(double x, double y); + static Point2 Expmap(Vector v); + static Vector Logmap(const Point2& p); void print(string s) const; double x(); double y(); + Vector localCoordinates(const Point2& p); + Point2 compose(const Point2& p2); + Point2 between(const Point2& p2); + Point2 retract(Vector v); }; class Point3 { Point3(); Point3(double x, double y, double z); Point3(Vector v); + static Point3 Expmap(Vector v); + static Vector Logmap(const Point3& p); void print(string s) const; + bool equals(const Point3& p, double tol); Vector vector() const; double x(); double y(); double z(); + Vector localCoordinates(const Point3& p); + Point3 retract(Vector v); + Point3 compose(const Point3& p2); + Point3 between(const Point3& p2); }; class Rot2 { Rot2(); Rot2(double theta); + static Rot2 Expmap(Vector v); + static Vector Logmap(const Rot2& p); + static Rot2 fromAngle(double theta); + static Rot2 fromDegrees(double theta); + static Rot2 fromCosSin(double c, double s); + static Rot2 relativeBearing(const Point2& d); // Ignoring derivative + static Rot2 atan2(double y, double x); void print(string s) const; - bool equals(const Rot2& pose, double tol) const; + bool equals(const Rot2& rot, double tol) const; + double theta() const; + double degrees() const; double c() const; double s() const; + Vector localCoordinates(const Rot2& p); + Rot2 retract(Vector v); + Rot2 compose(const Rot2& p2); + Rot2 between(const Rot2& p2); +}; + +class Rot3 { + Rot3(); + Rot3(Matrix R); + static Rot3 Expmap(Vector v); + static Vector Logmap(const Rot3& p); + static Rot3 ypr(double y, double p, double r); + static Rot3 Rx(double t); + static Rot3 Ry(double t); + static Rot3 Rz(double t); + static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(const Vector& xyz); + static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading) + static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft) + static Rot3 quaternion(double w, double x, double y, double z); + static Rot3 rodriguez(const Vector& v); + Matrix matrix() const; + Matrix transpose() const; + Vector xyz() const; + Vector ypr() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + void print(string s) const; + bool equals(const Rot3& rot, double tol) const; + Vector localCoordinates(const Rot3& p); + Rot3 retract(Vector v); + Rot3 compose(const Rot3& p2); + Rot3 between(const Rot3& p2); }; class Pose2 { @@ -32,15 +142,41 @@ class Pose2 { Pose2(double theta, const Point2& t); Pose2(const Rot2& r, const Point2& t); Pose2(Vector v); + static Pose2 Expmap(Vector v); + static Vector Logmap(const Pose2& p); void print(string s) const; bool equals(const Pose2& pose, double tol) const; double x() const; double y() const; double theta() const; int dim() const; - Pose2* compose_(const Pose2& p2); - Pose2* between_(const Pose2& p2); Vector localCoordinates(const Pose2& p); + Pose2 retract(Vector v); + Pose2 compose(const Pose2& p2); + Pose2 between(const Pose2& p2); + Rot2 bearing(const Point2& point); + double range(const Point2& point); +}; + +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t); + Pose3(Vector v); + Pose3(Matrix t); + static Pose3 Expmap(Vector v); + static Vector Logmap(const Pose3& p); + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + Matrix adjointMap() const; + Pose3 compose(const Pose3& p2); + Pose3 between(const Pose3& p2); + Pose3 retract(Vector v); + Point3 translation() const; + Rot3 rotation() const; }; class SharedGaussian { @@ -54,6 +190,12 @@ class SharedDiagonal { Vector sample() const; }; +class SharedNoiseModel { +#include + SharedNoiseModel(const SharedDiagonal& model); + SharedNoiseModel(const SharedGaussian& model); +}; + class VectorValues { VectorValues(); VectorValues(int nVars, int varDim); @@ -118,6 +260,14 @@ class GaussianFactorGraph { Matrix sparseJacobian_() const; }; +class GaussianSequentialSolver { + GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); + GaussianBayesNet* eliminate() const; + VectorValues* optimize() const; + GaussianFactor* marginalFactor(int j) const; + Matrix marginalCovariance(int j) const; +}; + class KalmanFilter { KalmanFilter(Vector x, const SharedDiagonal& model); void print(string s) const; @@ -129,14 +279,6 @@ class KalmanFilter { void update(Matrix H, Vector z, const SharedDiagonal& model); }; -class Landmark2 { - Landmark2(); - Landmark2(double x, double y); - void print(string s) const; - double x(); - double y(); -}; - class Ordering { Ordering(); void print(string s) const; @@ -144,22 +286,28 @@ class Ordering { void push_back(string key); }; -class PlanarSLAMValues { - PlanarSLAMValues(); +// Planar SLAM example domain +namespace planarSLAM { + +class Values { +#include + Values(); void print(string s) const; - Pose2* pose(int key); + Pose2 pose(int key) const; + Point2 point(int key) const; void insertPose(int key, const Pose2& pose); void insertPoint(int key, const Point2& point); }; -class PlanarSLAMGraph { - PlanarSLAMGraph(); +class Graph { +#include + Graph(); void print(string s) const; - double error(const PlanarSLAMValues& values) const; - Ordering* orderingCOLAMD(const PlanarSLAMValues& values) const; - GaussianFactorGraph* linearize(const PlanarSLAMValues& values, + double error(const planarSLAM::Values& values) const; + Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; + GaussianFactorGraph* linearize(const planarSLAM::Values& values, const Ordering& ordering) const; void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); @@ -169,18 +317,215 @@ class PlanarSLAMGraph { void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel); void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, const SharedNoiseModel& noiseModel); - PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate); + planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); }; -class PlanarSLAMOdometry { - PlanarSLAMOdometry(int key1, int key2, const Pose2& measured, +class Odometry { +#include + Odometry(int key1, int key2, const Pose2& measured, const SharedNoiseModel& model); void print(string s) const; - GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; + GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const; }; -class GaussianSequentialSolver { - GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); - GaussianBayesNet* eliminate(); - VectorValues* optimize(); +}///\namespace planarSLAM + +// Simulated2D Example Domain +namespace simulated2D { + +class Values { +#include + Values(); + 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 Graph { +#include + Graph(); +}; + +// TODO: add factors, etc. + +}///\namespace simulated2D + +// Simulated2DOriented Example Domain +namespace simulated2DOriented { + +class Values { +#include + Values(); + 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 Graph { +#include + Graph(); +}; + +// TODO: add factors, etc. + +}///\namespace simulated2DOriented + +//// 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 Ordering{ +// Ordering(string key); +// void print(string s) const; +// bool equals(const Ordering& ord, double tol) const; +// Ordering subtract(const Ordering& keys) const; +// void unique (); +// void reverse (); +// void push_back(string s); +//}; +// +//class GaussianFactorSet { +// GaussianFactorSet(); +// void push_back(GaussianFactor* factor); +//}; +// +//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; +//}; +// +//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 GaussianFactorGraph { +// GaussianConditional* eliminateOne(string key); +// GaussianBayesNet* eliminate_(const Ordering& ordering); +// VectorValues* optimize_(const Ordering& ordering); +// pair matrix(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 SymbolicFactor{ +// SymbolicFactor(const Ordering& keys); +// void print(string s) 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/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 69fa02a45..46f738f7d 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -277,6 +277,18 @@ bool assert_container_equality(const V& expected, const V& actual) { return true; } +/** + * Compare strings for unit tests + */ +bool assert_equal(const std::string& expected, const std::string& actual) { + if (expected == actual) + return true; + printf("Not equal:\n"); + std::cout << "expected: [" << expected << "]\n"; + std::cout << "actual: [" << actual << "]" << std::endl; + return false; +} + /** * Allow for testing inequality */ diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 16f07f5ab..3f015e9d3 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -164,13 +164,14 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) if(debug) { cout << "Partial LDL with " << nFrontal << " frontal scalars, "; - print(ABC, "ABC: "); + print(ABC, "LDL ABC: "); } // Compute Cholesky factorization of A, overwrites A = sqrt(D)*R // tic(1, "ldl"); Eigen::LDLT ldlt; ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView()); + if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl; if(ldlt.vectorD().unaryExpr(boost::bind(less(), _1, 0.0)).any()) { if(ISDEBUG("detailed_exceptions")) @@ -180,14 +181,15 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) } Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL - if (debug) cout << "Dsqrt: " << sqrtD << endl; + if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl; // U = sqrtD * L^ Matrix U = ldlt.matrixU(); + if(debug) cout << "LDL U:\n" << U << endl; // we store the permuted upper triangular matrix ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky - if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; + if(debug) cout << "LDL R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; // toc(1, "ldl"); // Compute S = inv(R') * B = inv(P'U')*B = inv(U')*P*B @@ -196,20 +198,19 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) ABC.topRightCorner(nFrontal, n-nFrontal) = ldlt.transpositionsP() * ABC.topRightCorner(nFrontal, n-nFrontal); ABC.block(0,0,nFrontal,nFrontal).triangularView().transpose().solveInPlace(ABC.topRightCorner(nFrontal, n-nFrontal)); } - if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; + if(debug) cout << "LDL S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; // toc(2, "compute S"); // Compute L = C - S' * S // tic(3, "compute L"); - if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if(debug) cout << "LDL C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); - if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if(debug) cout << "LDL L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; // toc(3, "compute L"); - if(debug) cout << "P: " << ldlt.transpositionsP().indices() << endl; - + if(debug) cout << "LDL P: " << ldlt.transpositionsP().indices() << endl; return ldlt.transpositionsP(); } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index de311249a..74cb2453d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -103,11 +103,6 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(compose(p2))); - } - /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); @@ -151,11 +146,6 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(between(p2))); - } - /** * Return point coordinates in pose coordinate frame */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index fa6f02828..6c6860f0f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -38,7 +38,7 @@ namespace gtsam { // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Experimental - unit tests of derivatives based on it do not check out yet - Matrix Pose3::AdjointMap() const { + Matrix Pose3::adjointMap() const { const Matrix R = R_.matrix(); const Vector t = t_.vector(); Matrix A = skewSymmetric(t)*R; @@ -188,7 +188,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { if (H1) { #ifdef CORRECT_POSE3_EXMAP - *H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface + *H1 = adjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); @@ -214,22 +214,23 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) + if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? + // FIXME: this function doesn't exist with this interface - should this be "*H1 = -adjointMap();" ? + { *H1 = - adjointMap(p); } #else - { - Matrix Rt = R_.transpose(); - Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; - Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; - Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt = collect(2, &Dt_R1, &Dt_t1); - *H1 = gtsam::stack(2, &DR, &Dt); - } + { + Matrix Rt = R_.transpose(); + Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; + Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; + Matrix DR = collect(2, &DR_R1, &DR_t1); + Matrix Dt = collect(2, &Dt_R1, &Dt_t1); + *H1 = gtsam::stack(2, &DR, &Dt); + } #endif - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt*(-t_)); - } + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt*(-t_)); + } /* ************************************************************************* */ // between = compose(p2,inverse(p1)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f03dcde06..17b547e79 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -99,16 +99,16 @@ namespace gtsam { /// @{ /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { return dimension; } + static size_t Dim() { return dimension; } /// Dimensionality of the tangent space = 6 DOF size_t dim() const { return dimension; } - /** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose + /// Retraction from R^6 to Pose3 manifold neighborhood around current pose Pose3 retract(const Vector& d) const; - /// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose + /// Local 6D coordinates of Pose3 manifold neighborhood around current pose Vector localCoordinates(const Pose3& T2) const; /// @} @@ -148,8 +148,8 @@ namespace gtsam { * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } + Matrix adjointMap() const; /// FIXME Not tested - marked as incorrect + Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect /** * wedge for Pose3: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 928b9b450..a4816914e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -25,224 +25,234 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Rot3M); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Rot3M); - static const Matrix I3 = eye(3); +static const Matrix I3 = eye(3); - /* ************************************************************************* */ - // static member functions to construct rotations +/* ************************************************************************* */ +Rot3M Rot3M::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} - Rot3M Rot3M::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} - Rot3M Rot3M::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} - Rot3M Rot3M::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); - } +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3M Rot3M::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3M( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} - // Considerably faster than composing matrices above ! - Rot3M Rot3M::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3M( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); - } - - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; #ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); + double l_n = wwTxx + wwTyy + wwTzz; + if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); #endif - double c = cos(theta), s = sin(theta), c_1 = 1 - c; + double c = cos(theta), s = sin(theta), c_1 = 1 - c; - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; - return Rot3M( c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); - } + return Rot3M( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3M(); - return rodriguez(w/t, t); - } +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3M(); + return rodriguez(w/t, t); +} - /* ************************************************************************* */ - bool Rot3M::equals(const Rot3M & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } +/* ************************************************************************* */ +bool Rot3M::equals(const Rot3M & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} - /* ************************************************************************* */ - Matrix Rot3M::matrix() const { - double r[] = { r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z() }; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::matrix() const { + Matrix R(3,3); + R << + r1_.x(), r2_.x(), r3_.x(), + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z(); + return R; +} - /* ************************************************************************* */ - Matrix Rot3M::transpose() const { - double r[] = { r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()}; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::transpose() const { + Matrix Rt(3,3); + Rt << + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z(); + return Rt; +} - /* ************************************************************************* */ - Point3 Rot3M::column(int index) const{ - if(index == 3) - return r3_; - else if (index == 2) - return r2_; - else - return r1_; // default returns r1 - } +/* ************************************************************************* */ +Point3 Rot3M::column(int index) const{ + if(index == 3) + return r3_; + else if (index == 2) + return r2_; + else + return r1_; // default returns r1 +} - /* ************************************************************************* */ - Vector Rot3M::xyz() const { - Matrix I;Vector q; - boost::tie(I,q)=RQ(matrix()); - return q; - } +/* ************************************************************************* */ +Vector Rot3M::xyz() const { + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; +} - Vector Rot3M::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); - } +/* ************************************************************************* */ +Vector Rot3M::ypr() const { + Vector q = xyz(); + return Vector_(3,q(2),q(1),q(0)); +} - Vector Rot3M::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); - } +/* ************************************************************************* */ +Vector Rot3M::rpy() const { + Vector q = xyz(); + return Vector_(3,q(0),q(1),q(2)); +} - /* ************************************************************************* */ - // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3M::Logmap(const Rot3M& R) { - double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) - return zero(3); - } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) - double theta = acos((tr-1.0)/2.0); - // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) - return (0.5 + theta*theta/12)*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(fabs(R.r3().z() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * - Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(fabs(R.r2().y() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * - Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * - Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); - } else { - double theta = acos((tr-1.0)/2.0); - return (theta/2.0/sin(theta))*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } - } - - /* ************************************************************************* */ - Point3 Rot3M::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = matrix(); - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); - } - - /* ************************************************************************* */ - // see doc/math.lyx, SO(3) section - Point3 Rot3M::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; - } - - /* ************************************************************************* */ - Rot3M Rot3M::compose (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; - } - - /* ************************************************************************* */ - Rot3M Rot3M::between (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); - } - - /* ************************************************************************* */ - pair RQ(const Matrix& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3M Qx = Rot3M::Rx(-x); - Matrix B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3M Qy = Rot3M::Ry(-y); - Matrix C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3M Qz = Rot3M::Rz(-z); - Matrix R = C * Qz.matrix(); - - Vector xyz = Vector_(3, x, y, z); - return make_pair(R, xyz); +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector Rot3M::Logmap(const Rot3M& R) { + double tr = R.r1().x()+R.r2().y()+R.r3().z(); + // FIXME should tr in statement below be absolute value? + if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) + return zero(3); + } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) + double theta = acos((tr-1.0)/2.0); + // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) + return (0.5 + theta*theta/12)*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + // FIXME: in statement below, is this the right comparision? + } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + if(fabs(R.r3().z() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * + Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); + else if(fabs(R.r2().y() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * + Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); + else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * + Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); + } else { + double theta = acos((tr-1.0)/2.0); + return (theta/2.0/sin(theta))*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); } +} - /* ************************************************************************* */ +/* ************************************************************************* */ +Point3 Rot3M::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = matrix(); + return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3M::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +Rot3M Rot3M::compose (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3M Rot3M::between (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); +} + +/* ************************************************************************* */ +pair RQ(const Matrix& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3M Qx = Rot3M::Rx(-x); + Matrix B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3M Qy = Rot3M::Ry(-y); + Matrix C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3M Qz = Rot3M::Rz(-z); + Matrix R = C * Qz.matrix(); + + Vector xyz = Vector_(3, x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index e41e01ef3..02ca733d1 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -226,10 +226,20 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3M::ypr(y,p,r) */ Vector rpy() const; + /** + * Accessors to get to components of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient + */ + inline double roll() const { return ypr()(2); } + inline double pitch() const { return ypr()(1); } + inline double yaw() const { return ypr()(0); } + /** Compute the quaternion representation of this rotation. * @return The quaternion */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index cb336bb7e..94b81cb0c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -51,7 +51,6 @@ TEST( Pose3, expmap_a) v(0) = 0.3; EXPECT(assert_equal(id.retract(v), Pose3(R, Point3()))); #ifdef CORRECT_POSE3_EXMAP - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; #else v(3)=0.2;v(4)=0.7;v(5)=-2; @@ -100,89 +99,6 @@ namespace screw { Pose3 expected(expectedR, expectedT); } -#ifdef CORRECT_POSE3_EXMAP -/* ************************************************************************* */ -TEST(Pose3, expmap_c) -{ - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); -} - -/* ************************************************************************* */ -// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) -TEST(Pose3, Adjoint) -{ - Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); - Vector xiprime = Adjoint(T, screw::xi); - EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - - Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); - Vector xiprime2 = Adjoint(T2, screw::xi); - EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - - Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); - Vector xiprime3 = Adjoint(T3, screw::xi); - EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); -} - -/* ************************************************************************* */ -/** Agrawal06iros version */ -Pose3 Agrawal06iros(const Vector& xi) { - Vector w = vector_range(xi, range(0,3)); - Vector v = vector_range(xi, range(3,6)); - double t = norm_2(w); - if (t < 1e-5) - return Pose3(Rot3(), expmap (v)); - else { - Matrix W = skewSymmetric(w/t); - Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), expmap (A * v)); - } -} - -/* ************************************************************************* */ -TEST(Pose3, expmaps_galore) -{ - Vector xi; Pose3 actual; - xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); - actual = Pose3::Expmap(xi); - EXPECT(assert_equal(expm(xi), actual,1e-6)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); - - xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); - for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { - Vector txi = xi*theta; - actual = Pose3::Expmap(txi); - EXPECT(assert_equal(expm(txi,30), actual,1e-6)); - EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); - Vector log = localCoordinates(actual); - EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6)); - EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps - } - - // Works with large v as well, but expm needs 10 iterations! - xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); - actual = Pose3::Expmap(xi); - EXPECT(assert_equal(expm(xi,10), actual,1e-5)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); -} - -/* ************************************************************************* */ -TEST(Pose3, Adjoint_compose) -{ - // To debug derivatives of compose, assert that - // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 - const Pose3& T1 = T; - Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); - Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = Adjoint(inverse(T2), x); - Pose3 actual = T1 * T2 * Pose3::Expmap(y); - EXPECT(assert_equal(expected, actual, 1e-6)); -} -#endif // SLOW_BUT_CORRECT_EXMAP - /* ************************************************************************* */ TEST(Pose3, expmap_c_full) { @@ -195,15 +111,15 @@ TEST(Pose3, expmap_c_full) TEST(Pose3, Adjoint_full) { Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); - Vector xiprime = T.Adjoint(screw::xi); + Vector xiprime = T.adjoint(screw::xi); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); - Vector xiprime2 = T2.Adjoint(screw::xi); + Vector xiprime2 = T2.adjoint(screw::xi); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); - Vector xiprime3 = T3.Adjoint(screw::xi); + Vector xiprime3 = T3.adjoint(screw::xi); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } @@ -259,7 +175,7 @@ TEST(Pose3, Adjoint_compose_full) const Pose3& T1 = T; Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = T2.inverse().Adjoint(x); + Vector y = T2.inverse().adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); EXPECT(assert_equal(expected, actual, 1e-6)); } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 2f6b9735e..8cb9e3278 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -178,6 +178,11 @@ namespace gtsam { /** check equality */ bool equals(const BayesTree& other, double tol = 1e-9) const; + /** deep copy from another tree */ + void cloneTo(shared_ptr& newTree) const { + root_->cloneToBayesTree(*newTree); + } + /** * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 549814312..83496dfc0 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -77,6 +77,20 @@ namespace gtsam { */ static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } + void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const { + sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_)); + sharedClique newClique = newTree.addClique(newConditional, parent_clique); + if (cachedFactor_) + newClique->cachedFactor_ = cachedFactor_->clone(); + else newClique->cachedFactor_ = typename FactorType::shared_ptr(); + if (!parent_clique) { + newTree.root_ = newClique; + } + BOOST_FOREACH(const shared_ptr& childClique, children_) { + childClique->cloneToBayesTree(newTree, newClique); + } + } + /** print this node */ void print(const std::string& s = "") const; diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 421d03ad5..99d717681 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -174,6 +174,13 @@ public: /** Access the container through the permutation (const version) */ const value_type& operator[](size_t index) const { return container_[permutation_[index]]; } + /** Assignment operator for cloning in ISAM2 */ + Permuted operator=(const Permuted& other) { + permutation_ = other.permutation_; + container_ = other.container_; + return *this; + } + /** Permute this view by applying a permutation to the underlying permutation */ void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 7f4b90748..2d889ee64 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -88,6 +88,9 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** Clone a factor (make a deep copy) */ + virtual GaussianFactor::shared_ptr clone() const = 0; + /** * Permutes the GaussianFactor, but for efficiency requires the permutation * to already be inverted. This acts just as a change-of-name for each diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index f2de0be4c..88e4364ae 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -191,6 +191,12 @@ namespace gtsam { /** Destructor */ virtual ~HessianFactor() {} + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new HessianFactor(*this))); + } + /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "") const; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b8f40d6ef..09089df20 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -136,6 +136,12 @@ namespace gtsam { virtual ~JacobianFactor() {} + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new JacobianFactor(*this))); + } + // Implementing Testable interface virtual void print(const std::string& s = "") const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index d4d177815..fe29936cb 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -27,12 +27,18 @@ namespace gtsam { */ template > class GaussianISAM2 : public ISAM2 { + typedef ISAM2 Base; public: /** Create an empty ISAM2 instance */ GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ GaussianISAM2() : ISAM2() {} + + void cloneTo(boost::shared_ptr& newGaussianISAM2) const { + boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); + Base::cloneTo(isam2); + } }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3e3d34996..f3e960db4 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -214,6 +214,27 @@ public: typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class + void cloneTo(boost::shared_ptr& newISAM2) const { + boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); + Base::cloneTo(bayesTree); + newISAM2->theta_ = theta_; + newISAM2->variableIndex_ = variableIndex_; + newISAM2->deltaUnpermuted_ = deltaUnpermuted_; + newISAM2->delta_ = delta_; + newISAM2->nonlinearFactors_ = nonlinearFactors_; + newISAM2->ordering_ = ordering_; + newISAM2->params_ = params_; +#ifndef NDEBUG + newISAM2->lastRelinVariables_ = lastRelinVariables_; +#endif + newISAM2->lastAffectedVariableCount = lastAffectedVariableCount; + newISAM2->lastAffectedFactorCount = lastAffectedFactorCount; + newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount; + newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount; + newISAM2->lastBacksubVariableCount = lastBacksubVariableCount; + newISAM2->lastNnzTop = lastNnzTop; + } + /** * Add new factors, updating the solution and relinearizing as needed. * @@ -279,6 +300,8 @@ public: size_t lastBacksubVariableCount; size_t lastNnzTop; + ISAM2Params params() const { return params_; } + //@} private: diff --git a/gtsam/slam/Landmark2.h b/gtsam/slam/Landmark2.h deleted file mode 100644 index f67cfed56..000000000 --- a/gtsam/slam/Landmark2.h +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Landmark2.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::Point2 Landmark2; - -} - diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 406431edd..fa51a9512 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -15,24 +15,12 @@ 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 +sources += simulated3D.cpp check_PROGRAMS += tests/testSimulated3D # Generic SLAM headers @@ -50,12 +38,6 @@ check_PROGRAMS += tests/testPose2SLAM sources += planarSLAM.cpp check_PROGRAMS += tests/testPlanarSLAM -# MATLAB Wrap headers for planarSLAM -headers += Landmark2.h -headers += PlanarSLAMGraph.h -headers += PlanarSLAMValues.h -headers += PlanarSLAMOdometry.h - # 3D Pose constraints sources += pose3SLAM.cpp check_PROGRAMS += tests/testPose3SLAM diff --git a/gtsam/slam/PlanarSLAMGraph.h b/gtsam/slam/PlanarSLAMGraph.h deleted file mode 100644 index 98eb9aaef..000000000 --- a/gtsam/slam/PlanarSLAMGraph.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMGraph.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Graph PlanarSLAMGraph; - -} - diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h deleted file mode 100644 index 941eda285..000000000 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Odometry PlanarSLAMOdometry; - -} - diff --git a/gtsam/slam/PlanarSLAMValues.h b/gtsam/slam/PlanarSLAMValues.h deleted file mode 100644 index d174229fd..000000000 --- a/gtsam/slam/PlanarSLAMValues.h +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMValues.h - * @brief Convenience for MATLAB wrapper, which does not allow for identically named methods - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Values PlanarSLAMValues; - -} - diff --git a/gtsam/slam/Simulated2DMeasurement.h b/gtsam/slam/Simulated2DMeasurement.h deleted file mode 100644 index 0ab661086..000000000 --- a/gtsam/slam/Simulated2DMeasurement.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DMeasurement.h - * @brief 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 deleted file mode 100644 index 5bd3afc14..000000000 --- a/gtsam/slam/Simulated2DOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DOdometry.h - * @brief 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 deleted file mode 100644 index 1a659c081..000000000 --- a/gtsam/slam/Simulated2DOrientedOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DOrientedOdometry.h - * @brief 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 deleted file mode 100644 index 54a83f1be..000000000 --- a/gtsam/slam/Simulated2DOrientedPosePrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DOrientedPosePrior.h - * @brief 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 deleted file mode 100644 index b1fe098f8..000000000 --- a/gtsam/slam/Simulated2DOrientedValues.h +++ /dev/null @@ -1,59 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DValues.h - * @brief 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 deleted file mode 100644 index 8fc896d92..000000000 --- a/gtsam/slam/Simulated2DPointPrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DPointPrior.h - * @brief 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 deleted file mode 100644 index 1c85e4d5b..000000000 --- a/gtsam/slam/Simulated2DPosePrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DPosePrior.h - * @brief 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 deleted file mode 100644 index c4d1d1534..000000000 --- a/gtsam/slam/Simulated2DValues.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DValues.h - * @brief 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/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp index 76e65a8cf..e2f6211a5 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/Simulated3D.cpp @@ -15,7 +15,7 @@ * @author Alex Cunningham **/ -#include +#include namespace gtsam { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 228ced08c..5776e989c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -71,10 +71,10 @@ namespace gtsam { // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - boost::shared_ptr pose(int key) { - Pose2 pose = (*this)[PoseKey(key)]; - return boost::shared_ptr(new Pose2(pose)); - } + Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + + /// get a point + Point2 point(int key) const { return (*this)[PointKey(key)]; } /// insert a pose void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } @@ -131,14 +131,11 @@ namespace gtsam { void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); - /// Optimize_ for MATLAB - boost::shared_ptr optimize_(const Values& initialEstimate) { + /// Optimize + Values optimize(const Values& initialEstimate) { typedef NonlinearOptimizer Optimizer; - boost::shared_ptr result( - new Values( - *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA))); - return result; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); } }; diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 3b63c9595..cae60bb80 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -21,6 +21,7 @@ #include #include #include +#include // \namespace @@ -72,13 +73,13 @@ namespace gtsam { } /// Return pose i - sharedPoint pose(const simulated2D::PoseKey& i) { - return sharedPoint(new Point2((*this)[i])); + Point2 pose(const simulated2D::PoseKey& i) const { + return (*this)[i]; } /// Return point j - sharedPoint point(const simulated2D::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); + Point2 point(const simulated2D::PointKey& j) const { + return (*this)[j]; } }; @@ -232,5 +233,12 @@ namespace gtsam { typedef GenericOdometry Odometry; typedef GenericMeasurement Measurement; + // Specialization of a graph for this example domain + // TODO: add functions to add factor types + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + }; + } // namespace simulated2D } // namespace gtsam diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 3710f29ad..b9cf150ef 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -21,6 +21,7 @@ #include #include #include +#include // \namespace @@ -33,7 +34,27 @@ namespace gtsam { typedef TypedSymbol PoseKey; typedef Values PoseValues; typedef Values PointValues; - typedef TupleValues2 Values; + + /// Specialized Values structure with syntactic sugar for + /// compatibility with matlab + class Values: public TupleValues2 { + public: + Values() {} + + void insertPose(const PoseKey& i, const Pose2& p) { + insert(i, p); + } + + void insertPoint(const PointKey& j, const Point2& p) { + insert(j, p); + } + + int nrPoses() const { return this->first_.size(); } + int nrPoints() const { return this->second_.size(); } + + Pose2 pose(const PoseKey& i) const { return (*this)[i]; } + Point2 point(const PointKey& j) const { return (*this)[j]; } + }; //TODO:: point prior is not implemented right now @@ -100,5 +121,12 @@ namespace gtsam { typedef GenericOdometry Odometry; + /// Graph specialization for syntactic sugar use with matlab + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + // TODO: add functions to add factors + }; + } // namespace simulated2DOriented } // namespace gtsam diff --git a/gtsam/slam/simulated3D.cpp b/gtsam/slam/simulated3D.cpp new file mode 100644 index 000000000..e2f6211a5 --- /dev/null +++ b/gtsam/slam/simulated3D.cpp @@ -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 Simulated3D.cpp +* @brief measurement functions and derivatives for simulated 3D robot +* @author Alex Cunningham +**/ + +#include + +namespace gtsam { + +namespace simulated3D { + +Point3 prior (const Point3& x, boost::optional H) { + if (H) *H = eye(3); + return x; +} + +Point3 odo(const Point3& x1, const Point3& x2, + boost::optional H1, boost::optional H2) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); + return x2 - x1; +} + +Point3 mea(const Point3& x, const Point3& l, + boost::optional H1, boost::optional H2) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); + return l - x; +} + +}} // namespace gtsam::simulated3D diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h new file mode 100644 index 000000000..32f05e967 --- /dev/null +++ b/gtsam/slam/simulated3D.h @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * 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 Simulated3D.h + * @brief measurement functions and derivatives for simulated 3D robot + * @author Alex Cunningham + **/ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +// \namespace + +namespace gtsam { +namespace simulated3D { + +/** + * This is a linear SLAM domain where both poses and landmarks are + * 3D points, without rotation. The structure and use is based on + * the simulated2D domain. + */ + +typedef gtsam::TypedSymbol PoseKey; +typedef gtsam::TypedSymbol PointKey; + +typedef Values PoseValues; +typedef Values PointValues; +typedef TupleValues2 Values; + +/** + * Prior on a single pose + */ +Point3 prior(const Point3& x, boost::optional H = boost::none); + +/** + * odometry between two poses + */ +Point3 odo(const Point3& x1, const Point3& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none); + +/** + * measurement between landmark and pose + */ +Point3 mea(const Point3& x, const Point3& l, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none); + +/** + * A prior factor on a single linear robot pose + */ +struct PointPrior3D: public NonlinearFactor1 { + + Point3 z_; ///< The prior pose value for the variable attached to this factor + + /** + * Constructor for a prior factor + * @param z is the measured/prior position for the pose + * @param model is the measurement model for the factor (Dimension: 3) + * @param j is the key for the pose + */ + PointPrior3D(const Point3& z, + const SharedNoiseModel& model, const PoseKey& j) : + NonlinearFactor1 (model, j), z_(z) { + } + + /** + * Evaluates the error at a given value of x, + * with optional derivatives. + * @param x is the current value of the variable + * @param H is an optional Jacobian matrix (Dimension: 3x3) + * @return Vector error between prior value and x (Dimension: 3) + */ + Vector evaluateError(const Point3& x, boost::optional H = + boost::none) { + return (prior(x, H) - z_).vector(); + } +}; + +/** + * Models a linear 3D measurement between 3D points + */ +struct Simulated3DMeasurement: public NonlinearFactor2 { + + Point3 z_; ///< Linear displacement between a pose and landmark + + /** + * Creates a measurement factor with a given measurement + * @param z is the measurement, a linear displacement between poses and landmarks + * @param model is a measurement model for the factor (Dimension: 3) + * @param j1 is the pose key of the robot + * @param j2 is the point key for the landmark + */ + Simulated3DMeasurement(const Point3& z, + const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : + NonlinearFactor2 ( + model, j1, j2), z_(z) { + } + + /** + * Error function with optional derivatives + * @param x1 a robot pose value + * @param x2 a landmark point value + * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3) + * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) + * @return vector error between measurement and prediction (Dimension: 3) + */ + Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { + return (mea(x1, x2, H1, H2) - z_).vector(); + } +}; + +}} // namespace simulated3D diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index de2c534bb..30f41e2fd 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/myconfigure.matlab b/myconfigure.matlab index 33addb665..a94644b77 100755 --- a/myconfigure.matlab +++ b/myconfigure.matlab @@ -1 +1,12 @@ -../configure --prefix=$HOME --with-toolbox=$HOME/toolbox --enable-build-toolbox CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install +../configure --prefix=$HOME \ + --with-toolbox=$HOME/toolbox \ + --enable-build-toolbox \ + --enable-install-matlab-tests \ + --enable-install-matlab-examples \ + --enable-install-wrap \ + --with-wrap=$HOME/bin \ + CFLAGS="-fno-inline -g -Wall" \ + CXXFLAGS="-fno-inline -g -Wall" \ + LDFLAGS="-fno-inline -g -Wall" \ + --disable-static \ + --disable-fast-install diff --git a/tests/matlab/test_gtsam.m b/tests/matlab/test_gtsam.m new file mode 100644 index 000000000..aab3dac0f --- /dev/null +++ b/tests/matlab/test_gtsam.m @@ -0,0 +1,10 @@ +% Test runner script - runs each test + +display 'Starting: testJacobianFactor' +testJacobianFactor + +display 'Starting: testKalmanFilter' +testKalmanFilter + +% end of tests +display 'Tests complete!' diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 378afade3..121717cf3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution) EXPECT(isam_check(fullgraph, fullinit, isam)); } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, clone) { + + // Pose and landmark key types from planarSLAM + typedef planarSLAM::PoseKey PoseKey; + typedef planarSLAM::PointKey PointKey; + + // Set up parameters + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + GaussianISAM2 isam(ISAM2Params(0.001, 0.0, 0, false, true)); + planarSLAM::Values fullinit; + planarSLAM::Graph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + EXPECT(isam_check(fullgraph, fullinit, isam)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + + // CLONING... + boost::shared_ptr > isam2 + = boost::shared_ptr >(new GaussianISAM2()); + isam.cloneTo(isam2); + + CHECK(assert_equal(isam, *isam2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index e0d01ab63..1610f98aa 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -22,34 +22,43 @@ #include "Argument.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -void Argument::matlab_unwrap(ofstream& ofs, - const string& matlabName) -{ +void Argument::matlab_unwrap(ofstream& ofs, const string& matlabName) const { ofs << " "; + string cppType = qualifiedType("::"); + string matlabType = qualifiedType(); + if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - ofs << "shared_ptr<" << type << "> " << name << " = unwrap_shared_ptr< "; + ofs << "shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer - ofs << type << "& " << name << " = *unwrap_shared_ptr< "; + ofs << cppType << "& " << name << " = *unwrap_shared_ptr< "; else // Not a pointer or a reference: emit an "unwrap" call // unwrap is specified in matlab.h as a series of template specializations // that know how to unpack the expected MATLAB object // example: double tol = unwrap< double >(in[2]); // example: Vector v = unwrap< Vector >(in[1]); - ofs << type << " " << name << " = unwrap< "; + ofs << cppType << " " << name << " = unwrap< "; - ofs << type << " >(" << matlabName; - if (is_ptr || is_ref) ofs << ", \"" << type << "\""; + ofs << cppType << " >(" << matlabName; + if (is_ptr || is_ref) ofs << ", \"" << matlabType << "\""; ofs << ");" << endl; } /* ************************************************************************* */ -string ArgumentList::types() { +string Argument::qualifiedType(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + return result + type; +} + +/* ************************************************************************* */ +string ArgumentList::types() const { string str; bool first=true; BOOST_FOREACH(Argument arg, *this) { @@ -59,7 +68,7 @@ string ArgumentList::types() { } /* ************************************************************************* */ -string ArgumentList::signature() { +string ArgumentList::signature() const { string str; BOOST_FOREACH(Argument arg, *this) str += arg.type[0]; @@ -67,7 +76,7 @@ string ArgumentList::signature() { } /* ************************************************************************* */ -string ArgumentList::names() { +string ArgumentList::names() const { string str; bool first=true; BOOST_FOREACH(Argument arg, *this) { @@ -77,7 +86,7 @@ string ArgumentList::names() { } /* ************************************************************************* */ -void ArgumentList::matlab_unwrap(ofstream& ofs, int start) { +void ArgumentList::matlab_unwrap(ofstream& ofs, int start) const { int index = start; BOOST_FOREACH(Argument arg, *this) { stringstream buf; diff --git a/wrap/Argument.h b/wrap/Argument.h index 2248e1790..cb5b1b8c8 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -18,27 +18,32 @@ #pragma once #include -#include +#include + +namespace wrap { /// Argument class struct Argument { bool is_const, is_ref, is_ptr; std::string type; std::string name; + std::vector namespaces; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + std::string qualifiedType(const std::string& delim = "") const; // adds namespaces to type + /// MATLAB code generation, MATLAB to C++ - void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName); + void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName) const; }; /// Argument list -struct ArgumentList: public std::list { - std::list args; - std::string types(); - std::string signature(); - std::string names(); +struct ArgumentList: public std::vector { + std::vector args; // why does it contain this? + std::string types() const; + std::string signature() const; + std::string names() const; // MATLAB code generation: @@ -47,6 +52,8 @@ struct ArgumentList: public std::list { * @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++ + void matlab_unwrap(std::ofstream& ofs, int start = 0) const; // MATLAB to C++ }; +} // \namespace wrap + diff --git a/wrap/Class.cpp b/wrap/Class.cpp index d701731d8..47e01f00b 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -14,6 +14,7 @@ * @author Frank Dellaert **/ +#include #include #include @@ -23,24 +24,28 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -void Class::matlab_proxy(const string& classFile) { +void Class::matlab_proxy(const string& classFile) const { // open destination classFile ofstream ofs(classFile.c_str()); if(!ofs) throw CantOpenFile(classFile); if(verbose_) cerr << "generating " << classFile << endl; + // get the name of actual matlab object + string matlabName = qualifiedName(); + // emit class proxy code - ofs << "classdef " << name << endl; + ofs << "classdef " << matlabName << endl; ofs << " properties" << endl; ofs << " self = 0" << endl; ofs << " end" << endl; ofs << " methods" << endl; - ofs << " function obj = " << name << "(varargin)" << endl; + ofs << " function obj = " << matlabName << "(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; + c.matlab_proxy_fragment(ofs,matlabName); + ofs << " if nargin ~= 13 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl; ofs << " end" << endl; ofs << " function display(obj), obj.print(''); end" << endl; ofs << " function disp(obj), obj.display; end" << endl; @@ -52,33 +57,98 @@ void Class::matlab_proxy(const string& classFile) { } /* ************************************************************************* */ -void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) { +void Class::matlab_constructors(const string& toolboxPath, const vector& using_namespaces) const { BOOST_FOREACH(Constructor c, constructors) { - c.matlab_mfile (toolboxPath, name); - c.matlab_wrapper(toolboxPath, name, nameSpace); + c.matlab_mfile (toolboxPath, qualifiedName()); + c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes); } } /* ************************************************************************* */ -void Class::matlab_methods(const string& classPath, const string& nameSpace) { +void Class::matlab_methods(const string& classPath, const vector& using_namespaces) const { + string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(Method m, methods) { m.matlab_mfile (classPath); - m.matlab_wrapper(classPath, name, nameSpace); + m.matlab_wrapper(classPath, name, cppName, matlabName, using_namespaces, includes); + } +} + +/* ************************************************************************* */ +void Class::matlab_static_methods(const string& toolboxPath, const vector& using_namespaces) const { + string matlabName = qualifiedName(), cppName = qualifiedName("::"); + BOOST_FOREACH(const StaticMethod& m, static_methods) { + m.matlab_mfile (toolboxPath, qualifiedName()); + m.matlab_wrapper(toolboxPath, name, matlabName, cppName, using_namespaces, includes); } } /* ************************************************************************* */ void Class::matlab_make_fragment(ofstream& ofs, const string& toolboxPath, - const string& mexFlags) -{ + const string& mexFlags) const { string mex = "mex " + mexFlags + " "; + string matlabClassName = qualifiedName(); BOOST_FOREACH(Constructor c, constructors) - ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl; - ofs << endl << "cd @" << name << endl; + ofs << mex << c.matlab_wrapper_name(matlabClassName) << ".cpp" << endl; + BOOST_FOREACH(StaticMethod sm, static_methods) + ofs << mex << matlabClassName + "_" + sm.name << ".cpp" << endl; + ofs << endl << "cd @" << matlabClassName << endl; BOOST_FOREACH(Method m, methods) - ofs << mex << m.name_ << ".cpp" << endl; + ofs << mex << m.name << ".cpp" << endl; ofs << endl; } /* ************************************************************************* */ +void Class::makefile_fragment(ofstream& ofs) const { +// new_Point2_.$(MEXENDING): new_Point2_.cpp +// $(MEX) $(mex_flags) new_Point2_.cpp +// new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp +// $(MEX) $(mex_flags) new_Point2_dd.cpp +// @Point2/x.$(MEXENDING): @Point2/x.cpp +// $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x +// @Point2/y.$(MEXENDING): @Point2/y.cpp +// $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y +// @Point2/dim.$(MEXENDING): @Point2/dim.cpp +// $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim +// +// Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) + + string matlabName = qualifiedName(); + + // collect names + vector file_names; + BOOST_FOREACH(Constructor c, constructors) { + string file_base = c.matlab_wrapper_name(matlabName); + file_names.push_back(file_base); + } + BOOST_FOREACH(StaticMethod c, static_methods) { + string file_base = matlabName + "_" + c.name; + file_names.push_back(file_base); + } + BOOST_FOREACH(Method c, methods) { + string file_base = "@" + matlabName + "/" + c.name; + file_names.push_back(file_base); + } + + BOOST_FOREACH(const string& file_base, file_names) { + ofs << file_base << ".$(MEXENDING): " << file_base << ".cpp" << endl; + ofs << "\t$(MEX) $(mex_flags) " << file_base << ".cpp -output " << file_base << endl; + } + + // class target + ofs << "\n" << matlabName << ": "; + BOOST_FOREACH(const string& file_base, file_names) { + ofs << file_base << ".$(MEXENDING) "; + } + ofs << "\n" << endl; +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; + return result + name; +} + +/* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index ca85b06b1..d8ebe1a10 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -18,10 +18,12 @@ #pragma once #include -#include #include "Constructor.h" #include "Method.h" +#include "StaticMethod.h" + +namespace wrap { /// Class has name, constructors, methods struct Class { @@ -29,19 +31,28 @@ struct Class { Class(bool verbose=true) : verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - std::string name; ///< Class name - std::list constructors; ///< Class constructors - std::list methods; ///< Class methods - bool verbose_; ///< verbose flag + std::string name; ///< Class name + std::vector constructors; ///< Class constructors + std::vector methods; ///< Class methods + std::vector static_methods; ///< Static methods + std::vector namespaces; ///< Stack of namespaces + std::vector includes; ///< header include overrides + bool verbose_; ///< verbose flag // And finally MATLAB code is emitted, methods below called by Module::matlab_code - void matlab_proxy(const std::string& classFile); ///< emit proxy class + void matlab_proxy(const std::string& classFile) const; ///< emit proxy class void matlab_constructors(const std::string& toolboxPath, - const std::string& nameSpace); ///< emit constructor wrappers + const std::vector& using_namespaces) const; ///< emit constructor wrappers void matlab_methods(const std::string& classPath, - const std::string& nameSpace); ///< emit method wrappers + const std::vector& using_namespaces) const; ///< emit method wrappers + void matlab_static_methods(const std::string& classPath, + const std::vector& using_namespaces) const; ///< emit static method wrappers void matlab_make_fragment(std::ofstream& ofs, const std::string& toolboxPath, - const std::string& mexFlags); ///< emit make fragment for global make script + const std::string& mexFlags) const; ///< emit make fragment for global make script + void makefile_fragment(std::ofstream& ofs) const; ///< emit makefile fragment + std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter }; +} // \namespace wrap + diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 9caf4793f..a822f722f 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -23,15 +23,16 @@ #include "Constructor.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -string Constructor::matlab_wrapper_name(const string& className) { +string Constructor::matlab_wrapper_name(const string& className) const { string str = "new_" + className + "_" + args.signature(); return str; } /* ************************************************************************* */ -void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) { +void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) const { ofs << " if nargin == " << args.size() << ", obj.self = " << matlab_wrapper_name(className) << "("; bool first = true; @@ -44,22 +45,22 @@ void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) } /* ************************************************************************* */ -void Constructor::matlab_mfile(const string& toolboxPath, const string& className) { +void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) const { - string name = matlab_wrapper_name(className); + string matlabName = matlab_wrapper_name(qualifiedMatlabName); // open destination m-file - string wrapperFile = toolboxPath + "/" + name + ".m"; + string wrapperFile = toolboxPath + "/" + matlabName + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "%"); - ofs << "function result = " << name << "(obj"; + generateHeaderComment(ofs, "%"); + ofs << "function result = " << matlabName << "(obj"; if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; - ofs << " error('need to compile " << name << ".cpp');" << endl; + ofs << " error('need to compile " << matlabName << ".cpp');" << endl; ofs << "end" << endl; // close file @@ -67,30 +68,29 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& classNam } /* ************************************************************************* */ -void Constructor::matlab_wrapper(const string& toolboxPath, - const string& className, - const string& nameSpace) -{ - - string name = matlab_wrapper_name(className); +void Constructor::matlab_wrapper(const string& toolboxPath, + const string& cppClassName, + const string& matlabClassName, + const vector& using_namespaces, const vector& includes) const { + string matlabName = matlab_wrapper_name(matlabClassName); // open destination wrapperFile - string wrapperFile = toolboxPath + "/" + name + ".cpp"; + string wrapperFile = toolboxPath + "/" + matlabName + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "//"); - ofs << "#include " << endl; - ofs << "#include <" << className << ".h>" << endl; - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateHeaderComment(ofs, "//"); + generateIncludes(ofs, name, includes); + generateUsingNamespace(ofs, using_namespaces); + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; ofs << "{" << endl; - ofs << " checkArguments(\"" << name << "\",nargout,nargin," << args.size() << ");" << endl; + ofs << " checkArguments(\"" << matlabName << "\",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 << " " << cppClassName << "* self = new " << cppClassName << "(" << args.names() << ");" << endl; // need qualified name, delim: "::" + ofs << " out[0] = wrap_constructed(self,\"" << matlabClassName << "\");" << endl; // need matlab qualified name ofs << "}" << endl; // close file diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 340ac0fd2..b9a44e2cc 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,10 +18,12 @@ #pragma once #include -#include +#include #include "Argument.h" +namespace wrap { + // Constructor class struct Constructor { @@ -32,6 +34,7 @@ struct Constructor { // Then the instance variables are set directly by the Module constructor ArgumentList args; + std::string name; bool verbose_; // MATLAB code generation @@ -39,17 +42,22 @@ struct Constructor { // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m /// wrapper name - std::string matlab_wrapper_name(const std::string& className); + std::string matlab_wrapper_name(const std::string& className) const; /// proxy class fragment - void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className); + void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className) const; /// m-file void matlab_mfile(const std::string& toolboxPath, - const std::string& className); + const std::string& qualifiedMatlabName) const; - /// wrapper + /// cpp wrapper void matlab_wrapper(const std::string& toolboxPath, - const std::string& className, const std::string& nameSpace); + const std::string& cppClassName, + const std::string& matlabClassName, + const std::vector& using_namespaces, + const std::vector& includes) const; }; +} // \namespace wrap + diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 895830537..8b3e4f493 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -9,15 +9,22 @@ AM_DEFAULT_SOURCE_EXT = .cpp headers = sources = -check_PROGRAMS = +check_PROGRAMS = +noinst_PROGRAMS = +wrap_PROGRAMS = +wrapdir = $(pkgincludedir)/wrap # 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 +sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp Method.cpp StaticMethod.cpp Class.cpp Module.cpp +check_PROGRAMS += tests/testSpirit tests/testWrap +if ENABLE_INSTALL_WRAP +wrap_PROGRAMS += wrap +else +noinst_PROGRAMS += wrap +endif #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed @@ -25,7 +32,6 @@ noinst_PROGRAMS = wrap #---------------------------------------------------------------------------------------------------- # Only install the header necessary for wrap interfaces to build with mex headers += matlab.h -wrapdir = $(pkgincludedir)/wrap wrap_HEADERS = $(headers) noinst_LTLIBRARIES = libwrap.la libwrap_la_SOURCES = $(sources) @@ -46,20 +52,79 @@ LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a # 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} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -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 + +# Set flags to pass to mex +mexFlags = +if ENABLE_UNSAFE_WRAP +mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +else +mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +endif + +# Find the extension for mex binaries +# this should be done with mexext with matlab +mexextension = +if LINUX +if IS_64BIT +mexextension += mexa64 +else +mexextension += mexglx +endif +else # Linux +if DARWIN +mexextension += mexmaci64 +else +mexextension += mex_bin +endif +endif # Linux + +all: generate_toolbox + +generate_toolbox: $(top_srcdir)/gtsam.h + ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${mexFlags} + +source_mode = -m 644 + +wrap-install-matlab-toolbox: generate_toolbox + install -d ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.m ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.cpp ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/Makefile ${toolbox}/gtsam + cp -ru ../toolbox/@* ${toolbox}/gtsam + +wrap-install-bin: wrap + install -d ${wrap} + install -c ./wrap ${wrap} + +wrap-install-matlab-tests: + install -d ${toolbox}/gtsam/tests + install ${source_mode} -c ../../tests/matlab/*.m ${toolbox}/gtsam/tests + +wrap-install-matlab-examples: + install -d ${toolbox}/gtsam/examples + install ${source_mode} -c ../../examples/matlab/*.m ${toolbox}/gtsam/examples + +wrap_install_targets = +wrap_install_targets += wrap-install-matlab-toolbox + +if ENABLE_INSTALL_WRAP +wrap_install_targets += wrap-install-bin +endif + +if ENABLE_INSTALL_MATLAB_TESTS +wrap_install_targets += wrap-install-matlab-tests +endif + +if ENABLE_INSTALL_MATLAB_EXAMPLES +wrap_install_targets += wrap-install-matlab-examples +endif + +install-exec-hook: ${wrap_install_targets} # clean local toolbox dir clean: diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 1e0cd134e..0d9ba98e2 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -23,44 +23,24 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -// 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) { +void Method::matlab_mfile(const string& classPath) const { // open destination m-file - string wrapperFile = classPath + "/" + name_ + ".m"; + string wrapperFile = classPath + "/" + name + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - string returnType = returns_pair_? "[first,second]" : "result"; - ofs << "function " << returnType << " = " << name_ << "(obj"; - if (args_.size()) ofs << "," << args_.names(); + string returnType = returnVal.matlab_returnType(); + ofs << "function " << returnType << " = " << name << "(obj"; + if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; - ofs << "% usage: obj." << name_ << "(" << args_.names() << ")" << endl; - ofs << " error('need to compile " << name_ << ".cpp');" << endl; + ofs << "% usage: obj." << name << "(" << args.names() << ")" << endl; + ofs << " error('need to compile " << name << ".cpp');" << endl; ofs << "end" << endl; // close file @@ -70,10 +50,11 @@ void Method::matlab_mfile(const string& classPath) { /* ************************************************************************* */ void Method::matlab_wrapper(const string& classPath, const string& className, - const string& nameSpace) -{ + const string& cppClassName, + const string& matlabClassName, + const vector& using_namespaces, const std::vector& includes) const { // open destination wrapperFile - string wrapperFile = classPath + "/" + name_ + ".cpp"; + string wrapperFile = classPath + "/" + name + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; @@ -81,10 +62,9 @@ void Method::matlab_wrapper(const string& classPath, // generate code // header - emit_header_comment(ofs, "//"); - ofs << "#include \n"; - ofs << "#include <" << className << ".h>\n"; - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateHeaderComment(ofs, "//"); + generateIncludes(ofs, className, includes); + generateUsingNamespace(ofs, using_namespaces); // call ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; @@ -94,39 +74,26 @@ void Method::matlab_wrapper(const string& classPath, // 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"; + 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; + ofs << " shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName + << " >(in[0],\"" << matlabClassName << "\");" << endl; // unwrap arguments, see Argument.cpp - args_.matlab_unwrap(ofs,1); + 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"; + if (returnVal.type1!="void") + ofs << returnVal.return_type(true,ReturnValue::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"; + returnVal.wrap_result(ofs); // finish ofs << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index 56679e67b..bcfac82e3 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -21,33 +21,35 @@ #include #include "Argument.h" +#include "ReturnValue.h" + +namespace wrap { /// Method class struct Method { /// Constructor creates empty object Method(bool verbose = true) : - returns_ptr_(false), returns_ptr2_(false), returns_pair_(false), verbose_( - verbose) { - } + verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - bool is_const_; - ArgumentList args_; - std::string returns_, returns2_, name_; - bool returns_ptr_, returns_ptr2_, returns_pair_; bool verbose_; - - enum pairing { - arg1, arg2, pair - }; - std::string return_type(bool add_ptr, pairing p); + bool is_const_; + std::string name; + ArgumentList args; + ReturnValue returnVal; // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 - void matlab_mfile(const std::string& classPath); ///< m-file + void matlab_mfile(const std::string& classPath) const; ///< m-file void matlab_wrapper(const std::string& classPath, - const std::string& className, const std::string& nameSpace); ///< wrapper + const std::string& className, + const std::string& cppClassName, + const std::string& matlabClassname, + const std::vector& using_namespaces, + const std::vector& includes) const; ///< cpp wrapper }; +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 7e35b0d2b..120b89f10 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -16,15 +16,19 @@ #include "Module.h" #include "utilities.h" +#include "pop_actor.h" //#define BOOST_SPIRIT_DEBUG #include +#include +#include #include #include #include using namespace std; +using namespace wrap; using namespace BOOST_SPIRIT_CLASSIC_NS; typedef rule Rule; @@ -38,102 +42,154 @@ typedef rule Rule; /* ************************************************************************* */ Module::Module(const string& interfacePath, - const string& moduleName, bool verbose) : name(moduleName), verbose_(verbose) + const string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) { // these variables will be imperatively updated to gradually build [cls] // The one with postfix 0 are used to reset the variables after parse. + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; - Constructor constructor0(verbose), constructor(verbose); - Method method0(verbose), method(verbose); - Class cls0(verbose),cls(verbose); + Constructor constructor0(enable_verbose), constructor(enable_verbose); + Method method0(enable_verbose), method(enable_verbose); + StaticMethod static_method0(enable_verbose), static_method(enable_verbose); + Class cls0(enable_verbose),cls(enable_verbose); + vector namespaces, namespaces_return; //---------------------------------------------------------------------------- // 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. + // The grammar is allows a very restricted C++ header // ---------------------------------------------------------------------------- + Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); + // 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 basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule keywords_p = + (str_p("const") | "static" | "namespace" | basisType_p); + + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p; + + Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; + + Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::"); Rule classPtr_p = + *namespace_arg_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)] >> + *namespace_arg_p >> 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 argEigenType_p = + eigenType_p[assign_a(arg.type)] >> + !ch_p('*')[assign_a(arg.is_ptr,true)]; - Rule ublasType = - (str_p("Vector") | "Matrix")[assign_a(arg.type)] >> - !ch_p('*')[assign_a(arg.is_ptr,true)]; + Rule eigenRef_p = + !str_p("const") [assign_a(arg.is_const,true)] >> + eigenType_p [assign_a(arg.type)] >> + ch_p('&') [assign_a(arg.is_ref,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)]) + ((basisType_p[assign_a(arg.type)] | argEigenType_p | classRef_p | eigenRef_p | classPtr_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 >> ')' >> ';') + (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) [assign_a(constructor.args,args)] + [assign_a(constructor.name,cls.name)] [assign_a(args,args0)] [push_back_a(cls.constructors, constructor)] [assign_a(constructor,constructor0)]; + Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); + 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)]); + (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::BASIS)]) | + ((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::CLASS)]) >> + !ch_p('*')[assign_a(retVal.isPtr1,true)]) | + (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::EIGEN)]); 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)]); + (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::BASIS)]) | + ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >> + !ch_p('*') [assign_a(retVal.isPtr2,true)]) | + (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::EIGEN)]); Rule pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(method.returns_pair_,true)]; + [assign_a(retVal.isPair,true)]; - Rule void_p = str_p("void")[assign_a(method.returns_)]; + Rule void_p = str_p("void")[assign_a(retVal.type1)]; 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_)] >> + (returnType_p >> methodName_p[assign_a(method.name)] >> '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(method.is_const_,true)] >> ';') - [assign_a(method.args_,args)] + !str_p("const")[assign_a(method.is_const_,true)] >> ';' >> *comments_p) + [assign_a(method.args,args)] [assign_a(args,args0)] + [assign_a(method.returnVal,retVal)] + [assign_a(retVal,retVal0)] [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 staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - Rule module_p = +class_p - [push_back_a(classes,cls)] - [assign_a(cls,cls0)] - >> !end_p; + Rule static_method_p = + (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name)] >> + '(' >> argumentList_p >> ')' >> ';' >> *comments_p) + [assign_a(static_method.args,args)] + [assign_a(args,args0)] + [assign_a(static_method.returnVal,retVal)] + [assign_a(retVal,retVal0)] + [push_back_a(cls.static_methods, static_method)] + [assign_a(static_method,static_method0)]; + + Rule includes_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(cls.includes)] >> ch_p('>'); + + Rule functions_p = includes_p | constructor_p | method_p | static_method_p; + + Rule class_p = (str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> + *(functions_p | comments_p) >> + str_p("};"))[assign_a(cls.namespaces, namespaces)][push_back_a(classes,cls)][assign_a(cls,cls0)]; + + Rule namespace_def_p = str_p("namespace") >> + namespace_name_p[push_back_a(namespaces)] + >> ch_p('{') >> + *(class_p | namespace_def_p | comments_p) >> + str_p("}///\\namespace") >> !namespace_name_p // end namespace, avoid confusion with classes + [pop_a(namespaces)]; + + Rule using_namespace_p = str_p("using") >> str_p("namespace") + >> namespace_name_p[push_back_a(using_namespaces)] >> ch_p(';'); + + Rule module_content_p = comments_p | using_namespace_p | class_p | namespace_def_p ; + + Rule module_p = *module_content_p >> !end_p; //---------------------------------------------------------------------------- // for debugging, define BOOST_SPIRIT_DEBUG @@ -162,34 +218,45 @@ Module::Module(const string& interfacePath, 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); + parse_info info = parse(contents.c_str(), module_p, space_p); if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); throw ParseFailed(info.length); } } +/* ************************************************************************* */ +template +void verifyArguments(const vector& validArgs, const vector& vt) { + BOOST_FOREACH(const T& t, vt) { + BOOST_FOREACH(Argument arg, t.args) { + string fullType = arg.qualifiedType("::"); + if(find(validArgs.begin(), validArgs.end(), fullType) + == validArgs.end()) + throw DependencyMissing(fullType, t.name); + } + } +} + /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, - const string& nameSpace, - const string& mexFlags) -{ - try { + const string& mexExt, const string& mexFlags) const { 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); + string matlabMakeFile = toolboxPath + "/make_" + name + ".m"; + ofstream ofs(matlabMakeFile.c_str()); + if(!ofs) throw CantOpenFile(matlabMakeFile); - if (verbose_) cerr << "generating " << makeFile << endl; - emit_header_comment(ofs,"%"); + // create the (actual) make file + string makeFile = toolboxPath + "/Makefile"; + ofstream make_ofs(makeFile.c_str()); + if(!make_ofs) throw CantOpenFile(makeFile); + + if (verbose) cerr << "generating " << matlabMakeFile << endl; + generateHeaderComment(ofs,"%"); ofs << "echo on" << endl << endl; ofs << "toolboxpath = mfilename('fullpath');" << endl; ofs << "delims = find(toolboxpath == '/');" << endl; @@ -197,36 +264,76 @@ void Module::matlab_code(const string& toolboxPath, ofs << "clear delims" << endl; ofs << "addpath(toolboxpath);" << endl << endl; + if (verbose) cerr << "generating " << makeFile << endl; + generateHeaderComment(make_ofs,"#"); + make_ofs << "\nMEX = mex\n"; + make_ofs << "MEXENDING = " << mexExt << "\n"; + make_ofs << "mex_flags = " << mexFlags << "\n\n"; + + //Dependency check list + vector validArgs; + validArgs.push_back("string"); + validArgs.push_back("int"); + validArgs.push_back("bool"); + validArgs.push_back("size_t"); + validArgs.push_back("double"); + validArgs.push_back("Vector"); + validArgs.push_back("Matrix"); + + // add 'all' to Makefile + make_ofs << "all: "; + BOOST_FOREACH(Class cls, classes) { + make_ofs << cls.qualifiedName() << " "; + //Create a list of parsed classes for dependency checking + validArgs.push_back(cls.qualifiedName("::")); + } + make_ofs << "\n\n"; + // generate proxy classes and wrappers BOOST_FOREACH(Class cls, classes) { // create directory if needed - string classPath = toolboxPath + "/@" + cls.name; + string classPath = toolboxPath + "/@" + cls.qualifiedName(); string installCmd = "install -d " + classPath; system(installCmd.c_str()); // create proxy class - string classFile = classPath + "/" + cls.name + ".m"; + string classFile = classPath + "/" + cls.qualifiedName() + ".m"; cls.matlab_proxy(classFile); + // verify all of the function arguments + verifyArguments(validArgs, cls.constructors); + verifyArguments(validArgs, cls.static_methods); + verifyArguments(validArgs, cls.methods); + // create constructor and method wrappers - cls.matlab_constructors(toolboxPath,nameSpace); - cls.matlab_methods(classPath,nameSpace); + cls.matlab_constructors(toolboxPath,using_namespaces); + cls.matlab_static_methods(toolboxPath,using_namespaces); + cls.matlab_methods(classPath,using_namespaces); // add lines to make m-file - ofs << "%% " << cls.name << endl; + ofs << "%% " << cls.qualifiedName() << endl; ofs << "cd(toolboxpath)" << endl; cls.matlab_make_fragment(ofs, toolboxPath, mexFlags); + + // add section to the (actual) make file + make_ofs << "# " << cls.qualifiedName() << endl; + cls.makefile_fragment(make_ofs); } // 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; - } -} + // make clean at end of Makefile + make_ofs << "\n\nclean: \n"; + make_ofs << "\trm -rf *.$(MEXENDING)\n"; + BOOST_FOREACH(Class cls, classes) + make_ofs << "\trm -rf @" << cls.qualifiedName() << "/*.$(MEXENDING)\n"; + + // finish Makefile + make_ofs << "\n" << endl; + make_ofs.close(); + } /* ************************************************************************* */ diff --git a/wrap/Module.h b/wrap/Module.h index 0227e86d9..ec057b644 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,26 +18,30 @@ #pragma once #include -#include +#include #include "Class.h" +namespace wrap { + /** * A module just has a name and a list of classes */ struct Module { std::string name; ///< module name - std::list classes; ///< list of classes - bool verbose_; ///< verbose flag + std::vector classes; ///< list of classes + bool verbose; ///< verbose flag + std::vector using_namespaces; ///< all default namespaces /// constructor that parses interface file - Module(const std::string& interfacePath, + Module(const std::string& interfacePath, const std::string& moduleName, - bool verbose=true); + bool enable_verbose=true); /// MATLAB code generation: void matlab_code(const std::string& path, - const std::string& nameSpace, - const std::string& mexFlags); + const std::string& mexExt, + const std::string& mexFlags) const; }; +} // \namespace wrap diff --git a/wrap/README b/wrap/README index 6996e3649..c47b34ddd 100644 --- a/wrap/README +++ b/wrap/README @@ -17,10 +17,10 @@ OBJECT CREATION new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there METHOD (AND CONSTRUCTOR) ARGUMENTS -- simple argument types of methods, such as "double", will be converted in the +- Simple argument types of methods, such as "double", will be converted in the mex warppers by calling unwrap, defined in matlab.h -- Vector and Matric arguments are normally passed by reference in GTSAM, but - in gtsam.h you need to pretedn they are passed by value, to trigger the +- Vector and Matrix arguments are normally passed by reference in GTSAM, but + in gtsam.h you need to pretend they are passed by value, to trigger the generation of the correct conversion routines unwrap and unwrap - passing classes as arguments works, provided they are passed by reference. This triggers a call to unwrap_shared_ptr diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp new file mode 100644 index 000000000..38d85d074 --- /dev/null +++ b/wrap/ReturnValue.cpp @@ -0,0 +1,78 @@ +/** + * @file ReturnValue.cpp + * + * @date Dec 1, 2011 + * @author Alex Cunningham + */ + +#include + +#include "ReturnValue.h" +#include "utilities.h" + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +string ReturnValue::return_type(bool add_ptr, pairing p) const { + if (p==pair && isPair) { + string str = "pair< " + + maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::")) + ", " + + maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::")) + " >"; + return str; + } else + return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::")); +} + +/* ************************************************************************* */ +string ReturnValue::matlab_returnType() const { + return isPair? "[first,second]" : "result"; +} + +/* ************************************************************************* */ +string ReturnValue::qualifiedType1(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim; + return result + type1; +} + +/* ************************************************************************* */ +string ReturnValue::qualifiedType2(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim; + return result + type2; +} + +/* ************************************************************************* */ +void ReturnValue::wrap_result(ostream& ofs) const { + string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(); + string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(); + + if (isPair) { + // first return value in pair + if (isPtr1) // if we already have a pointer + ofs << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n"; + else if (category1 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n"; + else // if basis type + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; + + // second return value in pair + if (isPtr2) // if we already have a pointer + ofs << " out[1] = wrap_shared_ptr(result.second,\"" << type2 << "\");\n"; + else if (category2 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[1] = wrap_shared_ptr(make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n"; + else + ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; + } + else if (isPtr1) + ofs << " out[0] = wrap_shared_ptr(result,\"" << type1 << "\");\n"; + else if (category1 == ReturnValue::CLASS) + ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n"; + else if (type1!="void") + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; +} + +/* ************************************************************************* */ + + diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h new file mode 100644 index 000000000..eb2406d80 --- /dev/null +++ b/wrap/ReturnValue.h @@ -0,0 +1,53 @@ +/** + * @file ReturnValue.h + * + * @brief Encapsulates a return value from a method + * + * @date Dec 1, 2011 + * @author Alex Cunningham + */ + +#include +#include + +#pragma once + +namespace wrap { + +struct ReturnValue { + + typedef enum { + CLASS, + EIGEN, + BASIS, + VOID + } return_category; + + ReturnValue(bool enable_verbosity = true) + : verbose(enable_verbosity), isPtr1(false), isPtr2(false), + isPair(false), category1(VOID), category2(VOID) + {} + + bool verbose; + std::string type1, type2; + bool isPtr1, isPtr2, isPair; + std::vector namespaces1, namespaces2; + + return_category category1, category2; + + typedef enum { + arg1, arg2, pair + } pairing; + + std::string return_type(bool add_ptr, pairing p) const; + + std::string qualifiedType1(const std::string& delim = "") const; + std::string qualifiedType2(const std::string& delim = "") const; + + std::string matlab_returnType() const; + + void wrap_result(std::ostream& ofs) const; + +}; + +} // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp new file mode 100644 index 000000000..ff4d31592 --- /dev/null +++ b/wrap/StaticMethod.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 Method.ccp + * @author Frank Dellaert + **/ + +#include +#include + +#include + +#include "StaticMethod.h" +#include "utilities.h" + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) const { + + // open destination m-file + string full_name = className + "_" + name; + string wrapperFile = toolboxPath + "/" + full_name + ".m"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + if(verbose) cerr << "generating " << wrapperFile << endl; + + // generate code + string returnType = returnVal.matlab_returnType(); + ofs << "function " << returnType << " = " << full_name << "("; + if (args.size()) ofs << args.names(); + ofs << ")" << endl; + ofs << "% usage: x = " << full_name << "(" << args.names() << ")" << endl; + ofs << " error('need to compile " << full_name << ".cpp');" << endl; + ofs << "end" << endl; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ +void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, + const string& matlabClassName, const string& cppClassName, + const vector& using_namespaces, + const vector& includes) const { + // open destination wrapperFile + string full_name = matlabClassName + "_" + name; + string wrapperFile = toolboxPath + "/" + full_name + ".cpp"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + if(verbose) cerr << "generating " << wrapperFile << endl; + + // generate code + + // header + generateHeaderComment(ofs, "//"); + generateIncludes(ofs, className, includes); + generateUsingNamespace(ofs, using_namespaces); + + // call + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + ofs << "{\n"; + + // check arguments + // NOTE: for static functions, there is no object passed + ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args.size() << ");\n"; + + // unwrap arguments, see Argument.cpp + args.matlab_unwrap(ofs,0); // We start at 0 because there is no self object + + ofs << " "; + + // call method with default type + if (returnVal.type1!="void") + ofs << returnVal.return_type(true,ReturnValue::pair) << " result = "; + ofs << cppClassName << "::" << name << "(" << args.names() << ");\n"; + + // wrap result + // example: out[0]=wrap(result); + returnVal.wrap_result(ofs); + + // finish + ofs << "}\n"; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h new file mode 100644 index 000000000..f9cc14af7 --- /dev/null +++ b/wrap/StaticMethod.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * 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 StaticMethod.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + **/ + +#pragma once + +#include +#include + +#include "Argument.h" +#include "ReturnValue.h" + +namespace wrap { + +/// StaticMethod class +struct StaticMethod { + + /// Constructor creates empty object + StaticMethod(bool verbosity = true) : + verbose(verbosity) {} + + // Then the instance variables are set directly by the Module constructor + bool verbose; + std::string name; + ArgumentList args; + ReturnValue returnVal; + + // MATLAB code generation + // toolboxPath is the core toolbox directory, e.g., ../matlab + // NOTE: static functions are not inside the class, and + // are created with [ClassName]_[FunctionName]() format + + void matlab_mfile(const std::string& toolboxPath, const std::string& className) const; ///< m-file + void matlab_wrapper(const std::string& toolboxPath, + const std::string& className, const std::string& matlabClassName, + const std::string& cppClassName, + const std::vector& using_namespaces, + const std::vector& includes) const; ///< cpp wrapper +}; + +} // \namespace wrap + diff --git a/wrap/geometry.h b/wrap/geometry.h deleted file mode 100644 index 9b1d80b00..000000000 --- a/wrap/geometry.h +++ /dev/null @@ -1,39 +0,0 @@ - -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 index 410798718..add125cb5 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -27,6 +27,7 @@ extern "C" { } #include +#include #include #include @@ -36,9 +37,6 @@ using namespace std; using namespace boost; // not usual, but for conciseness of generated code // start GTSAM Specifics ///////////////////////////////////////////////// -//typedef gtsam::Vector Vector; // NOTE: outside of gtsam namespace -//typedef gtsam::Matrix Matrix; - // to make keys be constructed from strings: #define GTSAM_MAGIC_KEY // to enable Matrix and Vector constructor for SharedGaussian: diff --git a/wrap/pop_actor.h b/wrap/pop_actor.h new file mode 100644 index 000000000..97bfdcca6 --- /dev/null +++ b/wrap/pop_actor.h @@ -0,0 +1,59 @@ +/** + * @file pop_actor.h + * + * @brief An actor to pop a vector/container, based off of the clear_actor + * + * @date Dec 8, 2011 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace boost { namespace spirit { + +BOOST_SPIRIT_CLASSIC_NAMESPACE_BEGIN + + /////////////////////////////////////////////////////////////////////////// + // Summary: + // A semantic action policy that calls pop_back method. + // (This doc uses convention available in actors.hpp) + // + // Actions (what it does): + // ref.pop_back(); + // + // Policy name: + // clear_action + // + // Policy holder, corresponding helper method: + // ref_actor, clear_a( ref ); + // + // () operators: both. + // + // See also ref_actor for more details. + /////////////////////////////////////////////////////////////////////////// + struct pop_action + { + template< + typename T + > + void act(T& ref_) const + { + ref_.pop_back(); + } + }; + + /////////////////////////////////////////////////////////////////////////// + // helper method that creates a and_assign_actor. + /////////////////////////////////////////////////////////////////////////// + template + inline ref_actor pop_a(T& ref_) + { + return ref_actor(ref_); + } + +BOOST_SPIRIT_CLASSIC_NAMESPACE_END + +}} + diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index f81c416fe..e845db94a 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/dim.m b/wrap/tests/expected/@Point2/dim.m index afd9ee5f7..40715292e 100644 --- a/wrap/tests/expected/@Point2/dim.m +++ b/wrap/tests/expected/@Point2/dim.m @@ -1,5 +1,4 @@ function result = dim(obj) % usage: obj.dim() -% automatically generated by wrap on 2011-Oct-31 error('need to compile dim.cpp'); end diff --git a/wrap/tests/expected/@Point2/vectorConfusion.cpp b/wrap/tests/expected/@Point2/vectorConfusion.cpp new file mode 100644 index 000000000..cdb06b2df --- /dev/null +++ b/wrap/tests/expected/@Point2/vectorConfusion.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("vectorConfusion",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + VectorNotEigen result = self->vectorConfusion(); + out[0] = wrap_shared_ptr(make_shared< VectorNotEigen >(result),"VectorNotEigen"); +} diff --git a/wrap/tests/expected/@Point2/vectorConfusion.m b/wrap/tests/expected/@Point2/vectorConfusion.m new file mode 100644 index 000000000..d141e8085 --- /dev/null +++ b/wrap/tests/expected/@Point2/vectorConfusion.m @@ -0,0 +1,4 @@ +function result = vectorConfusion(obj) +% usage: obj.vectorConfusion() + error('need to compile vectorConfusion.cpp'); +end diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp index da5ed8a0c..d245748d0 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/x.m b/wrap/tests/expected/@Point2/x.m index 3309b286c..f91039ab7 100644 --- a/wrap/tests/expected/@Point2/x.m +++ b/wrap/tests/expected/@Point2/x.m @@ -1,5 +1,4 @@ function result = x(obj) % usage: obj.x() -% automatically generated by wrap on 2011-Oct-31 error('need to compile x.cpp'); end diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index 3d61a4048..6342d6238 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/y.m b/wrap/tests/expected/@Point2/y.m index 015a11144..f9f36ae90 100644 --- a/wrap/tests/expected/@Point2/y.m +++ b/wrap/tests/expected/@Point2/y.m @@ -1,5 +1,4 @@ function result = y(obj) % usage: obj.y() -% automatically generated by wrap on 2011-Oct-31 error('need to compile y.cpp'); end diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 6daaadfd1..ad36e32b8 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("norm",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/Test.m b/wrap/tests/expected/@Test/Test.m index 19b4442e4..8574a0882 100644 --- a/wrap/tests/expected/@Test/Test.m +++ b/wrap/tests/expected/@Test/Test.m @@ -5,6 +5,7 @@ classdef Test methods function obj = Test(varargin) if nargin == 0, obj.self = new_Test_(); end + if nargin == 2, obj.self = new_Test_dM(varargin{1},varargin{2}); end if nargin ~= 13 && obj.self == 0, error('Test constructor failed'); end end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp new file mode 100644 index 000000000..e44b74f1b --- /dev/null +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("arg_EigenConstRef",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); + self->arg_EigenConstRef(value); +} diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.m b/wrap/tests/expected/@Test/arg_EigenConstRef.m new file mode 100644 index 000000000..e353faa29 --- /dev/null +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.m @@ -0,0 +1,4 @@ +function result = arg_EigenConstRef(obj,value) +% usage: obj.arg_EigenConstRef(value) + error('need to compile arg_EigenConstRef.cpp'); +end diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp new file mode 100644 index 000000000..a2c237c05 --- /dev/null +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + pair< Test, shared_ptr > result = self->create_MixedPtrs(); + out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test"); + out[1] = wrap_shared_ptr(result.second,"Test"); +} diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.m b/wrap/tests/expected/@Test/create_MixedPtrs.m new file mode 100644 index 000000000..9062cabcb --- /dev/null +++ b/wrap/tests/expected/@Test/create_MixedPtrs.m @@ -0,0 +1,4 @@ +function [first,second] = create_MixedPtrs(obj) +% usage: obj.create_MixedPtrs() + error('need to compile create_MixedPtrs.cpp'); +end diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index 690466298..33722bd14 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/create_ptrs.m b/wrap/tests/expected/@Test/create_ptrs.m index 07325487f..11af0ac5b 100644 --- a/wrap/tests/expected/@Test/create_ptrs.m +++ b/wrap/tests/expected/@Test/create_ptrs.m @@ -1,5 +1,4 @@ function [first,second] = create_ptrs(obj) % usage: obj.create_ptrs() -% automatically generated by wrap on 2011-Oct-31 error('need to compile create_ptrs.cpp'); end diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index 9c516c6b7..e92a58b10 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp new file mode 100644 index 000000000..99cf67f0b --- /dev/null +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Point2Ptr",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + bool value = unwrap< bool >(in[1]); + shared_ptr result = self->return_Point2Ptr(value); + out[0] = wrap_shared_ptr(result,"Point2"); +} diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.m b/wrap/tests/expected/@Test/return_Point2Ptr.m new file mode 100644 index 000000000..2da8b3710 --- /dev/null +++ b/wrap/tests/expected/@Test/return_Point2Ptr.m @@ -0,0 +1,4 @@ +function result = return_Point2Ptr(obj,value) +% usage: obj.return_Point2Ptr(value) + error('need to compile return_Point2Ptr.cpp'); +end diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp new file mode 100644 index 000000000..19256f9ac --- /dev/null +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Test",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); + Test result = self->return_Test(value); + out[0] = wrap_shared_ptr(make_shared< Test >(result),"Test"); +} diff --git a/wrap/tests/expected/@Test/return_Test.m b/wrap/tests/expected/@Test/return_Test.m new file mode 100644 index 000000000..01f4ec61b --- /dev/null +++ b/wrap/tests/expected/@Test/return_Test.m @@ -0,0 +1,4 @@ +function result = return_Test(obj,value) +% usage: obj.return_Test(value) + error('need to compile return_Test.cpp'); +end diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index b4ebecfd8..39ed01f15 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_TestPtr.m b/wrap/tests/expected/@Test/return_TestPtr.m index 5da63f9c1..e1d0c90f5 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.m +++ b/wrap/tests/expected/@Test/return_TestPtr.m @@ -1,5 +1,4 @@ function result = return_TestPtr(obj,value) % usage: obj.return_TestPtr(value) -% automatically generated by wrap on 2011-Oct-31 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 index 5a797b963..016bf2934 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_bool.m b/wrap/tests/expected/@Test/return_bool.m index bf41045d3..185ab992d 100644 --- a/wrap/tests/expected/@Test/return_bool.m +++ b/wrap/tests/expected/@Test/return_bool.m @@ -1,5 +1,4 @@ function result = return_bool(obj,value) % usage: obj.return_bool(value) -% automatically generated by wrap on 2011-Oct-31 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 index c1dc96f9d..c087c194b 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_double.m b/wrap/tests/expected/@Test/return_double.m index e8c154440..a6ba733cf 100644 --- a/wrap/tests/expected/@Test/return_double.m +++ b/wrap/tests/expected/@Test/return_double.m @@ -1,5 +1,4 @@ function result = return_double(obj,value) % usage: obj.return_double(value) -% automatically generated by wrap on 2011-Oct-31 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 index adcb76bfd..3c2ea29b0 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_field.m b/wrap/tests/expected/@Test/return_field.m index d99196354..278ffa411 100644 --- a/wrap/tests/expected/@Test/return_field.m +++ b/wrap/tests/expected/@Test/return_field.m @@ -1,5 +1,4 @@ function result = return_field(obj,t) % usage: obj.return_field(t) -% automatically generated by wrap on 2011-Oct-31 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 index fc24edc3c..c9bada69e 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_int.m b/wrap/tests/expected/@Test/return_int.m index a36990b35..8b613285c 100644 --- a/wrap/tests/expected/@Test/return_int.m +++ b/wrap/tests/expected/@Test/return_int.m @@ -1,5 +1,4 @@ function result = return_int(obj,value) % usage: obj.return_int(value) -% automatically generated by wrap on 2011-Oct-31 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 index 07c59cebc..acd39c9f5 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix1.m b/wrap/tests/expected/@Test/return_matrix1.m index 87200044f..29743158c 100644 --- a/wrap/tests/expected/@Test/return_matrix1.m +++ b/wrap/tests/expected/@Test/return_matrix1.m @@ -1,5 +1,4 @@ function result = return_matrix1(obj,value) % usage: obj.return_matrix1(value) -% automatically generated by wrap on 2011-Oct-31 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 index 6a942b54d..50e2ee462 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix2.m b/wrap/tests/expected/@Test/return_matrix2.m index 05b9c9c9c..e9ec91678 100644 --- a/wrap/tests/expected/@Test/return_matrix2.m +++ b/wrap/tests/expected/@Test/return_matrix2.m @@ -1,5 +1,4 @@ function result = return_matrix2(obj,value) % usage: obj.return_matrix2(value) -% automatically generated by wrap on 2011-Oct-31 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 index 637f0b365..a10e79109 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_pair.m b/wrap/tests/expected/@Test/return_pair.m index 61a0138f8..a97f7c46e 100644 --- a/wrap/tests/expected/@Test/return_pair.m +++ b/wrap/tests/expected/@Test/return_pair.m @@ -1,5 +1,4 @@ function [first,second] = return_pair(obj,v,A) % usage: obj.return_pair(v,A) -% automatically generated by wrap on 2011-Oct-31 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 index ae1ceae84..6c2ab46a6 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_ptrs.m b/wrap/tests/expected/@Test/return_ptrs.m index 8eb7d0ce2..ef7f8e5fc 100644 --- a/wrap/tests/expected/@Test/return_ptrs.m +++ b/wrap/tests/expected/@Test/return_ptrs.m @@ -1,5 +1,4 @@ function [first,second] = return_ptrs(obj,p1,p2) % usage: obj.return_ptrs(p1,p2) -% automatically generated by wrap on 2011-Oct-31 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 index 1ffaf6a4f..af1524ec7 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_size_t.m b/wrap/tests/expected/@Test/return_size_t.m index 9124824b7..bc2734410 100644 --- a/wrap/tests/expected/@Test/return_size_t.m +++ b/wrap/tests/expected/@Test/return_size_t.m @@ -1,5 +1,4 @@ function result = return_size_t(obj,value) % usage: obj.return_size_t(value) -% automatically generated by wrap on 2011-Oct-31 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 index 3055a8422..71a86e63b 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_string.m b/wrap/tests/expected/@Test/return_string.m index ad22de567..3f2304a65 100644 --- a/wrap/tests/expected/@Test/return_string.m +++ b/wrap/tests/expected/@Test/return_string.m @@ -1,5 +1,4 @@ function result = return_string(obj,value) % usage: obj.return_string(value) -% automatically generated by wrap on 2011-Oct-31 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 index e1a384957..df8779989 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector1.m b/wrap/tests/expected/@Test/return_vector1.m index a0ee6e8ba..f67382978 100644 --- a/wrap/tests/expected/@Test/return_vector1.m +++ b/wrap/tests/expected/@Test/return_vector1.m @@ -1,5 +1,4 @@ function result = return_vector1(obj,value) % usage: obj.return_vector1(value) -% automatically generated by wrap on 2011-Oct-31 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 index 1380643c1..ac87ab83a 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector2.m b/wrap/tests/expected/@Test/return_vector2.m index 3768e9e0e..95b6bcfd6 100644 --- a/wrap/tests/expected/@Test/return_vector2.m +++ b/wrap/tests/expected/@Test/return_vector2.m @@ -1,5 +1,4 @@ function result = return_vector2(obj,value) % usage: obj.return_vector2(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_vector2.cpp'); end diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile new file mode 100644 index 000000000..03f69a496 --- /dev/null +++ b/wrap/tests/expected/Makefile @@ -0,0 +1,91 @@ +# automatically generated by wrap on 2011-Dec-06 + +MEX = mex +MEXENDING = mexa64 +mex_flags = -O5 + +all: Point2 Point3 Test + +# Point2 +new_Point2_.$(MEXENDING): new_Point2_.cpp + $(MEX) $(mex_flags) new_Point2_.cpp -output new_Point2_ +new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp + $(MEX) $(mex_flags) new_Point2_dd.cpp -output new_Point2_dd +@Point2/x.$(MEXENDING): @Point2/x.cpp + $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x +@Point2/y.$(MEXENDING): @Point2/y.cpp + $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y +@Point2/dim.$(MEXENDING): @Point2/dim.cpp + $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim +@Point2/vectorConfusion.$(MEXENDING): @Point2/vectorConfusion.cpp + $(MEX) $(mex_flags) @Point2/vectorConfusion.cpp -output @Point2/vectorConfusion + +Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) @Point2/vectorConfusion.$(MEXENDING) + +# Point3 +new_Point3_ddd.$(MEXENDING): new_Point3_ddd.cpp + $(MEX) $(mex_flags) new_Point3_ddd.cpp -output new_Point3_ddd +Point3_staticFunction.$(MEXENDING): Point3_staticFunction.cpp + $(MEX) $(mex_flags) Point3_staticFunction.cpp -output Point3_staticFunction +Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp + $(MEX) $(mex_flags) Point3_StaticFunctionRet.cpp -output Point3_StaticFunctionRet +@Point3/norm.$(MEXENDING): @Point3/norm.cpp + $(MEX) $(mex_flags) @Point3/norm.cpp -output @Point3/norm + +Point3: new_Point3_ddd.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_StaticFunctionRet.$(MEXENDING) @Point3/norm.$(MEXENDING) + +# Test +new_Test_.$(MEXENDING): new_Test_.cpp + $(MEX) $(mex_flags) new_Test_.cpp -output new_Test_ +new_Test_dM.$(MEXENDING): new_Test_dM.cpp + $(MEX) $(mex_flags) new_Test_dM.cpp -output new_Test_dM +@Test/return_pair.$(MEXENDING): @Test/return_pair.cpp + $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair +@Test/return_bool.$(MEXENDING): @Test/return_bool.cpp + $(MEX) $(mex_flags) @Test/return_bool.cpp -output @Test/return_bool +@Test/return_size_t.$(MEXENDING): @Test/return_size_t.cpp + $(MEX) $(mex_flags) @Test/return_size_t.cpp -output @Test/return_size_t +@Test/return_int.$(MEXENDING): @Test/return_int.cpp + $(MEX) $(mex_flags) @Test/return_int.cpp -output @Test/return_int +@Test/return_double.$(MEXENDING): @Test/return_double.cpp + $(MEX) $(mex_flags) @Test/return_double.cpp -output @Test/return_double +@Test/return_string.$(MEXENDING): @Test/return_string.cpp + $(MEX) $(mex_flags) @Test/return_string.cpp -output @Test/return_string +@Test/return_vector1.$(MEXENDING): @Test/return_vector1.cpp + $(MEX) $(mex_flags) @Test/return_vector1.cpp -output @Test/return_vector1 +@Test/return_matrix1.$(MEXENDING): @Test/return_matrix1.cpp + $(MEX) $(mex_flags) @Test/return_matrix1.cpp -output @Test/return_matrix1 +@Test/return_vector2.$(MEXENDING): @Test/return_vector2.cpp + $(MEX) $(mex_flags) @Test/return_vector2.cpp -output @Test/return_vector2 +@Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp + $(MEX) $(mex_flags) @Test/return_matrix2.cpp -output @Test/return_matrix2 +@Test/arg_EigenConstRef.$(MEXENDING): @Test/arg_EigenConstRef.cpp + $(MEX) $(mex_flags) @Test/arg_EigenConstRef.cpp -output @Test/arg_EigenConstRef +@Test/return_field.$(MEXENDING): @Test/return_field.cpp + $(MEX) $(mex_flags) @Test/return_field.cpp -output @Test/return_field +@Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp + $(MEX) $(mex_flags) @Test/return_TestPtr.cpp -output @Test/return_TestPtr +@Test/return_Test.$(MEXENDING): @Test/return_Test.cpp + $(MEX) $(mex_flags) @Test/return_Test.cpp -output @Test/return_Test +@Test/return_Point2Ptr.$(MEXENDING): @Test/return_Point2Ptr.cpp + $(MEX) $(mex_flags) @Test/return_Point2Ptr.cpp -output @Test/return_Point2Ptr +@Test/create_ptrs.$(MEXENDING): @Test/create_ptrs.cpp + $(MEX) $(mex_flags) @Test/create_ptrs.cpp -output @Test/create_ptrs +@Test/create_MixedPtrs.$(MEXENDING): @Test/create_MixedPtrs.cpp + $(MEX) $(mex_flags) @Test/create_MixedPtrs.cpp -output @Test/create_MixedPtrs +@Test/return_ptrs.$(MEXENDING): @Test/return_ptrs.cpp + $(MEX) $(mex_flags) @Test/return_ptrs.cpp -output @Test/return_ptrs +@Test/print.$(MEXENDING): @Test/print.cpp + $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print + +Test: new_Test_.$(MEXENDING) new_Test_dM.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/arg_EigenConstRef.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) + + + +clean: + rm -rf *.$(MEXENDING) + rm -rf @Point2/*.$(MEXENDING) + rm -rf @Point3/*.$(MEXENDING) + rm -rf @Test/*.$(MEXENDING) + + diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.cpp b/wrap/tests/expected/Point3_StaticFunctionRet.cpp new file mode 100644 index 000000000..657f90bb1 --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunctionRet.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("Point3_StaticFunctionRet",nargout,nargin,1); + double z = unwrap< double >(in[0]); + Point3 result = Point3::StaticFunctionRet(z); + out[0] = wrap_shared_ptr(make_shared< Point3 >(result),"Point3"); +} diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.m b/wrap/tests/expected/Point3_StaticFunctionRet.m new file mode 100644 index 000000000..f2ba314f1 --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunctionRet.m @@ -0,0 +1,4 @@ +function result = Point3_StaticFunctionRet() +% usage: x = Point3_StaticFunctionRet() + error('need to compile Point3_StaticFunctionRet.cpp'); +end diff --git a/wrap/tests/expected/Point3_staticFunction.cpp b/wrap/tests/expected/Point3_staticFunction.cpp new file mode 100644 index 000000000..e0519e20e --- /dev/null +++ b/wrap/tests/expected/Point3_staticFunction.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("Point3_staticFunction",nargout,nargin,0); + double result = Point3::staticFunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected/Point3_staticFunction.m b/wrap/tests/expected/Point3_staticFunction.m new file mode 100644 index 000000000..441bb5bc6 --- /dev/null +++ b/wrap/tests/expected/Point3_staticFunction.m @@ -0,0 +1,4 @@ +function result = Point3_staticFunction() +% usage: x = Point3_staticFunction() + error('need to compile Point3_staticFunction.cpp'); +end diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index cad4eab1d..39123f65d 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Nov-04 +% automatically generated by wrap on 2011-Dec-06 echo on toolboxpath = mfilename('fullpath'); @@ -16,10 +16,13 @@ cd @Point2 mex -O5 x.cpp mex -O5 y.cpp mex -O5 dim.cpp +mex -O5 vectorConfusion.cpp %% Point3 cd(toolboxpath) mex -O5 new_Point3_ddd.cpp +mex -O5 Point3_staticFunction.cpp +mex -O5 Point3_StaticFunctionRet.cpp cd @Point3 mex -O5 norm.cpp @@ -27,8 +30,10 @@ mex -O5 norm.cpp %% Test cd(toolboxpath) mex -O5 new_Test_.cpp +mex -O5 new_Test_dM.cpp cd @Test +mex -O5 return_pair.cpp mex -O5 return_bool.cpp mex -O5 return_size_t.cpp mex -O5 return_int.cpp @@ -38,10 +43,13 @@ 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 arg_EigenConstRef.cpp mex -O5 return_field.cpp mex -O5 return_TestPtr.cpp +mex -O5 return_Test.cpp +mex -O5 return_Point2Ptr.cpp mex -O5 create_ptrs.cpp +mex -O5 create_MixedPtrs.cpp mex -O5 return_ptrs.cpp mex -O5 print.cpp diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index 4745ae6a1..5cfcad238 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point2_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Point2_.m b/wrap/tests/expected/new_Point2_.m index 5b3756ef7..dc02b426b 100644 --- a/wrap/tests/expected/new_Point2_.m +++ b/wrap/tests/expected/new_Point2_.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-06 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 index 30db28cd9..6ab5721a1 100644 --- a/wrap/tests/expected/new_Point2_dd.cpp +++ b/wrap/tests/expected/new_Point2_dd.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point2_dd",nargout,nargin,2); diff --git a/wrap/tests/expected/new_Point2_dd.m b/wrap/tests/expected/new_Point2_dd.m index f83962584..7d4bf7b86 100644 --- a/wrap/tests/expected/new_Point2_dd.m +++ b/wrap/tests/expected/new_Point2_dd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-06 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 index c83518386..285ed9ca6 100644 --- a/wrap/tests/expected/new_Point3_ddd.cpp +++ b/wrap/tests/expected/new_Point3_ddd.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point3_ddd",nargout,nargin,3); diff --git a/wrap/tests/expected/new_Point3_ddd.m b/wrap/tests/expected/new_Point3_ddd.m index 3b15dcfdb..b3004b4bb 100644 --- a/wrap/tests/expected/new_Point3_ddd.m +++ b/wrap/tests/expected/new_Point3_ddd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-06 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 index 9afe37223..9c9aaab88 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Test_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Test_.m b/wrap/tests/expected/new_Test_.m index 03f3ca25f..9064b5f23 100644 --- a/wrap/tests/expected/new_Test_.m +++ b/wrap/tests/expected/new_Test_.m @@ -1,4 +1,4 @@ +% automatically generated by wrap on 2011-Dec-06 function result = new_Test_(obj) -% automatically generated by wrap on 2011-Oct-31 error('need to compile new_Test_.cpp'); end diff --git a/wrap/tests/expected/new_Test_b.cpp b/wrap/tests/expected/new_Test_b.cpp new file mode 100644 index 000000000..921c692af --- /dev/null +++ b/wrap/tests/expected/new_Test_b.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-01 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_Test_b",nargout,nargin,1); + bool value = unwrap< bool >(in[0]); + Test* self = new Test(value); + out[0] = wrap_constructed(self,"Test"); +} diff --git a/wrap/tests/expected/new_Test_b.m b/wrap/tests/expected/new_Test_b.m new file mode 100644 index 000000000..a07945d9f --- /dev/null +++ b/wrap/tests/expected/new_Test_b.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-01 +function result = new_Test_b(obj,value) + error('need to compile new_Test_b.cpp'); +end diff --git a/wrap/tests/expected/new_Test_dM.cpp b/wrap/tests/expected/new_Test_dM.cpp new file mode 100644 index 000000000..4179921da --- /dev/null +++ b/wrap/tests/expected/new_Test_dM.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_Test_dM",nargout,nargin,2); + double a = unwrap< double >(in[0]); + Matrix b = unwrap< Matrix >(in[1]); + Test* self = new Test(a,b); + out[0] = wrap_constructed(self,"Test"); +} diff --git a/wrap/tests/expected_namespaces/@ClassD/ClassD.m b/wrap/tests/expected_namespaces/@ClassD/ClassD.m new file mode 100644 index 000000000..d6b5b452a --- /dev/null +++ b/wrap/tests/expected_namespaces/@ClassD/ClassD.m @@ -0,0 +1,13 @@ +classdef ClassD + properties + self = 0 + end + methods + function obj = ClassD(varargin) + if nargin == 0, obj.self = new_ClassD_(); end + if nargin ~= 13 && obj.self == 0, error('ClassD constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m new file mode 100644 index 000000000..f5de15c80 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m @@ -0,0 +1,13 @@ +classdef ns1ClassA + properties + self = 0 + end + methods + function obj = ns1ClassA(varargin) + if nargin == 0, obj.self = new_ns1ClassA_(); end + if nargin ~= 13 && obj.self == 0, error('ns1ClassA constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m new file mode 100644 index 000000000..189dab25b --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m @@ -0,0 +1,13 @@ +classdef ns1ClassB + properties + self = 0 + end + methods + function obj = ns1ClassB(varargin) + if nargin == 0, obj.self = new_ns1ClassB_(); end + if nargin ~= 13 && obj.self == 0, error('ns1ClassB constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp new file mode 100644 index 000000000..d051c74ec --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("memberFunction",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); + double result = self->memberFunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m new file mode 100644 index 000000000..8e8d24bd7 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m @@ -0,0 +1,4 @@ +function result = memberFunction(obj) +% usage: obj.memberFunction() + error('need to compile memberFunction.cpp'); +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m new file mode 100644 index 000000000..f0521377e --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m @@ -0,0 +1,13 @@ +classdef ns2ClassA + properties + self = 0 + end + methods + function obj = ns2ClassA(varargin) + if nargin == 0, obj.self = new_ns2ClassA_(); end + if nargin ~= 13 && obj.self == 0, error('ns2ClassA constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m new file mode 100644 index 000000000..78fc02073 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m @@ -0,0 +1,13 @@ +classdef ns2ClassC + properties + self = 0 + end + methods + function obj = ns2ClassC(varargin) + if nargin == 0, obj.self = new_ns2ClassC_(); end + if nargin ~= 13 && obj.self == 0, error('ns2ClassC constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m new file mode 100644 index 000000000..35891c5a7 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m @@ -0,0 +1,13 @@ +classdef ns2ns3ClassB + properties + self = 0 + end + methods + function obj = ns2ns3ClassB(varargin) + if nargin == 0, obj.self = new_ns2ns3ClassB_(); end + if nargin ~= 13 && obj.self == 0, error('ns2ns3ClassB constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile new file mode 100644 index 000000000..4fe9b0655 --- /dev/null +++ b/wrap/tests/expected_namespaces/Makefile @@ -0,0 +1,64 @@ +# automatically generated by wrap on 2011-Dec-08 + +MEX = mex +MEXENDING = mexa64 +mex_flags = -O5 + +all: ns1ClassA ns1ClassB ns2ClassA ns2ns3ClassB ns2ClassC ClassD + +# ns1ClassA +new_ns1ClassA_.$(MEXENDING): new_ns1ClassA_.cpp + $(MEX) $(mex_flags) new_ns1ClassA_.cpp -output new_ns1ClassA_ + +ns1ClassA: new_ns1ClassA_.$(MEXENDING) + +# ns1ClassB +new_ns1ClassB_.$(MEXENDING): new_ns1ClassB_.cpp + $(MEX) $(mex_flags) new_ns1ClassB_.cpp -output new_ns1ClassB_ + +ns1ClassB: new_ns1ClassB_.$(MEXENDING) + +# ns2ClassA +new_ns2ClassA_.$(MEXENDING): new_ns2ClassA_.cpp + $(MEX) $(mex_flags) new_ns2ClassA_.cpp -output new_ns2ClassA_ +ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp + $(MEX) $(mex_flags) ns2ClassA_afunction.cpp -output ns2ClassA_afunction +@ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp + $(MEX) $(mex_flags) @ns2ClassA/memberFunction.cpp -output @ns2ClassA/memberFunction +@ns2ClassA/nsArg.$(MEXENDING): @ns2ClassA/nsArg.cpp + $(MEX) $(mex_flags) @ns2ClassA/nsArg.cpp -output @ns2ClassA/nsArg +@ns2ClassA/nsReturn.$(MEXENDING): @ns2ClassA/nsReturn.cpp + $(MEX) $(mex_flags) @ns2ClassA/nsReturn.cpp -output @ns2ClassA/nsReturn + +ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) @ns2ClassA/nsReturn.$(MEXENDING) + +# ns2ns3ClassB +new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp + $(MEX) $(mex_flags) new_ns2ns3ClassB_.cpp -output new_ns2ns3ClassB_ + +ns2ns3ClassB: new_ns2ns3ClassB_.$(MEXENDING) + +# ns2ClassC +new_ns2ClassC_.$(MEXENDING): new_ns2ClassC_.cpp + $(MEX) $(mex_flags) new_ns2ClassC_.cpp -output new_ns2ClassC_ + +ns2ClassC: new_ns2ClassC_.$(MEXENDING) + +# ClassD +new_ClassD_.$(MEXENDING): new_ClassD_.cpp + $(MEX) $(mex_flags) new_ClassD_.cpp -output new_ClassD_ + +ClassD: new_ClassD_.$(MEXENDING) + + + +clean: + rm -rf *.$(MEXENDING) + rm -rf @ns1ClassA/*.$(MEXENDING) + rm -rf @ns1ClassB/*.$(MEXENDING) + rm -rf @ns2ClassA/*.$(MEXENDING) + rm -rf @ns2ns3ClassB/*.$(MEXENDING) + rm -rf @ns2ClassC/*.$(MEXENDING) + rm -rf @ClassD/*.$(MEXENDING) + + diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m new file mode 100644 index 000000000..59d850c42 --- /dev/null +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -0,0 +1,52 @@ +% automatically generated by wrap on 2011-Dec-08 +echo on + +toolboxpath = mfilename('fullpath'); +delims = find(toolboxpath == '/'); +toolboxpath = toolboxpath(1:(delims(end)-1)); +clear delims +addpath(toolboxpath); + +%% ns1ClassA +cd(toolboxpath) +mex -O5 new_ns1ClassA_.cpp + +cd @ns1ClassA + +%% ns1ClassB +cd(toolboxpath) +mex -O5 new_ns1ClassB_.cpp + +cd @ns1ClassB + +%% ns2ClassA +cd(toolboxpath) +mex -O5 new_ns2ClassA_.cpp +mex -O5 ns2ClassA_afunction.cpp + +cd @ns2ClassA +mex -O5 memberFunction.cpp +mex -O5 nsArg.cpp +mex -O5 nsReturn.cpp + +%% ns2ns3ClassB +cd(toolboxpath) +mex -O5 new_ns2ns3ClassB_.cpp + +cd @ns2ns3ClassB + +%% ns2ClassC +cd(toolboxpath) +mex -O5 new_ns2ClassC_.cpp + +cd @ns2ClassC + +%% ClassD +cd(toolboxpath) +mex -O5 new_ClassD_.cpp + +cd @ClassD + +cd(toolboxpath) + +echo off diff --git a/wrap/tests/expected_namespaces/new_ClassD_.cpp b/wrap/tests/expected_namespaces/new_ClassD_.cpp new file mode 100644 index 000000000..ec7212786 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ClassD_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ClassD_",nargout,nargin,0); + ClassD* self = new ClassD(); + out[0] = wrap_constructed(self,"ClassD"); +} diff --git a/wrap/tests/expected_namespaces/new_ClassD_.m b/wrap/tests/expected_namespaces/new_ClassD_.m new file mode 100644 index 000000000..c5f53f130 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ClassD_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ClassD_(obj) + error('need to compile new_ClassD_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp new file mode 100644 index 000000000..2db8ef767 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns1ClassA_",nargout,nargin,0); + ns1::ClassA* self = new ns1::ClassA(); + out[0] = wrap_constructed(self,"ns1ClassA"); +} diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.m b/wrap/tests/expected_namespaces/new_ns1ClassA_.m new file mode 100644 index 000000000..89cd8b0a2 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns1ClassA_(obj) + error('need to compile new_ns1ClassA_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp new file mode 100644 index 000000000..b4ac7038a --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns1ClassB_",nargout,nargin,0); + ns1::ClassB* self = new ns1::ClassB(); + out[0] = wrap_constructed(self,"ns1ClassB"); +} diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.m b/wrap/tests/expected_namespaces/new_ns1ClassB_.m new file mode 100644 index 000000000..5430f85aa --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns1ClassB_(obj) + error('need to compile new_ns1ClassB_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp new file mode 100644 index 000000000..cc4ec309b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns2ClassA_",nargout,nargin,0); + ns2::ClassA* self = new ns2::ClassA(); + out[0] = wrap_constructed(self,"ns2ClassA"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.m b/wrap/tests/expected_namespaces/new_ns2ClassA_.m new file mode 100644 index 000000000..bb8b2a24a --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ClassA_(obj) + error('need to compile new_ns2ClassA_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp new file mode 100644 index 000000000..b43a7cd6b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns2ClassC_",nargout,nargin,0); + ns2::ClassC* self = new ns2::ClassC(); + out[0] = wrap_constructed(self,"ns2ClassC"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.m b/wrap/tests/expected_namespaces/new_ns2ClassC_.m new file mode 100644 index 000000000..91e643c4b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ClassC_(obj) + error('need to compile new_ns2ClassC_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp new file mode 100644 index 000000000..3916ed3ff --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns2ns3ClassB_",nargout,nargin,0); + ns2::ns3::ClassB* self = new ns2::ns3::ClassB(); + out[0] = wrap_constructed(self,"ns2ns3ClassB"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m new file mode 100644 index 000000000..54b38a16c --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ns3ClassB_(obj) + error('need to compile new_ns2ns3ClassB_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp new file mode 100644 index 000000000..dff68090a --- /dev/null +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("ns2ClassA_afunction",nargout,nargin,0); + double result = ns2::ClassA::afunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.m b/wrap/tests/expected_namespaces/ns2ClassA_afunction.m new file mode 100644 index 000000000..0482d1548 --- /dev/null +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.m @@ -0,0 +1,4 @@ +function result = ns2ClassA_afunction() +% usage: x = ns2ClassA_afunction() + error('need to compile ns2ClassA_afunction.cpp'); +end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h new file mode 100644 index 000000000..d77d6ee77 --- /dev/null +++ b/wrap/tests/geometry.h @@ -0,0 +1,80 @@ + // comments! + +// set the default namespace +// location of namespace isn't significant +using namespace geometry; + +class Point2 { + Point2(); + Point2(double x, double y); + double x() const; + double y() const; + int dim() const; + VectorNotEigen vectorConfusion(); +}; + +class Point3 { + Point3(double x, double y, double z); + double norm() const; + + // static functions - use static keyword and uppercase + static double staticFunction(); + static Point3 StaticFunctionRet(double z); +}; + +// another comment + +// another comment + +/** + * A multi-line comment! + */ + +class Test { +#include + + /* a comment! */ + // another comment + Test(); + + pair return_pair (Vector v, Matrix A) const; // intentionally the first method + + bool return_bool (bool value) const; // comment after a line! + size_t return_size_t (size_t value) const; + int return_int (int value) const; + double return_double (double value) const; + + Test(double a, Matrix b); // a constructor in the middle of a class + + // comments in the middle! + + // (more) comments in the middle! + + string return_string (string value) const; + Vector return_vector1(Vector value) const; + Matrix return_matrix1(Matrix value) const; + Vector return_vector2(Vector value) const; + Matrix return_matrix2(Matrix value) const; + void arg_EigenConstRef(const Matrix& value) const; + + bool return_field(const Test& t) const; + + Test* return_TestPtr(Test* value) const; + Test return_Test(Test* value) const; + + Point2* return_Point2Ptr(bool value) const; + + pair create_ptrs () const; + pair create_MixedPtrs () const; + pair return_ptrs (Test* p1, Test* p2) const; + + void print() const; + + // comments at the end! + + // even more comments at the end! +}; + +// comments at the end! + +// even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h new file mode 100644 index 000000000..f27a518c6 --- /dev/null +++ b/wrap/tests/testNamespaces.h @@ -0,0 +1,45 @@ +/** + * This is a wrap header to verify permutations on namespaces + */ + +namespace ns1 { + +class ClassA { + ClassA(); +}; + +class ClassB { + ClassB(); +}; + +}///\namespace ns1 + +namespace ns2 { + +class ClassA { + ClassA(); + static double afunction(); + double memberFunction(); + int nsArg(const ns1::ClassB& arg); + ns2::ns3::ClassB nsReturn(double q); +}; + +namespace ns3 { + +class ClassB { + ClassB(); +}; + +}///\namespace ns3 + +class ClassC { + ClassC(); +}; + +}///\namespace ns2 + +class ClassD { + ClassD(); +}; + + diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 3f68952f3..d4a6d151e 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -18,6 +18,7 @@ #include #include #include +#include using namespace std; using namespace BOOST_SPIRIT_CLASSIC_NS; @@ -36,31 +37,31 @@ Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double" | "Ve /* ************************************************************************* */ TEST( spirit, real ) { // check if we can parse 8.99 as a real - CHECK(parse("8.99", real_p, space_p).full); + EXPECT(parse("8.99", real_p, space_p).full); // make sure parsing fails on this one - CHECK(!parse("zztop", real_p, space_p).full); + EXPECT(!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); + EXPECT(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); + EXPECT(parse("int int", str_p("int") >> *str_p("int"), space_p).full); + EXPECT(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); + EXPECT(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 + // note 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); + EXPECT(!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); + EXPECT(parse("const \t string", phrase_level_p, space_p).full); } /* ************************************************************************* */ @@ -81,24 +82,58 @@ 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); + EXPECT(!parse("Point3", basisType_p, space_p).full); + EXPECT(parse("string", basisType_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, className_p ) { - CHECK(parse("Point3", className_p, space_p).full); + EXPECT(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); + EXPECT(parse("Point3 &", classRef_p, space_p).full); + EXPECT(parse("Point3&", classRef_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, constMethod_p ) { - CHECK(parse("double norm() const;", constMethod_p, space_p).full); + EXPECT(parse("double norm() const;", constMethod_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, return_value_p ) { + bool isEigen = true; + string actual_return_type; + string actual_function_name; + + Rule basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p; + + Rule funcName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + + Rule returnType_p = + (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]) | + (className_p[assign_a(actual_return_type)][assign_a(isEigen,false)]) | + (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]); + + Rule testFunc_p = returnType_p >> funcName_p[assign_a(actual_function_name)] >> str_p("();"); + + EXPECT(parse("VectorNotEigen doesNotReturnAnEigenVector();", testFunc_p, space_p).full); + EXPECT(!isEigen); + EXPECT(actual_return_type == "VectorNotEigen"); + EXPECT(actual_function_name == "doesNotReturnAnEigenVector"); + + EXPECT(parse("Vector actuallyAVector();", testFunc_p, space_p).full); + EXPECT(isEigen); + EXPECT(actual_return_type == "Vector"); + EXPECT(actual_function_name == "actuallyAVector"); } /* ************************************************************************* */ diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 0650538a5..d4277533a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,6 +15,7 @@ * @author Frank Dellaert **/ +#include #include #include #include @@ -24,78 +25,186 @@ #include using namespace std; -static bool verbose = false; +using namespace wrap; +static bool enable_verbose = false; #ifdef TOPSRCDIR static string topdir = TOPSRCDIR; #else -static string topdir = "notarealdirectory"; // If TOPSRCDIR is not defined, we error +static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error #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"); + 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); + THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); + CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); + + // clean out previous generated code + string cleanCmd = "rm -rf actual"; + system(cleanCmd.c_str()); + + string path = topdir + "/wrap/tests"; + Module module(path.c_str(), "testWrap1",enable_verbose); + CHECK_EXCEPTION(module.matlab_code("actual", "mexa64", "-O5"), DependencyMissing); } /* ************************************************************************* */ TEST( wrap, parse ) { - string path = topdir + "/wrap"; + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "geometry",enable_verbose); + EXPECT_LONGS_EQUAL(3, module.classes.size()); - Module module(path.c_str(), "geometry",verbose); - EXPECT(module.classes.size()==3); + // check using declarations + EXPECT_LONGS_EQUAL(1, module.using_namespaces.size()); + EXPECT(assert_equal("geometry", module.using_namespaces.front())); + + // check first class, Point2 + { + Class cls = module.classes.at(0); + EXPECT(assert_equal("Point2", cls.name)); + EXPECT_LONGS_EQUAL(2, cls.constructors.size()); + EXPECT_LONGS_EQUAL(4, cls.methods.size()); + EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); + EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + } // check second class, Point3 - Class cls = *(++module.classes.begin()); - EXPECT(cls.name=="Point3"); - EXPECT(cls.constructors.size()==1); - EXPECT(cls.methods.size()==1); + { + Class cls = module.classes.at(1); + EXPECT(assert_equal("Point3", cls.name)); + EXPECT_LONGS_EQUAL(1, cls.constructors.size()); + EXPECT_LONGS_EQUAL(1, cls.methods.size()); + EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); + EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); - // first constructor takes 3 doubles - Constructor c1 = cls.constructors.front(); - EXPECT(c1.args.size()==3); + // first constructor takes 3 doubles + Constructor c1 = cls.constructors.front(); + EXPECT_LONGS_EQUAL(3, c1.args.size()); - // 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 first double argument + Argument a1 = c1.args.front(); + EXPECT(!a1.is_const); + EXPECT(assert_equal("double", a1.type)); + EXPECT(!a1.is_ref); + EXPECT(assert_equal("x", a1.name)); - // check method - Method m1 = cls.methods.front(); - EXPECT(m1.returns_=="double"); - EXPECT(m1.name_=="norm"); - EXPECT(m1.args_.size()==0); - EXPECT(m1.is_const_); + // check method + Method m1 = cls.methods.front(); + EXPECT(assert_equal("double", m1.returnVal.type1)); + EXPECT(assert_equal("norm", m1.name)); + EXPECT_LONGS_EQUAL(0, m1.args.size()); + EXPECT(m1.is_const_); + } + + // Test class is the third one + { + LONGS_EQUAL(3, module.classes.size()); + Class testCls = module.classes.at(2); + EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); + EXPECT_LONGS_EQUAL(19, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); + EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); + EXPECT_LONGS_EQUAL( 1, testCls.includes.size()); + EXPECT(assert_equal("folder/path/to/Test.h", testCls.includes.front())); + + // function to parse: pair return_pair (Vector v, Matrix A) const; + Method m2 = testCls.methods.front(); + EXPECT(m2.returnVal.isPair); + EXPECT(m2.returnVal.category1 == ReturnValue::EIGEN); + EXPECT(m2.returnVal.category2 == ReturnValue::EIGEN); + } +} + +/* ************************************************************************* */ +TEST( wrap, parse_namespaces ) { + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "testNamespaces",enable_verbose); + EXPECT_LONGS_EQUAL(6, module.classes.size()); + + Class cls1 = module.classes.at(0); + EXPECT(assert_equal("ClassA", cls1.name)); + EXPECT_LONGS_EQUAL(1, cls1.namespaces.size()); + EXPECT(assert_equal("ns1", cls1.namespaces.front())); + + Class cls2 = module.classes.at(1); + EXPECT(assert_equal("ClassB", cls2.name)); + EXPECT_LONGS_EQUAL(1, cls2.namespaces.size()); + EXPECT(assert_equal("ns1", cls2.namespaces.front())); + + Class cls3 = module.classes.at(2); + EXPECT(assert_equal("ClassA", cls3.name)); + EXPECT_LONGS_EQUAL(1, cls3.namespaces.size()); + EXPECT(assert_equal("ns2", cls3.namespaces.front())); + + Class cls4 = module.classes.at(3); + EXPECT(assert_equal("ClassB", cls4.name)); + EXPECT_LONGS_EQUAL(2, cls4.namespaces.size()); + EXPECT(assert_equal("ns2", cls4.namespaces.front())); + EXPECT(assert_equal("ns3", cls4.namespaces.back())); + + Class cls5 = module.classes.at(4); + EXPECT(assert_equal("ClassC", cls5.name)); + EXPECT_LONGS_EQUAL(1, cls5.namespaces.size()); + EXPECT(assert_equal("ns2", cls5.namespaces.front())); + + Class cls6 = module.classes.at(5); + EXPECT(assert_equal("ClassD", cls6.name)); + EXPECT_LONGS_EQUAL(0, cls6.namespaces.size()); +} + +/* ************************************************************************* */ +TEST( wrap, matlab_code_namespaces ) { + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "testNamespaces",enable_verbose); + EXPECT_LONGS_EQUAL(6, module.classes.size()); + string path = topdir + "/wrap"; + + // clean out previous generated code + string cleanCmd = "rm -rf actual_namespaces"; + system(cleanCmd.c_str()); + + // emit MATLAB code + string exp_path = path + "/tests/expected_namespaces/"; + string act_path = "actual_namespaces/"; + module.matlab_code("actual_namespaces", "mexa64", "-O5"); + + EXPECT(files_equal(exp_path + "make_testNamespaces.m", act_path + "make_testNamespaces.m")); + EXPECT(files_equal(exp_path + "Makefile" , act_path + "Makefile" )); } /* ************************************************************************* */ TEST( wrap, matlab_code ) { // Parse into class object + string header_path = topdir + "/wrap/tests"; + Module module(header_path,"geometry",enable_verbose); string path = topdir + "/wrap"; - Module module(path,"geometry",verbose); + + // clean out previous generated code + string cleanCmd = "rm -rf actual"; + system(cleanCmd.c_str()); // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual", "", "-O5"); + module.matlab_code("actual", "mexa64", "-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_staticFunction.m" , "actual/Point3_staticFunction.m" )); + EXPECT(files_equal(path + "/tests/expected/Point3_staticFunction.cpp", "actual/Point3_staticFunction.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" )); @@ -103,13 +212,17 @@ TEST( wrap, matlab_code ) { 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/create_MixedPtrs.cpp", "actual/@Test/create_MixedPtrs.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_Test.cpp" , "actual/@Test/return_Test.cpp" )); + EXPECT(files_equal(path + "/tests/expected/@Test/return_Point2Ptr.cpp", "actual/@Test/return_Point2Ptr.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" )); + EXPECT(files_equal(path + "/tests/expected/make_geometry.m", "actual/make_geometry.m")); + EXPECT(files_equal(path + "/tests/expected/Makefile" , "actual/Makefile" )); } /* ************************************************************************* */ diff --git a/wrap/tests/testWrap1.h b/wrap/tests/testWrap1.h new file mode 100644 index 000000000..0cc16fc71 --- /dev/null +++ b/wrap/tests/testWrap1.h @@ -0,0 +1,23 @@ +//Header file to test dependency checking +// +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t);//What is Rot3? Throw here + Pose3(Vector v); + Pose3(Matrix t); + static Pose3 Expmap(Vector v); + static Vector Logmap(const Pose3& p); + static Rot3 testStaticDep(Rot3& r);//What is Rot3? Throw here + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + Matrix adjointMap() const; + Pose3 compose(const Pose3& p2); + Pose3 between(const Pose3& p2); + Pose3 retract(Vector v); + Point3 translation() const; + Rot3 rotation() const; //What is Rot3? Throw here +}; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 72833f79a..ebf1ad020 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -15,12 +15,14 @@ **/ #include -#include +#include #include #include "utilities.h" +namespace wrap { + using namespace std; using namespace boost::gregorian; @@ -39,6 +41,16 @@ string file_contents(const string& filename, bool skipheader) { return ss.str(); } +/* ************************************************************************* */ +bool assert_equal(const string& expected, const string& actual) { + if (expected == actual) + return true; + printf("Not equal:\n"); + cout << "expected: [" << expected << "]\n"; + cout << "actual: [" << actual << "]" << endl; + return false; +} + /* ************************************************************************* */ bool files_equal(const string& expected, const string& actual, bool skipheader) { try { @@ -56,13 +68,46 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "expection: " << reason << endl; return false; } + catch (CantOpenFile& e) { + cerr << "file opening error: " << e.what() << endl; + return false; + } return true; } /* ************************************************************************* */ -void emit_header_comment(ofstream& ofs, const string& delimiter) { +void generateHeaderComment(ofstream& ofs, const string& delimiter) { date today = day_clock::local_day(); ofs << delimiter << " automatically generated by wrap on " << today << endl; } /* ************************************************************************* */ +string maybe_shared_ptr(bool add, const string& type) { + string str = add? "shared_ptr<" : ""; + str += type; + if (add) str += ">"; + return str; +} + +/* ************************************************************************* */ +void generateUsingNamespace(ofstream& ofs, const vector& using_namespaces) { + if (using_namespaces.empty()) return; + BOOST_FOREACH(const string& s, using_namespaces) + ofs << "using namespace " << s << ";" << endl; +} + +/* ************************************************************************* */ +void generateIncludes(std::ofstream& ofs, const std::string& class_name, + const std::vector& includes) { + ofs << "#include " << endl; + if (includes.empty()) // add a default include + ofs << "#include <" << class_name << ".h>" << endl; + else { + BOOST_FOREACH(const string& s, includes) + ofs << "#include <" << s << ">" << endl; + } +} + +/* ************************************************************************* */ + +} // \namespace wrap diff --git a/wrap/utilities.h b/wrap/utilities.h index d3ab3e197..3837c2b95 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -16,9 +16,13 @@ #pragma once +#include #include +#include #include +namespace wrap { + class CantOpenFile : public std::exception { private: std::string filename_; @@ -38,11 +42,28 @@ class ParseFailed : public std::exception { ~ParseFailed() throw() {} virtual const char* what() const throw() { std::stringstream buf; - buf << "Parse failed at character " << (length_+1); + int len = length_+1; + buf << "Parse failed at character [" << len << "]"; return buf.str().c_str(); } }; +class DependencyMissing : public std::exception { + private: + std::string dependency_; + std::string location_; + public: + DependencyMissing(const std::string& dep, const std::string& loc) { + dependency_ = dep; + location_ = loc; + } + ~DependencyMissing() throw() {} + virtual const char* what() const throw() { + return ("Missing dependency " + dependency_ + " in " + location_).c_str(); + } +}; + + /** * read contents of a file into a std::string */ @@ -54,7 +75,27 @@ std::string file_contents(const std::string& filename, bool skipheader=false); */ bool files_equal(const std::string& expected, const std::string& actual, bool skipheader=true); +/** + * Compare strings for unit tests + */ +bool assert_equal(const std::string& expected, const std::string& actual); /** * emit a header at the top of generated files */ -void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); +void generateHeaderComment(std::ofstream& ofs, const std::string& delimiter); + +// auxiliary function to wrap an argument into a shared_ptr template +std::string maybe_shared_ptr(bool add, const std::string& type); + +/** + * Creates the "using namespace [name];" declarations + */ +void generateUsingNamespace(std::ofstream& ofs, const std::vector& using_namespaces); + +/** + * Creates the #include statements + */ +void generateIncludes(std::ofstream& ofs, const std::string& class_name, + const std::vector& includes); + +} // \namespace wrap diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index d18664cda..e2aaa6aed 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file wrap.ccp + * @file wrap.cpp * @brief wraps functions * @author Frank Dellaert **/ @@ -24,38 +24,39 @@ using namespace std; /** * Top-level function to wrap a module + * @param mexExt is the extension for mex binaries for this os/cpu * @param interfacePath path to where interface file lives, e.g., borg/gtsam * @param moduleName name of the module to be generated e.g. gtsam * @param toolboxPath path where the toolbox should be generated, e.g. borg/gtsam/build * @param nameSpace e.g. gtsam * @param mexFlags extra arguments for mex script, i.e., include flags etc... */ -void generate_matlab_toolbox(const string& interfacePath, +void generate_matlab_toolbox(const string& mexExt, + const string& interfacePath, const string& moduleName, const string& toolboxPath, - const string& nameSpace, const string& mexFlags) { // Parse interface file into class object // This recursively creates Class objects, Method objects, etc... - Module module(interfacePath, moduleName, true); + wrap::Module module(interfacePath, moduleName, true); // Then emit MATLAB code - module.matlab_code(toolboxPath,nameSpace,mexFlags); + module.matlab_code(toolboxPath,mexExt,mexFlags); } /** * main parses arguments and calls generate_matlab_toolbox above - * Typyically called from "make all" using appropriate arguments + * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { - if (argc<5 || argc>6) { + if (argc<6 || argc>7) { cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; cerr << "usage: wrap interfacePath moduleName toolboxPath" << endl; + cerr << " mexExtension : OS/CPU-dependent extension for MEX binaries" << 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