diff --git a/.cproject b/.cproject index 62b32938c..412359938 100644 --- a/.cproject +++ b/.cproject @@ -2353,6 +2353,38 @@ true true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + make -j5 @@ -3222,22 +3254,6 @@ true true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - make -j5 diff --git a/CMakeLists.txt b/CMakeLists.txt index bc7d8aecd..754279e53 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH) endif() # Set the version number for the library -set (GTSAM_VERSION_MAJOR 3) -set (GTSAM_VERSION_MINOR 1) +set (GTSAM_VERSION_MAJOR 4) +set (GTSAM_VERSION_MINOR 0) set (GTSAM_VERSION_PATCH 0) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") diff --git a/examples/Data/pose3example-rewritten.txt b/examples/Data/pose3example-rewritten.txt deleted file mode 100644 index 374e6c171..000000000 --- a/examples/Data/pose3example-rewritten.txt +++ /dev/null @@ -1,11 +0,0 @@ -VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 -VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 -VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446 -VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024 -VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488 -EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.30338 -0.513226 0.542221 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 -EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 diff --git a/gtsam.h b/gtsam.h index 678e2a3d6..40d204a5f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,8 +156,14 @@ virtual class Value { size_t dim() const; }; +class Vector3 { + Vector3(Vector v); +}; +class Vector6 { + Vector6(Vector v); +}; #include -virtual class LieScalar : gtsam::Value { +class LieScalar { // Standard constructors LieScalar(); LieScalar(double d); @@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value { }; #include -virtual class LieVector : gtsam::Value { +class LieVector { // Standard constructors LieVector(); LieVector(Vector v); @@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value { }; #include -virtual class LieMatrix : gtsam::Value { +class LieMatrix { // Standard constructors LieMatrix(); LieMatrix(Matrix v); @@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value { // geometry //************************************************************************* -virtual class Point2 : gtsam::Value { +class Point2 { // Standard Constructors Point2(); Point2(double x, double y); @@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value { void serialize() const; }; -virtual class StereoPoint2 : gtsam::Value { +class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); @@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value { void serialize() const; }; -virtual class Point3 : gtsam::Value { +class Point3 { // Standard Constructors Point3(); Point3(double x, double y, double z); @@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value { void serialize() const; }; -virtual class Rot2 : gtsam::Value { +class Rot2 { // Standard Constructors and Named Constructors Rot2(); Rot2(double theta); @@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value { void serialize() const; }; -virtual class Rot3 : gtsam::Value { +class Rot3 { // Standard Constructors and Named Constructors Rot3(); Rot3(Matrix R); @@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value { void serialize() const; }; -virtual class Pose2 : gtsam::Value { +class Pose2 { // Standard Constructor Pose2(); Pose2(const gtsam::Pose2& pose); @@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value { void serialize() const; }; -virtual class Pose3 : gtsam::Value { +class Pose3 { // Standard Constructors Pose3(); Pose3(const gtsam::Pose3& pose); @@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value { }; #include -virtual class Unit3 : gtsam::Value { +class Unit3 { // Standard Constructors Unit3(); Unit3(const gtsam::Point3& pose); @@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value { }; #include -virtual class EssentialMatrix : gtsam::Value { +class EssentialMatrix { // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); @@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value { double error(Vector vA, Vector vB); }; -virtual class Cal3_S2 : gtsam::Value { +class Cal3_S2 { // Standard Constructors Cal3_S2(); Cal3_S2(double fx, double fy, double s, double u0, double v0); @@ -643,7 +649,7 @@ virtual class Cal3_S2 : gtsam::Value { }; #include -virtual class Cal3DS2 : gtsam::Value { +class Cal3DS2 { // Standard Constructors Cal3DS2(); Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); @@ -699,7 +705,43 @@ class Cal3_S2Stereo { double baseline() const; }; -virtual class CalibratedCamera : gtsam::Value { +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double u0() const; + double v0() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase + + // enabling serialization functionality + void serialize() const; +}; + +class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); CalibratedCamera(const gtsam::Pose3& pose); @@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value { void serialize() const; }; -virtual class SimpleCamera : gtsam::Value { +class SimpleCamera { // Standard Constructors and Named Constructors SimpleCamera(); SimpleCamera(const gtsam::Pose3& pose); @@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value { }; template -virtual class PinholeCamera : gtsam::Value { +class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); PinholeCamera(const gtsam::Pose3& pose); @@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value { void serialize() const; }; -virtual class StereoCamera : gtsam::Value { +class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); @@ -862,7 +904,7 @@ virtual class SymbolicFactor { }; #include -class SymbolicFactorGraph { +virtual class SymbolicFactorGraph { SymbolicFactorGraph(); SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); @@ -1664,15 +1706,12 @@ class Values { void print(string s) const; bool equals(const gtsam::Values& other, double tol) const; - void insert(size_t j, const gtsam::Value& value); void insert(const gtsam::Values& values); - void update(size_t j, const gtsam::Value& val); void update(const gtsam::Values& values); void erase(size_t j); void swap(gtsam::Values& values); bool exists(size_t j) const; - gtsam::Value at(size_t j) const; gtsam::KeyList keys() const; gtsam::VectorValues zeroVectors() const; @@ -1682,6 +1721,37 @@ class Values { // enabling serialization functionality void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + void insert(size_t j, const gtsam::Point2& t); + void insert(size_t j, const gtsam::Point3& t); + void insert(size_t j, const gtsam::Rot2& t); + void insert(size_t j, const gtsam::Pose2& t); + void insert(size_t j, const gtsam::Rot3& t); + void insert(size_t j, const gtsam::Pose3& t); + void insert(size_t j, const gtsam::Cal3_S2& t); + void insert(size_t j, const gtsam::Cal3DS2& t); + void insert(size_t j, const gtsam::Cal3Bundler& t); + void insert(size_t j, const gtsam::EssentialMatrix& t); + + void update(size_t j, const gtsam::Point2& t); + void update(size_t j, const gtsam::Point3& t); + void update(size_t j, const gtsam::Rot2& t); + void update(size_t j, const gtsam::Pose2& t); + void update(size_t j, const gtsam::Rot3& t); + void update(size_t j, const gtsam::Pose3& t); + void update(size_t j, const gtsam::Cal3_S2& t); + void update(size_t j, const gtsam::Cal3DS2& t); + void update(size_t j, const gtsam::Cal3Bundler& t); + void update(size_t j, const gtsam::EssentialMatrix& t); + + template + T at(size_t j); }; // Actually a FastList @@ -2077,7 +2147,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2088,7 +2158,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2099,7 +2169,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2280,7 +2350,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph, namespace imuBias { #include -virtual class ConstantBias : gtsam::Value { +class ConstantBias { // Standard Constructor ConstantBias(); ConstantBias(Vector biasAcc, Vector biasGyro); @@ -2340,7 +2410,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, const gtsam::imuBias::ConstantBias& bias, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; @@ -2383,7 +2453,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in index fa5a51c70..f53e37f07 100644 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ b/gtsam/3rdparty/gtsam_eigen_includes.h.in @@ -27,3 +27,7 @@ #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> + + + + diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 1c01f7bb7..6c109675d 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -70,9 +70,9 @@ struct print { }; // equals for Matrix types -template -struct equals > { - typedef Eigen::Matrix type; +template +struct equals > { + typedef Eigen::Matrix type; typedef bool result_type; bool operator()(const type& A, const type& B, double tol) { return equal_with_abs_tol(A, B, tol); @@ -80,9 +80,9 @@ struct equals > { }; // print for Matrix types -template -struct print > { - typedef Eigen::Matrix type; +template +struct print > { + typedef Eigen::Matrix type; typedef void result_type; void operator()(const type& A, const std::string& str) { std::cout << str << ": " << A << std::endl; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index ba597ccc6..1d7eda12f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -253,10 +253,10 @@ struct DefaultChart > { typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), "DefaultChart has not been implemented yet for dynamically sized matrices"); static vector local(const T& origin, const T& other) { - return reshape(other) - reshape(origin); + return reshape(other) - reshape(origin); } static T retract(const T& origin, const vector& d) { - return origin + reshape(d); + return origin + reshape(d); } static int getDimension(const T&origin) { return origin.rows() * origin.cols(); diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 00caed44a..132bf79ad 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -294,10 +294,10 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) { inline Matrix trans(const Matrix& A) { return A.transpose(); } /// Reshape functor -template +template struct Reshape { //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff) - typedef Eigen::Map > ReshapedType; + typedef Eigen::Map > ReshapedType; static inline ReshapedType reshape(const Eigen::Matrix & in) { return in.data(); } @@ -305,7 +305,7 @@ struct Reshape { /// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output) template -struct Reshape { +struct Reshape { typedef const Eigen::Matrix & ReshapedType; static inline ReshapedType reshape(const Eigen::Matrix & in) { return in; @@ -314,7 +314,7 @@ struct Reshape { /// Reshape specialization that does nothing as shape stays the same template -struct Reshape { +struct Reshape { typedef const Eigen::Matrix & ReshapedType; static inline ReshapedType reshape(const Eigen::Matrix & in) { return in; @@ -323,17 +323,17 @@ struct Reshape { /// Reshape specialization that does transpose template -struct Reshape { +struct Reshape { typedef typename Eigen::Matrix::ConstTransposeReturnType ReshapedType; static inline ReshapedType reshape(const Eigen::Matrix & in) { return in.transpose(); } }; -template -inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ +template +inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); - return Reshape::reshape(m); + return Reshape::reshape(m); } /** diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 39f4b7996..95524e115 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -37,7 +37,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue { +class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { typedef Cal3DS2_Base Base; @@ -87,21 +87,24 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable - /// @} private: + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3DS2", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); } + /// @} + }; // Define GTSAM traits diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index b8181ab4d..e4397a449 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { } /* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, +static Matrix29 D2dcalibration(double x, double y, double xx, double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; + const Matrix2& DK) { + Matrix25 DR1; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; + Matrix24 DR2; DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; + Matrix29 D; D << DR1, DK * DR2; return D; } /* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, +static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { + const Matrix2& DK) { const double drdx = 2. * x; const double drdy = 2. * y; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; @@ -82,7 +82,7 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - Eigen::Matrix DR; + Matrix2 DR; DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // y * dgdx + dDydx, g + y * dgdy + dDydy; @@ -90,8 +90,9 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, } /* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -110,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; if (H1 || H2) DK << fx_, s_, 0.0, fy_; // Derivative for calibration @@ -125,6 +126,26 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } +/* ************************************************************************* */ +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + if (H1 || H2) { + Matrix29 D1; + Matrix2 D2; + Point2 pu = uncalibrate(p, D1, D2); + if (H1) + *H1 = D1; + if (H2) + *H2 = D2; + return pu; + + } else { + return uncalibrate(p); + } +} + /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. @@ -161,7 +182,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double rr = xx + yy; const double r4 = rr * rr; const double g = (1 + k1_ * rr + k2_ * r4); - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); } @@ -176,7 +197,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 7be5a6874..61c01e349 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -114,9 +113,14 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ + Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + Point2 uncalibrate(const Point2& p, + boost::optional Dcal, + boost::optional Dp) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; @@ -127,7 +131,6 @@ public: /// Derivative of uncalibrate wrpt the calibration parameters Matrix D2d_calibration(const Point2& p) const ; - private: /// @} diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index e7b408982..6113714a1 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { } /* ************************************************************************* */ +// todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, // Part2: project NPlane point to pixel plane: use Cal3DS2 Point2 m(x,y); - Matrix H1base, H2base; // jacobians from Base class + Matrix29 H1base; + Matrix2 H2base; // jacobians from Base class Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration if (H1) { // part1 - Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, - -ys * sqrt_nx * xi_sqrt_nx2); - Matrix DDS2U = H2base * DU; + Vector2 DU; + DU << -xs * sqrt_nx * xi_sqrt_nx2, // + -ys * sqrt_nx * xi_sqrt_nx2; - *H1 = collect(2, &H1base, &DDS2U); + H1->resize(2,10); + H1->block<2,9>(0,0) = H1base; + H1->block<2,1>(0,9) = H2base * DU; } + // Inlined derivative for points if (H2) { // part1 const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; const double mid = -(xi * xs*ys) * denom; - Matrix DU = (Matrix(2, 2) << - (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, - mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); + Matrix2 DU; + DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; *H2 = H2base * DU; } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index dc167173e..fb99592f7 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -40,7 +40,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { typedef Cal3Unified This; typedef Cal3DS2_Base Base; @@ -129,8 +129,6 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index e23b22093..8763504c0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -58,6 +58,8 @@ public: return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng)); } + virtual ~EssentialMatrix() {} + /// @} /// @name Testable diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d1dc7625c..92f43b6be 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -187,7 +187,7 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /* ************************************************************************* */ + /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const; /// Return the block diagonal of the Hessian for this factor diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index c180f1160..9af362fba 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -134,30 +135,16 @@ void BlockJacobiPreconditioner::build( size_t nnz = 0; for ( size_t i = 0 ; i < n ; ++i ) { const size_t dim = dims_[i]; - blocks.push_back(Matrix::Zero(dim, dim)); + // blocks.push_back(Matrix::Zero(dim, dim)); // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ; nnz += dim*dim; } - /* compute the block diagonal by scanning over the factors */ + /* getting the block diagonals over the factors */ BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const KeyInfoEntry &entry = keyInfo.find(*it)->second; - const Matrix &Ai = jf->getA(it); - blocks[entry.index()] += (Ai.transpose() * Ai); - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { - const KeyInfoEntry &entry = keyInfo.find(*it)->second; - const Matrix &Hii = hf->info(it, it).selfadjointView(); - blocks[entry.index()] += Hii; - } - } - else { - throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } + std::map hessianMap = gf->hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); } /* if necessary, allocating the memory for cacheing the factorization results */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index a8771f995..318c1aa5b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -57,7 +57,7 @@ namespace gtsam { class PreintegratedMeasurements { public: imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) + Matrix measurementCovariance; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) @@ -216,11 +216,21 @@ namespace gtsam { H_vel_pos, H_vel_vel, H_vel_angles, H_angles_pos, H_angles_vel, H_angles_angles; - - // first order uncertainty propagation + // first order uncertainty propagation: // the deltaT allows to pass from continuous time noise to discrete time noise + // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) + // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ; + // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT + // + // Matrix G(9,9); + // G << I_3x3 * deltaT, Z_3x3, Z_3x3, + // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, + // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; + // + // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ if(!use2ndOrderIntegration_){ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 0d559cfe6..4595a70ed 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -215,7 +215,7 @@ namespace gtsam { Values::Values(const Values::ConstFiltered& view) { BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, static_cast(key_value.value)); } } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 60639e8b7..219cacfd1 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -383,6 +383,12 @@ TEST(Values, filter) { expectedSubValues1.insert(3, pose3); EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); + // ConstFilter by Key + Values::ConstFiltered constfiltered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + EXPECT_LONGS_EQUAL(2, (long)constfiltered.size()); + Values fromconstfiltered(constfiltered); + EXPECT(assert_equal(expectedSubValues1, fromconstfiltered)); + // Filter by type i = 0; Values::ConstFiltered pose_filtered = values.filter(); diff --git a/gtsam/slam/ImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h similarity index 92% rename from gtsam/slam/ImplicitSchurFactor.h rename to gtsam/slam/RegularImplicitSchurFactor.h index 68f23e339..ecf8adfec 100644 --- a/gtsam/slam/ImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -1,5 +1,5 @@ /** - * @file ImplicitSchurFactor.h + * @file RegularImplicitSchurFactor.h * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor * @author Frank Dellaert * @author Luca Carlone @@ -17,13 +17,13 @@ namespace gtsam { /** - * ImplicitSchurFactor + * RegularImplicitSchurFactor */ template // -class ImplicitSchurFactor: public GaussianFactor { +class RegularImplicitSchurFactor: public GaussianFactor { public: - typedef ImplicitSchurFactor This; ///< Typedef to this class + typedef RegularImplicitSchurFactor This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class protected: @@ -40,11 +40,11 @@ protected: public: /// Constructor - ImplicitSchurFactor() { + RegularImplicitSchurFactor() { } /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b - ImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, + RegularImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b) : Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { initKeys(); @@ -58,7 +58,7 @@ public: } /// Destructor - virtual ~ImplicitSchurFactor() { + virtual ~RegularImplicitSchurFactor() { } // Write access, only use for construction! @@ -87,7 +87,7 @@ public: /// print void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << " ImplicitSchurFactor " << std::endl; + std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; std::cout << " E_ \n" << E_ << std::endl; @@ -96,7 +96,7 @@ public: /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) + if (!dynamic_cast(&lf)) return false; else { return false; @@ -110,21 +110,21 @@ public: virtual Matrix augmentedJacobian() const { throw std::runtime_error( - "ImplicitSchurFactor::augmentedJacobian non implemented"); + "RegularImplicitSchurFactor::augmentedJacobian non implemented"); return Matrix(); } virtual std::pair jacobian() const { - throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented"); + throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented"); return std::make_pair(Matrix(), Vector()); } virtual Matrix augmentedInformation() const { throw std::runtime_error( - "ImplicitSchurFactor::augmentedInformation non implemented"); + "RegularImplicitSchurFactor::augmentedInformation non implemented"); return Matrix(); } virtual Matrix information() const { throw std::runtime_error( - "ImplicitSchurFactor::information non implemented"); + "RegularImplicitSchurFactor::information non implemented"); return Matrix(); } @@ -210,18 +210,18 @@ public: } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, + return boost::make_shared >(Fblocks_, PointCovariance_, E_, b_); - throw std::runtime_error("ImplicitSchurFactor::clone non implemented"); + throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented"); } virtual bool empty() const { return false; } virtual GaussianFactor::shared_ptr negate() const { - return boost::make_shared >(Fblocks_, + return boost::make_shared >(Fblocks_, PointCovariance_, E_, b_); - throw std::runtime_error("ImplicitSchurFactor::negate non implemented"); + throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented"); } // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing @@ -454,7 +454,7 @@ public: } }; -// ImplicitSchurFactor +// RegularImplicitSchurFactor } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1ecaaf170..8ae26e7cd 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -21,7 +21,7 @@ #include "JacobianFactorQ.h" #include "JacobianFactorSVD.h" -#include "ImplicitSchurFactor.h" +#include "RegularImplicitSchurFactor.h" #include "RegularHessianFactor.h" #include @@ -73,7 +73,7 @@ public: /** * Constructor - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param body_P_sensor is the transform from sensor to body frame (default identity) */ SmartFactorBase(boost::optional body_P_sensor = boost::none) : body_P_sensor_(body_P_sensor) { @@ -271,8 +271,13 @@ public: Vector bi; try { - bi = - -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + if(body_P_sensor_){ + Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); + Matrix J(6, 6); + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + Fi = Fi * J; + } } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); @@ -624,11 +629,11 @@ public: } // **************************************************************************************************** - boost::shared_ptr > createImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - typename boost::shared_ptr > f( - new ImplicitSchurFactor()); + typename boost::shared_ptr > f( + new RegularImplicitSchurFactor()); computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), cameras, point, lambda, diagonalDamping); f->initKeys(); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 043528fea..bfd73d9fb 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -120,7 +120,7 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param body_P_sensor is the transform from sensor to body frame (default identity) */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, @@ -298,7 +298,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) { - std::cout << "createImplicitSchurFactor: degenerate configuration" + std::cout << "createRegularImplicitSchurFactor: degenerate configuration" << std::endl; } return false; @@ -409,12 +409,12 @@ public: } // create factor - boost::shared_ptr > createImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createImplicitSchurFactor(cameras, point_, lambda); + return Base::createRegularImplicitSchurFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::shared_ptr >(); } /// create factor @@ -685,7 +685,7 @@ public: inline bool isPointBehindCamera() const { return cheiralityException_; } - /** return chirality verbosity */ + /** return cheirality verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; } diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 273102bda..f871ab3aa 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -63,7 +63,7 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param body_P_sensor is the transform from sensor to body frame (default identity) */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, @@ -157,6 +157,9 @@ public: size_t i=0; BOOST_FOREACH(const Key& k, this->keys_) { Pose3 pose = values.at(k); + if(Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + typename Base::Camera camera(pose, *K_all_[i++]); cameras.push_back(camera); } diff --git a/gtsam/slam/tests/testImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp similarity index 94% rename from gtsam/slam/tests/testImplicitSchurFactor.cpp rename to gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 77faaacc1..0161d4fb6 100644 --- a/gtsam/slam/tests/testImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -6,7 +6,7 @@ */ //#include -#include +#include //#include #include //#include "gtsam_unstable/slam/JacobianFactorQR.h" @@ -38,19 +38,19 @@ const vector > Fblocks = list_of > // const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.); //************************************************************************************* -TEST( implicitSchurFactor, creation ) { +TEST( regularImplicitSchurFactor, creation ) { // Matrix E = Matrix::Ones(6,3); Matrix E = zeros(6, 3); E.block<2,2>(0, 0) = eye(2); E.block<2,3>(2, 0) = 2 * ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); - ImplicitSchurFactor<6> expected(Fblocks, E, P, b); + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); EXPECT(assert_equal(expectedP, P)); } /* ************************************************************************* */ -TEST( implicitSchurFactor, addHessianMultiply ) { +TEST( regularImplicitSchurFactor, addHessianMultiply ) { Matrix E = zeros(6, 3); E.block<2,2>(0, 0) = eye(2); @@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) { keys += 0,1,2,3; Vector x = xvalues.vector(keys); Vector expected = zero(24); - ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); + RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); // Create ImplicitSchurFactor - ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); VectorValues zero = 0 * yExpected;// quick way to get zero w right structure { // First Version @@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) { } /* ************************************************************************* */ -TEST(implicitSchurFactor, hessianDiagonal) +TEST(regularImplicitSchurFactor, hessianDiagonal) { /* TESTED AGAINST MATLAB * F = [ones(2,6) zeros(2,6) zeros(2,6) @@ -207,7 +207,7 @@ TEST(implicitSchurFactor, hessianDiagonal) E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; Matrix3 P = (E.transpose() * E).inverse(); - ImplicitSchurFactor<6> factor(Fblocks, E, P, b); + RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b); // hessianDiagonal VectorValues expected; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 6b849ba5a..4e4fde3e4 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ if(isDebugTest) tictoc_print_(); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + + SimpleCamera cam1(cameraPose1, *K); // with camera poses + SimpleCamera cam2(cameraPose2, *K); + SimpleCamera cam3(cameraPose3, *K); + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // These are the poses we want to estimate, from camera measurements + Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); + Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); + Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + double rankTol = 1; + double linThreshold = -1; + bool manageDegeneracy = false; + bool enableEPI = false; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, bodyPose1); + gtValues.insert(x2, bodyPose2); + gtValues.insert(x3, bodyPose3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Values values; + values.insert(x1, bodyPose1); + values.insert(x2, bodyPose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, bodyPose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} + + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 27460bd72..95e635516 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -4,14 +4,15 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; -virtual class gtsam::LieScalar; -virtual class gtsam::LieVector; -virtual class gtsam::Point2; -virtual class gtsam::Rot2; -virtual class gtsam::Pose2; -virtual class gtsam::Point3; -virtual class gtsam::Rot3; -virtual class gtsam::Pose3; +class gtsam::Vector6; +class gtsam::LieScalar; +class gtsam::LieVector; +class gtsam::Point2; +class gtsam::Rot2; +class gtsam::Pose2; +class gtsam::Point3; +class gtsam::Rot3; +class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; @@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; -virtual class gtsam::Cal3_S2; -virtual class gtsam::Cal3DS2; +class gtsam::Cal3_S2; +class gtsam::Cal3DS2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -48,7 +49,7 @@ class Dummy { }; #include -virtual class PoseRTV : gtsam::Value { +class PoseRTV { PoseRTV(); PoseRTV(Vector rtv); PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); @@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value { }; #include -virtual class Pose3Upright : gtsam::Value { +class Pose3Upright { Pose3Upright(); Pose3Upright(const gtsam::Pose3Upright& x); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); @@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value { }; // \class Pose3Upright #include -virtual class BearingS2 : gtsam::Value { +class BearingS2 { BearingS2(); BearingS2(double azimuth, double elevation); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); @@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { virtual class Reconstruction : gtsam::NonlinearFactor { Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); - Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const; + Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const; }; virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, double h, Matrix Inertia, Vector Fu, double m); - Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const; + Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const; }; //************************************************************************* diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index f49b985ba..40fc54751 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -64,7 +64,8 @@ public: } /// Access via key VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(),keys_.end(),key); + FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), + key); DenseIndex block = it - keys_.begin(); return Ab_(block); } @@ -98,7 +99,7 @@ struct CallRecord { template void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference + jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> @@ -311,9 +312,9 @@ public: //----------------------------------------------------------------------------- /// Leaf Expression -template > +template > class LeafExpression: public ExpressionNode { - typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? + typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? /// The key into values Key key_; @@ -347,8 +348,8 @@ public: } /// Construct an execution trace for reverse AD - virtual const value_type& traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + virtual const value_type& traceExecution(const Values& values, + ExecutionTrace& trace, char* raw) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); } @@ -358,7 +359,7 @@ public: //----------------------------------------------------------------------------- /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression >: public ExpressionNode { +class LeafExpression > : public ExpressionNode { typedef T value_type; /// The key into values @@ -405,6 +406,7 @@ public: // C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost // and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. // to recursively generate a class, that will be the base for function nodes. +// // The class generated, for three arguments A1, A2, and A3 will be // // struct Base1 : Argument, FunctionalBase { @@ -429,6 +431,30 @@ public: // // All this magic happens when we generate the Base3 base class of FunctionalNode // by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode +// +// Similarly, the inner Record struct will be +// +// struct Record1 : JacobianTrace, CallRecord::value> { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Record2 : JacobianTrace, Record1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A1 ... +// }; +// +// struct Record3 : JacobianTrace, Record2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... +// }; +// +// struct Record : Record3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// + //----------------------------------------------------------------------------- /// meta-function to generate fixed-size JacobianTA type @@ -457,6 +483,7 @@ struct FunctionalBase: ExpressionNode { /// Construct an execution trace for reverse AD void trace(const Values& values, Record* record, char*& raw) const { + // base case: does not do anything } }; @@ -562,15 +589,23 @@ struct GenerateFunctionalNode: Argument, Base { template struct FunctionalNode { + /// The following typedef generates the recursively defined Base class typedef typename boost::mpl::fold, GenerateFunctionalNode >::type Base; + /** + * The type generated by this meta-function derives from Base + * and adds access functions as well as the crucial [trace] function + */ struct type: public Base { // Argument types and derived, note these are base 0 ! + // These are currently not used - useful for Phoenix in future +#ifdef EXPRESSIONS_PHOENIX typedef TYPES Arguments; typedef typename boost::mpl::transform >::type Jacobians; typedef typename boost::mpl::transform >::type Optionals; +#endif /// Reset expression shared pointer template @@ -725,7 +760,8 @@ public: typedef boost::function< T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, typename OptionalJacobian::type)> Function; + typename OptionalJacobian::type, + typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index 84a1ca720..b2cdcdf34 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; using namespace gtsam; @@ -143,7 +144,7 @@ TEST(ExpressionFactor, Triple) { // Test out invoke TEST(ExpressionFactor, Invoke) { - assert(invoke(add,boost::fusion::make_vector(1,1)) == 2); + EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); // Creating a Pose3 (is there another way?) boost::fusion::vector pair; diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 849cf7a05..ace13dc41 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -17,12 +17,14 @@ #include #include +#include "FindSeparator.h" + extern "C" { #include #include "metislib.h" } -#include "FindSeparator.h" + namespace gtsam { namespace partition { diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index c147552b3..9082c0101 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -25,384 +25,405 @@ namespace gtsam { +/** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam VALUE the Value type + * @addtogroup SLAM + */ +template +class BetweenFactorEM: public NonlinearFactor { + +public: + + typedef VALUE T; + +private: + + typedef BetweenFactorEM This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key1_; + gtsam::Key key2_; + + VALUE measured_; /** The measurement */ + + SharedGaussian model_inlier_; + SharedGaussian model_outlier_; + + double prior_inlier_; + double prior_outlier_; + + bool flag_bump_up_near_zero_probs_; + + /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T) + +public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + BetweenFactorEM() { + } + + /** Constructor */ + BetweenFactorEM(Key key1, Key key2, const VALUE& measured, + const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, + const double prior_inlier, const double prior_outlier, + const bool flag_bump_up_near_zero_probs = false) : + Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_( + measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_( + prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_( + flag_bump_up_near_zero_probs) { + } + + virtual ~BetweenFactorEM() { + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << "," + << keyFormatter(key2_) << ")\n"; + measured_.print(" measured: "); + model_inlier_->print(" noise model inlier: "); + model_outlier_->print(" noise model outlier: "); + std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << "," + << prior_outlier_ << ")\n"; + // Base::print(s, keyFormatter); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This *t = dynamic_cast(&f); + + if (t && Base::equals(f)) + return key1_ == t->key1_ && key2_ == t->key2_ + && + // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here + // model_outlier_->equals(t->model_outlier_ ) && + prior_outlier_ == t->prior_outlier_ + && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_); + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + virtual double error(const gtsam::Values& x) const { + return whitenedError(x).squaredNorm(); + } + + /* ************************************************************************* */ /** - * A class for a measurement predicted by "between(config[key1],config[key2])" - * @tparam VALUE the Value type - * @addtogroup SLAM + * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - template - class BetweenFactorEM: public NonlinearFactor { + /* This version of linearize recalculates the noise model each time */ + virtual boost::shared_ptr linearize( + const gtsam::Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); - public: + //std::cout<<"About to linearize"< A(this->size()); + gtsam::Vector b = -whitenedError(x, A); + A1 = A[0]; + A2 = A[1]; - typedef VALUE T; + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, + gtsam::noiseModel::Unit::Create(b.size()))); + } - private: + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { - typedef BetweenFactorEM This; - typedef gtsam::NonlinearFactor Base; + bool debug = true; - gtsam::Key key1_; - gtsam::Key key2_; + const T& p1 = x.at(key1_); + const T& p2 = x.at(key2_); - VALUE measured_; /** The measurement */ + Matrix H1, H2; - SharedGaussian model_inlier_; - SharedGaussian model_outlier_; + T hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) - double prior_inlier_; - double prior_outlier_; + Vector err = measured_.localCoordinates(hx); - bool flag_bump_up_near_zero_probs_; + // Calculate indicator probabilities (inlier and outlier) + Vector p_inlier_outlier = calcIndicatorProb(x); + double p_inlier = p_inlier_outlier[0]; + double p_outlier = p_inlier_outlier[1]; - /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); - public: + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() + * model_outlier_->R(); - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + Vector err_wh_eq; + err_wh_eq.resize(err_wh_inlier.rows() * 2); + err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier) + * err_wh_outlier.array(); - /** default constructor - only use for serialization */ - BetweenFactorEM() {} + if (H) { + // stack Jacobians for the two indicators for each of the key - /** Constructor */ - BetweenFactorEM(Key key1, Key key2, const VALUE& measured, - const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, - const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) : - Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured), - model_inlier_(model_inlier), model_outlier_(model_outlier), - prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){ + Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1); + Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1); + Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); + + Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2); + Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2); + Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); + + (*H)[0].resize(H1_aug.rows(), H1_aug.cols()); + (*H)[1].resize(H2_aug.rows(), H2_aug.cols()); + + (*H)[0] = H1_aug; + (*H)[1] = H2_aug; } - virtual ~BetweenFactorEM() {} + if (debug) { + // std::cout<<"unwhitened error: "<print(" noise model inlier: "); - model_outlier_->print(" noise model outlier: "); - std::cout << "(prior_inlier, prior_outlier_) = (" - << prior_inlier_ << "," - << prior_outlier_ << ")\n"; - // Base::print(s, keyFormatter); + // Matrix Cov_inlier = invCov_inlier.inverse(); + // Matrix Cov_outlier = invCov_outlier.inverse(); + // std::cout<<"Cov_inlier: "< (&f); + return err_wh_eq; + } - if(t && Base::equals(f)) - return key1_ == t->key1_ && key2_ == t->key2_ && - // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here - // model_outlier_->equals(t->model_outlier_ ) && - prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_); - else - return false; + /* ************************************************************************* */ + gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const { + + bool debug = false; + + Vector err = unwhitenedError(x); + + // Calculate indicator probabilities (inlier and outlier) + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); + + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() + * model_outlier_->R(); + + double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) + * exp(-0.5 * err_wh_inlier.dot(err_wh_inlier)); + double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) + * exp(-0.5 * err_wh_outlier.dot(err_wh_outlier)); + + if (debug) { + std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", " + << err[1] << ", " << err[2] << std::endl; + std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0] + << ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl; + std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): " + << err_wh_inlier.dot(err_wh_inlier) << std::endl; + std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): " + << err_wh_outlier.dot(err_wh_outlier) << std::endl; + + std::cout + << "in calcIndicatorProb. p_inlier, p_outlier before normalization: " + << p_inlier << ", " << p_outlier << std::endl; } - /** implement functions needed to derive from Factor */ + double sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; - /* ************************************************************************* */ - virtual double error(const gtsam::Values& x) const { - return whitenedError(x).squaredNorm(); + if (flag_bump_up_near_zero_probs_) { + // Bump up near-zero probabilities (as in linerFlow.h) + double minP = 0.05; // == 0.1 / 2 indicator variables + if (p_inlier < minP || p_outlier < minP) { + if (p_inlier < minP) + p_inlier = minP; + if (p_outlier < minP) + p_outlier = minP; + sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + } } - /* ************************************************************************* */ - /** - * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, - * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ - * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + return (Vector(2) << p_inlier, p_outlier); + } + + /* ************************************************************************* */ + gtsam::Vector unwhitenedError(const gtsam::Values& x) const { + + const T& p1 = x.at(key1_); + const T& p2 = x.at(key2_); + + Matrix H1, H2; + + T hx = p1.between(p2, H1, H2); // h(x) + + return measured_.localCoordinates(hx); + } + + /* ************************************************************************* */ + void set_flag_bump_up_near_zero_probs(bool flag) { + flag_bump_up_near_zero_probs_ = flag; + } + + /* ************************************************************************* */ + bool get_flag_bump_up_near_zero_probs() const { + return flag_bump_up_near_zero_probs_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_inlier() const { + return model_inlier_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_outlier() const { + return model_outlier_; + } + + /* ************************************************************************* */ + Matrix get_model_inlier_cov() const { + return (model_inlier_->R().transpose() * model_inlier_->R()).inverse(); + } + + /* ************************************************************************* */ + Matrix get_model_outlier_cov() const { + return (model_outlier_->R().transpose() * model_outlier_->R()).inverse(); + } + + /* ************************************************************************* */ + void updateNoiseModels(const gtsam::Values& values, + const gtsam::NonlinearFactorGraph& graph) { + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) */ - /* This version of linearize recalculates the noise model each time */ - virtual boost::shared_ptr linearize(const gtsam::Values& x) const { - // Only linearize if the factor is active - if (!this->active(x)) - return boost::shared_ptr(); - //std::cout<<"About to linearize"< A(this->size()); - gtsam::Vector b = -whitenedError(x, A); - A1 = A[0]; - A2 = A[1]; + // get joint covariance of the involved states + std::vector Keys; + Keys.push_back(key1_); + Keys.push_back(key2_); + Marginals marginals(graph, values, Marginals::QR); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(key1_, key1_); + Matrix cov2 = joint_marginal12(key2_, key2_); + Matrix cov12 = joint_marginal12(key1_, key2_); - return gtsam::GaussianFactor::shared_ptr( - new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size()))); - } + updateNoiseModels_givenCovs(values, cov1, cov2, cov12); + } + /* ************************************************************************* */ + void updateNoiseModels_givenCovs(const gtsam::Values& values, + const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) { + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ - /* ************************************************************************* */ - gtsam::Vector whitenedError(const gtsam::Values& x, - boost::optional&> H = boost::none) const { + const T& p1 = values.at(key1_); + const T& p2 = values.at(key2_); - bool debug = true; + Matrix H1, H2; + p1.between(p2, H1, H2); // h(x) - const T& p1 = x.at(key1_); - const T& p2 = x.at(key2_); + Matrix H; + H.resize(H1.rows(), H1.rows() + H2.rows()); + H << H1, H2; // H = [H1 H2] - Matrix H1, H2; + Matrix joint_cov; + joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols()); + joint_cov << cov1, cov12, cov12.transpose(), cov2; - T hx = p1.between(p2, H1, H2); // h(x) - // manifold equivalent of h(x)-z -> log(z,h(x)) + Matrix cov_state = H * joint_cov * H.transpose(); - Vector err = measured_.localCoordinates(hx); + // model_inlier_->print("before:"); - // Calculate indicator probabilities (inlier and outlier) - Vector p_inlier_outlier = calcIndicatorProb(x); - double p_inlier = p_inlier_outlier[0]; - double p_outlier = p_inlier_outlier[1]; + // update inlier and outlier noise models + Matrix covRinlier = + (model_inlier_->R().transpose() * model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance( + covRinlier + cov_state); - Vector err_wh_inlier = model_inlier_->whiten(err); - Vector err_wh_outlier = model_outlier_->whiten(err); + Matrix covRoutlier = + (model_outlier_->R().transpose() * model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance( + covRoutlier + cov_state); - Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); - Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); + // model_inlier_->print("after:"); + // std::cout<<"covRinlier + cov_state: "<Whiten(H1); - Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1); - Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); + virtual size_t dim() const { + return model_inlier_->R().rows() + model_inlier_->R().cols(); + } - Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2); - Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2); - Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); +private: - (*H)[0].resize(H1_aug.rows(),H1_aug.cols()); - (*H)[1].resize(H2_aug.rows(),H2_aug.cols()); + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; +// \class BetweenFactorEM - (*H)[0] = H1_aug; - (*H)[1] = H2_aug; - } - - if (debug){ - // std::cout<<"unwhitened error: "<whiten(err); - Vector err_wh_outlier = model_outlier_->whiten(err); - - Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); - Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); - - double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); - double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); - - if (debug){ - std::cout<<"in calcIndicatorProb. err_unwh: "<(key1_); - const T& p2 = x.at(key2_); - - Matrix H1, H2; - - T hx = p1.between(p2, H1, H2); // h(x) - - return measured_.localCoordinates(hx); - } - - /* ************************************************************************* */ - void set_flag_bump_up_near_zero_probs(bool flag) { - flag_bump_up_near_zero_probs_ = flag; - } - - /* ************************************************************************* */ - bool get_flag_bump_up_near_zero_probs() const { - return flag_bump_up_near_zero_probs_; - } - - /* ************************************************************************* */ - SharedGaussian get_model_inlier() const { - return model_inlier_; - } - - /* ************************************************************************* */ - SharedGaussian get_model_outlier() const { - return model_outlier_; - } - - /* ************************************************************************* */ - Matrix get_model_inlier_cov() const { - return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); - } - - /* ************************************************************************* */ - Matrix get_model_outlier_cov() const { - return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); - } - - /* ************************************************************************* */ - void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ - - // get joint covariance of the involved states - std::vector Keys; - Keys.push_back(key1_); - Keys.push_back(key2_); - Marginals marginals( graph, values, Marginals::QR ); - JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); - Matrix cov1 = joint_marginal12(key1_, key1_); - Matrix cov2 = joint_marginal12(key2_, key2_); - Matrix cov12 = joint_marginal12(key1_, key2_); - - updateNoiseModels_givenCovs(values, cov1, cov2, cov12); - } - - /* ************************************************************************* */ - void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ - - const T& p1 = values.at(key1_); - const T& p2 = values.at(key2_); - - Matrix H1, H2; - T hx = p1.between(p2, H1, H2); // h(x) - - Matrix H; - H.resize(H1.rows(), H1.rows()+H2.rows()); - H << H1, H2; // H = [H1 H2] - - Matrix joint_cov; - joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); - joint_cov << cov1, cov12, - cov12.transpose(), cov2; - - Matrix cov_state = H*joint_cov*H.transpose(); - - // model_inlier_->print("before:"); - - // update inlier and outlier noise models - Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); - model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); - - Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); - model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); - - // model_inlier_->print("after:"); - // std::cout<<"covRinlier + cov_state: "<R().rows() + model_inlier_->R().cols(); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; // \class BetweenFactorEM - -} /// namespace gtsam +}/// namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 35010cdc6..2f0b2c7a8 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -372,7 +372,7 @@ namespace gtsam { const T& p2 = values.at(keyB_); Matrix H1, H2; - T hx = p1.between(p2, H1, H2); // h(x) + p1.between(p2, H1, H2); // h(x) Matrix H; H.resize(H1.rows(), H1.rows()+H2.rows()); diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index 134c318cb..f0bbb9072 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -57,7 +57,8 @@ int main(int argc, char* argv[]) { volatile double fpm = 0.5; // fraction of points matched volatile size_t nm = fpm * n * np; // number of matches - cout << format("\nTesting with %1% images, %2% points, %3% matches\n") % m % N % nm; + cout << format("\nTesting with %1% images, %2% points, %3% matches\n") + % (int)m % (int)N % (int)nm; cout << "Generating " << nm << " matches" << endl; boost::variate_generator > rn( boost::mt19937(), boost::uniform_int(0, N - 1)); @@ -67,7 +68,7 @@ int main(int argc, char* argv[]) { for (size_t k = 0; k < nm; k++) matches.push_back(Match(rn(), rn())); - os << format("%1%,%2%,%3%,") % m % N % nm; + os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm; { // DSFBase version diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index d5a851f1d..dabf283c6 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -35,12 +35,12 @@ gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ -gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { +gtsam::Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity& factor) { return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); } -gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { - return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); +gtsam::Vector3 predictionErrorVel(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity& factor) { + return factor.evaluateError(p1, v1, b1, p2, v2).tail(3); } #include @@ -64,21 +64,21 @@ int main() { Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); + Vector3 Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); - LieVector Vel2 = Vel1.compose( dv ); + Vector3 Vel2 = Vel1 + dv; imuBias::ConstantBias Bias1; Values values; diff --git a/gtsam_unstable/timing/timeSFMExpressions.cpp b/gtsam_unstable/timing/timeSFMExpressions.cpp index fc2a8d97e..678e4db60 100644 --- a/gtsam_unstable/timing/timeSFMExpressions.cpp +++ b/gtsam_unstable/timing/timeSFMExpressions.cpp @@ -47,15 +47,15 @@ int main() { // Create values Values values; values.insert(Symbol('K', 0), Cal3_S2()); - for (int i = 0; i < M; i++) + for (size_t i = 0; i < M; i++) values.insert(Symbol('x', i), Pose3()); - for (int j = 0; j < N; j++) + for (size_t j = 0; j < N; j++) values.insert(Symbol('p', j), Point3(0, 0, 1)); long timeLog = clock(); NonlinearFactorGraph graph; - for (int i = 0; i < M; i++) { - for (int j = 0; j < N; j++) { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { NonlinearFactor::shared_ptr f = boost::make_shared< ExpressionFactor > #ifdef TERNARY diff --git a/matlab/+gtsam/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m index 874dbf523..9b52f016f 100644 --- a/matlab/+gtsam/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals); M = 1; while result.exists(symbol('x',M)) ii = symbol('x',M); - pose_i = result.at(ii); + pose_i = result.atPose3(ii); if options.hardConstraint && (M==1) gtsam.plotPose3(pose_i,[],10); else diff --git a/matlab/+gtsam/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m index 82b3754ef..6fa81e072 100644 --- a/matlab/+gtsam/VisualISAMStep.m +++ b/matlab/+gtsam/VisualISAMStep.m @@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex}) end %% Initial estimates for the new pose. -prevPose = result.at(symbol('x',nextPoseIndex-1)); +prevPose = result.atPose3(symbol('x',nextPoseIndex-1)); initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry)); %% Update ISAM diff --git a/matlab/+gtsam/load3D.m b/matlab/+gtsam/load3D.m index 94202e0c8..d536162e1 100644 --- a/matlab/+gtsam/load3D.m +++ b/matlab/+gtsam/load3D.m @@ -46,9 +46,9 @@ for i=1:n graph.add(BetweenFactorPose3(i1, i2, dpose, model)); if successive if i2>i1 - initial.insert(i2,initial.at(i1).compose(dpose)); + initial.insert(i2,initial.atPose3(i1).compose(dpose)); else - initial.insert(i1,initial.at(i2).compose(dpose.inverse)); + initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse)); end end end diff --git a/matlab/+gtsam/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m index d4703a5d7..8e91fa19d 100644 --- a/matlab/+gtsam/plot2DPoints.m +++ b/matlab/+gtsam/plot2DPoints.m @@ -18,15 +18,18 @@ hold on % Plot points and covariance matrices for i = 0:keys.size-1 key = keys.at(i); - p = values.at(key); - if isa(p, 'gtsam.Point2') + try + p = values.atPoint2(key); if haveMarginals P = marginals.marginalCovariance(key); gtsam.plotPoint2(p, linespec, P); else gtsam.plotPoint2(p, linespec); end + catch err + % I guess it's not a Point2 end + end if ~holdstate diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index 45e7fe467..1c29213cd 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,7 +8,7 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); @@ -25,24 +25,26 @@ plot(X,Y,linespec); % Quiver can also be vectorized if no marginals asked if ~haveMarginals - C = cos(theta); - S = sin(theta); - quiver(X,Y,C,S,0.1,linespec); + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); else - % plotPose2 does both quiver and covariance matrix - keys = KeyVector(values.keys); - for i = 0:keys.size-1 - key = keys.at(i); - x = values.at(key); - if isa(x, 'gtsam.Pose2') - P = marginals.marginalCovariance(key); - gtsam.plotPose2(x,linespec(1), P); + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 + key = keys.at(i); + try + x = values.atPose2(key); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); + catch err + % I guess it's not a Pose2 + end end - end end if ~holdstate - hold off + hold off end end diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 8feab1744..46e161807 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -18,14 +18,16 @@ hold on % Plot points and covariance matrices for i = 0:keys.size-1 key = keys.at(i); - p = values.at(key); - if isa(p, 'gtsam.Point3') + try + p = values.atPoint3(key); if haveMarginals P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); else gtsam.plotPoint3(p, linespec); end + catch + % I guess it's not a Point3 end end diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index d24e21297..05483e589 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -17,39 +17,48 @@ hold on lastIndex = []; for i = 0:keys.size-1 key = keys.at(i); - x = values.at(key); - if isa(x, 'gtsam.Pose3') + try + x = values.atPose3(key); if ~isempty(lastIndex) % Draw line from last pose then covariance ellipse on top of % last pose. lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + try + lastPose = values.atPose3(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + gtsam.plotPose3(lastPose, P, scale); + catch err + warning(['no Pose3 at ' lastKey]); + end + lastIndex = i; + end + catch + warning(['no Pose3 at ' key]); + end + + % Draw final pose + if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + try + lastPose = values.atPose3(lastKey); if haveMarginals P = marginals.marginalCovariance(lastKey); else P = []; end gtsam.plotPose3(lastPose, P, scale); + catch + warning(['no Pose3 at ' lastIndex]); end - lastIndex = i; end -end - -% Draw final pose -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - else - P = []; + + if ~holdstate + hold off end - gtsam.plotPose3(lastPose, P, scale); -end - -if ~holdstate - hold off -end - + end \ No newline at end of file diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m index ef1516017..aec933d74 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -71,9 +71,9 @@ marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); plot2DPoints(result, 'b', marginals); -plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); -plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); -plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-'); +plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-'); +plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-'); +plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-'); axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/gtsam_examples/PlanarSLAMExample_graph.m b/matlab/gtsam_examples/PlanarSLAMExample_graph.m index 9ca88e49a..db6484c5c 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_graph.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_graph.m @@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]); [graph,initial] = load2D(datafile, model); %% Add a Gaussian prior on a pose in the middle -priorMean = initial.at(40); +priorMean = initial.atPose2(40); priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]); graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 327c64d4d..375ed645c 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); - point{j} = sample.at(key); + point{j} = sample.atPoint2(key); Q{j}=marginals.marginalCovariance(key); S{j}=chol(Q{j}); % for sampling end -plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); -plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); -plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-'); +plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-'); +plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-'); +plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-'); view(2); axis auto; axis equal %% Do Sampling on point 2 diff --git a/matlab/gtsam_examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m index 65ce28dbb..789d1c483 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses cla; hold on -plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-'); +plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-'); marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m index 3d2265d76..d2676845c 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_circle.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose2(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose2(0); +p1 = hexagon.atPose2(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config initial = Values; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); -initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); -initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); -initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); -initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); +initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate cla diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index 83ec949cc..c479278c1 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -41,7 +41,7 @@ marginals = Marginals(graph, result); toc P={}; for i=1:result.size()-1 - pose_i = result.at(i); + pose_i = result.atPose2(i); P{i}=marginals.marginalCovariance(i); plotPose2(pose_i,'b',P{i}) end diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m index 1d9c3b579..5a9c8bf03 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose3(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose3(0); +p1 = hexagon.atPose3(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 39e48c204..de6f9e86d 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0. %% Plot Initial Estimate cla -first = initial.at(0); +first = initial.atPose3(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3DTrajectory(initial,'g-',false); drawnow; diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index b77733abd..464448335 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -45,7 +45,7 @@ for i=1:size(measurements,1) if ~initial.exists(symbol('l',sf(2))) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose - pose = initial.at(symbol('x', sf(1))); + pose = initial.atPose3(symbol('x', sf(1))); world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); initial.insert(symbol('l',sf(2)), world_point); end @@ -54,7 +54,7 @@ toc %% add a constraint on the starting pose key = symbol('x',1); -first_pose = initial.at(key); +first_pose = initial.atPose3(key); graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize diff --git a/matlab/gtsam_tests/testLocalizationExample.m b/matlab/gtsam_tests/testLocalizationExample.m index 5f1d89c99..ed091ea71 100644 --- a/matlab/gtsam_tests/testLocalizationExample.m +++ b/matlab/gtsam_tests/testLocalizationExample.m @@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); for i=1:3 - graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); + graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)); end %% Initialize to noisy points @@ -46,7 +46,7 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); P={}; for i=1:result.size() - pose_i = result.at(i); - CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); + pose_i = result.atPose2(i); + CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4)); P{i}=marginals.marginalCovariance(i); end diff --git a/matlab/gtsam_tests/testOdometryExample.m b/matlab/gtsam_tests/testOdometryExample.m index 442b36801..9bd4762a7 100644 --- a/matlab/gtsam_tests/testOdometryExample.m +++ b/matlab/gtsam_tests/testOdometryExample.m @@ -39,5 +39,5 @@ marginals = Marginals(graph, result); marginals.marginalCovariance(1); %% Check first pose equality -pose_1 = result.at(1); +pose_1 = result.atPose2(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index 7694a1a0b..d3cab5d1f 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -60,10 +60,10 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); %% Check first pose and point equality -pose_1 = result.at(symbol('x',1)); +pose_1 = result.atPose2(symbol('x',1)); marginals.marginalCovariance(symbol('x',1)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -point_1 = result.at(symbol('l',1)); +point_1 = result.atPoint2(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 8c70c27e7..72e5331f2 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -57,6 +57,6 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); P = marginals.marginalCovariance(1); -pose_1 = result.at(1); +pose_1 = result.atPose2(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/gtsam_tests/testPose3SLAMExample.m b/matlab/gtsam_tests/testPose3SLAMExample.m index dafad4e47..51ba69240 100644 --- a/matlab/gtsam_tests/testPose3SLAMExample.m +++ b/matlab/gtsam_tests/testPose3SLAMExample.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose3(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose3(0); +p1 = hexagon.atPose3(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1))); %% optimize optimizer = LevenbergMarquardtOptimizer(fg, initial); result = optimizer.optimizeSafely; -pose_1 = result.at(1); +pose_1 = result.atPose3(1); CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4)); diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m index 2b04be8f1..985cbdb2c 100644 --- a/matlab/gtsam_tests/testSFMExample.m +++ b/matlab/gtsam_tests/testSFMExample.m @@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1)); %% Check optimized results, should be equal to ground truth for i=1:size(truth.cameras,2) - pose_i = result.at(symbol('x',i)); + pose_i = result.atPose3(symbol('x',i)); CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) end for j=1:size(truth.points,2) - point_j = result.at(symbol('p',j)); + point_j = result.atPoint3(symbol('p',j)); CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) end diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m index e2d6f2479..218d0ace1 100644 --- a/matlab/gtsam_tests/testStereoVOExample.m +++ b/matlab/gtsam_tests/testStereoVOExample.m @@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); result = optimizer.optimize(); %% check equality for the first pose and point -pose_x1 = result.at(x1); +pose_x1 = result.atPose3(x1); CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4)); -point_l1 = result.at(l1); +point_l1 = result.atPoint3(l1); CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testVisualISAMExample.m b/matlab/gtsam_tests/testVisualISAMExample.m index 40aca458e..223e823a6 100644 --- a/matlab/gtsam_tests/testVisualISAMExample.m +++ b/matlab/gtsam_tests/testVisualISAMExample.m @@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras end for i=1:size(truth.cameras,2) - pose_i = result.at(symbol('x',i)); + pose_i = result.atPose3(symbol('x',i)); CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) end for j=1:size(truth.points,2) - point_j = result.at(symbol('l',j)); + point_j = result.atPoint3(symbol('l',j)); CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) end diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index c379179c5..e3705948d 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -30,11 +30,11 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample -display 'Starting: testSerialization' -testSerialization - display 'Starting: testUtilities' testUtilities +display 'Starting: testSerialization' +testSerialization + % end of tests display 'Tests complete!' diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index 844987324..182533672 100755 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -61,4 +61,4 @@ if [ $? -ne 0 ]; then fi # Create package -tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox +tar czf gtsam-toolbox-3.2.0-$platform.tgz -C stage/gtsam_toolbox toolbox diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 38a40521a..04c4e9d52 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -13,6 +13,7 @@ * @file testPCGSolver.cpp * @brief Unit tests for PCGSolver class * @author Yong-Dian Jian + * @date Aug 06, 2014 */ #include @@ -51,6 +52,7 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ +// Test cholesky decomposition TEST( PCGSolver, llt ) { Matrix R = (Matrix(3,3) << 1., -1., -1., @@ -90,6 +92,7 @@ TEST( PCGSolver, llt ) { } /* ************************************************************************* */ +// Test Dummy Preconditioner TEST( PCGSolver, dummy ) { LevenbergMarquardtParams paramsPCG; @@ -110,6 +113,7 @@ TEST( PCGSolver, dummy ) } /* ************************************************************************* */ +// Test Block-Jacobi Precondioner TEST( PCGSolver, blockjacobi ) { LevenbergMarquardtParams paramsPCG; @@ -130,6 +134,7 @@ TEST( PCGSolver, blockjacobi ) } /* ************************************************************************* */ +// Test Incremental Subgraph PCG Solver TEST( PCGSolver, subgraph ) { LevenbergMarquardtParams paramsPCG; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp new file mode 100644 index 000000000..40d49a934 --- /dev/null +++ b/tests/testPreconditioner.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPreconditioner.cpp + * @brief Unit tests for Preconditioners + * @author Sungtae An + * @date Nov 6, 2014 + **/ + +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +static GaussianFactorGraph createSimpleGaussianFactorGraph() { + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2); + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2); + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); + return fg; +} + +/* ************************************************************************* */ +// Copy of BlockJacobiPreconditioner::build +std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) +{ + const size_t n = keyInfo.size(); + std::vector dims_ = keyInfo.colSpec(); + + /* prepare the buffer of block diagonals */ + std::vector blocks; blocks.reserve(n); + + /* allocate memory for the factorization of block diagonals */ + size_t nnz = 0; + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t dim = dims_[i]; + blocks.push_back(Matrix::Zero(dim, dim)); + // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ; + nnz += dim*dim; + } + + /* compute the block diagonal by scanning over the factors */ + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + const KeyInfoEntry &entry = keyInfo.find(*it)->second; + const Matrix &Ai = jf->getA(it); + blocks[entry.index()] += (Ai.transpose() * Ai); + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { + const KeyInfoEntry &entry = keyInfo.find(*it)->second; + const Matrix &Hii = hf->info(it, it).selfadjointView(); + blocks[entry.index()] += Hii; + } + } + else { + throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } + + return blocks; +} + +/* ************************************************************************* */ +TEST( Preconditioner, buildBlocks ) { + // Create simple Gaussian factor graph and initial values + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); + Values initial; + initial.insert(0,Point2(4, 5)); + initial.insert(1,Point2(0, 1)); + initial.insert(2,Point2(-5, 7)); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =gfg.hessianBlockDiagonal(); + + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); + + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + for(; it1!=expectedHessian.end(); it1++, it2++) + EXPECT(assert_equal(it1->second, *it2)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index e924c8402..a2d11a756 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -62,9 +62,9 @@ int main(int argc, char* argv[]) { gtsam::SubMatrix top = mat.block(0, 0, n, n); gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2); - cout << format(" Basic: %1%x%2%\n") % m % n; - cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n; - cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % n % 0 % n; + cout << format(" Basic: %1%x%2%\n") % (int)m % (int)n; + cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)m % 0 % (int)n; + cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)n % 0 % (int)n; cout << format(" Block: mat(%1%:%2%, %3%:%4%)\n") % size_t(m/4) % size_t(m-m/4) % size_t(n/4) % size_t(n-n/4); cout << endl; diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 2113ad56d..bd39589f4 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -89,7 +89,7 @@ int main() Matrix Dpose, Dpoint; long timeLog = clock(); for(int i = 0; i < n; i++) - camera.project(point1, Dpose, Dpoint); + camera.project(point1, Dpose, Dpoint, boost::none); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index d76556e4a..4989afb0d 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -28,34 +28,53 @@ using namespace std; using namespace wrap; +/* ************************************************************************* */ +Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { + Argument instArg = *this; + instArg.type = ts(type); + return instArg; +} + +/* ************************************************************************* */ +ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, *this) { + Argument instArg = arg.expandTemplate(ts); + instArgList.push_back(instArg); + } + return instArgList; +} + /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) + BOOST_FOREACH(const string& ns, type.namespaces) result += ns + delim; - if (type == "string" || type == "unsigned char" || type == "char") + if (type.name == "string" || type.name == "unsigned char" + || type.name == "char") return result + "char"; - if (type == "Vector" || type == "Matrix") + if (type.name == "Vector" || type.name == "Matrix") return result + "double"; - if (type == "int" || type == "size_t") + if (type.name == "int" || type.name == "size_t") return result + "numeric"; - if (type == "bool") + if (type.name == "bool") return result + "logical"; - return result + type; + return result + type.name; } /* ************************************************************************* */ bool Argument::isScalar() const { - return (type == "bool" || type == "char" || type == "unsigned char" - || type == "int" || type == "size_t" || type == "double"); + return (type.name == "bool" || type.name == "char" + || type.name == "unsigned char" || type.name == "int" + || type.name == "size_t" || type.name == "double"); } /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; - string cppType = qualifiedType("::"); - string matlabUniqueType = qualifiedType(); + string cppType = type.qualifiedName("::"); + string matlabUniqueType = type.qualifiedName(); if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer @@ -78,14 +97,6 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << ");" << endl; } -/* ************************************************************************* */ -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; @@ -93,7 +104,7 @@ string ArgumentList::types() const { BOOST_FOREACH(Argument arg, *this) { if (!first) str += ","; - str += arg.type; + str += arg.type.name; first = false; } return str; @@ -105,14 +116,14 @@ string ArgumentList::signature() const { bool cap = false; BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type) + BOOST_FOREACH(char ch, arg.type.name) if (isupper(ch)) { sig += ch; //If there is a capital letter, we don't want to read it below cap = true; } if (!cap) - sig += arg.type[0]; + sig += arg.type.name[0]; //Reset to default cap = false; } @@ -136,7 +147,8 @@ string ArgumentList::names() const { /* ************************************************************************* */ bool ArgumentList::allScalar() const { BOOST_FOREACH(Argument arg, *this) - if (!arg.isScalar()) return false; + if (!arg.isScalar()) + return false; return true; } @@ -158,42 +170,43 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { BOOST_FOREACH(Argument arg, *this) { if (!first) file.oss << ", "; - file.oss << arg.type << " " << arg.name; + file.oss << arg.type.name << " " << arg.name; first = false; } file.oss << ")"; } /* ************************************************************************* */ -void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, - const string& wrapperName, int id, bool staticMethod) const { - returnVal.emit_matlab(file); - file.oss << wrapperName << "(" << id; +void ArgumentList::emit_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const string& wrapperName, int id, + bool staticMethod) const { + returnVal.emit_matlab(proxyFile); + proxyFile.oss << wrapperName << "(" << id; if (!staticMethod) - file.oss << ", this"; - file.oss << ", varargin{:});\n"; + proxyFile.oss << ", this"; + proxyFile.oss << ", varargin{:});\n"; } /* ************************************************************************* */ -void ArgumentList::emit_conditional_call(FileWriter& file, +void ArgumentList::emit_conditional_call(FileWriter& proxyFile, const ReturnValue& returnVal, const string& wrapperName, int id, bool staticMethod) const { // Check nr of arguments - file.oss << "if length(varargin) == " << size(); + proxyFile.oss << "if length(varargin) == " << size(); if (size() > 0) - file.oss << " && "; - // ...and their types + proxyFile.oss << " && "; + // ...and their type.names bool first = true; for (size_t i = 0; i < size(); i++) { if (!first) - file.oss << " && "; - file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") - << "')"; + proxyFile.oss << " && "; + proxyFile.oss << "isa(varargin{" << i + 1 << "},'" + << (*this)[i].matlabClass(".") << "')"; first = false; } - file.oss << "\n"; + proxyFile.oss << "\n"; // output call to C++ wrapper - file.oss << " "; - emit_call(file, returnVal, wrapperName, id, staticMethod); + proxyFile.oss << " "; + emit_call(proxyFile, returnVal, wrapperName, id, staticMethod); } /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 6f791978a..3d8d7288f 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,36 +19,39 @@ #pragma once +#include "TemplateSubstitution.h" #include "FileWriter.h" #include "ReturnValue.h" -#include -#include - namespace wrap { /// Argument class struct Argument { + Qualified type; 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) { } + Argument expandTemplate(const TemplateSubstitution& ts) const; + /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; /// Check if will be unwrapped using scalar login in wrap/matlab.h bool isScalar() const; - /// adds namespaces to type - std::string qualifiedType(const std::string& delim = "") const; - /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; + + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { + os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") + << (arg.is_ref ? "&" : ""); + return os; + } + }; /// Argument list is just a container with Arguments @@ -66,6 +69,8 @@ struct ArgumentList: public std::vector { /// Check if all arguments scalar bool allScalar() const; + ArgumentList expandTemplate(const TemplateSubstitution& ts) const; + // MATLAB code generation: /** @@ -83,24 +88,38 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit emit MATLAB call to wrapper - * @param file output stream + * emit emit MATLAB call to proxy + * @param proxyFile output stream * @param returnVal the return value * @param wrapperName of method or function * @param staticMethod flag to emit "this" in call */ - void emit_call(FileWriter& file, const ReturnValue& returnVal, + void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, const std::string& wrapperName, int id, bool staticMethod = false) const; /** - * emit conditional MATLAB call to wrapper (checking arguments first) - * @param file output stream + * emit conditional MATLAB call to proxy (checking arguments first) + * @param proxyFile output stream * @param returnVal the return value * @param wrapperName of method or function * @param staticMethod flag to emit "this" in call */ - void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod = false) const; + void emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const std::string& wrapperName, int id, + bool staticMethod = false) const; + + friend std::ostream& operator<<(std::ostream& os, + const ArgumentList& argList) { + os << "("; + if (argList.size() > 0) + os << argList.front(); + if (argList.size() > 1) + for (size_t i = 1; i < argList.size(); i++) + os << ", " << argList[i]; + os << ")"; + return os; + } + }; } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 075c98811..0e480f0fd 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -33,7 +33,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, +void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { @@ -41,17 +41,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, createNamespaceStructure(namespaces, toolboxPath); // open destination classFile - string classFile = toolboxPath; - if (!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; + string classFile = matlabName(toolboxPath); FileWriter proxyFile(classFile, verbose_, "%"); // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = - qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + const string matlabQualName = qualifiedName("."); + const string matlabUniqueName = qualifiedName(); + const string cppName = qualifiedName("::"); + const string matlabBaseName = qualifiedParent.qualifiedName("."); + const string cppBaseName = qualifiedParent.qualifiedName("::"); // emit class proxy code // we want our class to inherit the handle class for memory purposes @@ -75,13 +73,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); wrapperFile.oss << "\n"; + // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) { + for (size_t i = 0; i < constructor.nrOverloads(); i++) { + ArgumentList args = constructor.argumentList(i); const int id = (int) functionNames.size(); constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), - id, a); + id, args); const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); + cppName, matlabUniqueName, cppBaseName, id, args); wrapperFile.oss << "\n"; functionNames.push_back(wrapFunctionName); } @@ -144,19 +144,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, proxyFile.emit(true); } -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - /* ************************************************************************* */ void Class::pointer_constructor_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const string& wrapperName, + FileWriter& wrapperFile, Str wrapperName, vector& functionNames) const { - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( - "::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + const string matlabUniqueName = qualifiedName(); + const string cppName = qualifiedName("::"); + const string baseCppName = qualifiedParent.qualifiedName("::"); const int collectorInsertId = (int) functionNames.size(); const string collectorInsertFunctionName = matlabUniqueName @@ -247,128 +242,126 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } /* ************************************************************************* */ -vector expandArgumentListsTemplate( - const vector& argLists, const string& templateArg, - const vector& instName, - const std::vector& expandedClassNamespace, - const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if (arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end() - 1); - instArg.type = instName.back(); - } else if (arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), - expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, - const string& templateArg, const vector& instName, - const std::vector& expandedClassNamespace, - const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, - templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if (retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); - instRetVal.type1 = instName.back(); - } else if (retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), - expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if (retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); - instRetVal.type2 = instName.back(); - } else if (retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), - expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, - const vector& instName, - const std::vector& expandedClassNamespace, - const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; - inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, - expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, - instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate( - cls.constructor.args_list, templateArg, instName, expandedClassNamespace, - expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; +Class Class::expandTemplate(const TemplateSubstitution& ts) const { + Class inst = *this; + inst.methods = expandMethodTemplate(methods, ts); + inst.static_methods = expandMethodTemplate(static_methods, ts); + inst.constructor = constructor.expandTemplate(ts); inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; return inst; } /* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, - const vector >& instantiations) const { +vector Class::expandTemplate(Str templateArg, + const vector& instantiations) const { vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, - this->namespaces, expandedName); - inst.name = expandedName; + BOOST_FOREACH(const Qualified& instName, instantiations) { + Qualified expandedClass = (Qualified) (*this); + expandedClass.name += instName.name; + const TemplateSubstitution ts(templateArg, instName, expandedClass); + Class inst = expandTemplate(ts); + inst.name = expandedClass.name; inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" - + wrap::qualifiedName("::", instName) + ">"; + inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + + ">"; result.push_back(inst); } return result; } /* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, - const vector& instantiation, - const std::vector& expandedClassNamespace, - const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, - expandedClassNamespace, expandedClassName); +void Class::addMethod(bool verbose, bool is_const, Str methodName, + const ArgumentList& argumentList, const ReturnValue& returnValue, + Str templateArgName, const vector& templateArgValues) { + // Check if templated + if (!templateArgName.empty() && templateArgValues.size() > 0) { + // Create method to expand + // For all values of the template argument, create a new method + BOOST_FOREACH(const Qualified& instName, templateArgValues) { + const TemplateSubstitution ts(templateArgName, instName, this->name); + // substitute template in arguments + ArgumentList expandedArgs = argumentList.expandTemplate(ts); + // do the same for return type + ReturnValue expandedRetVal = returnValue.expandTemplate(ts); + // Now stick in new overload stack with expandedMethodName key + // but note we use the same, unexpanded methodName in overload + string expandedMethodName = methodName + instName.name; + methods[expandedMethodName].addOverload(methodName, expandedArgs, + expandedRetVal, is_const, instName, verbose); + } + } else + // just add overload + methods[methodName].addOverload(methodName, argumentList, returnValue, + is_const, Qualified(), verbose); } /* ************************************************************************* */ -std::string Class::getTypedef() const { +void Class::erase_serialization() { + Methods::iterator it = methods.find("serializable"); + if (it != methods.end()) { +#ifndef WRAP_DISABLE_SERIALIZE + isSerializable = true; +#else + // cout << "Ignoring serializable() flag in class " << name << endl; +#endif + methods.erase(it); + } + + it = methods.find("serialize"); + if (it != methods.end()) { +#ifndef WRAP_DISABLE_SERIALIZE + isSerializable = true; + hasSerialization = true; +#else + // cout << "Ignoring serialize() flag in class " << name << endl; +#endif + methods.erase(it); + } +} + +/* ************************************************************************* */ +void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { + + hasSerialiable |= isSerializable; + + // verify all of the function arguments + //TODO:verifyArguments(validTypes, constructor.args_list); + verifyArguments(validTypes, static_methods); + verifyArguments(validTypes, methods); + + // verify function return types + verifyReturnTypes(validTypes, static_methods); + verifyReturnTypes(validTypes, methods); + + // verify parents + if (!qualifiedParent.empty() + && find(validTypes.begin(), validTypes.end(), + qualifiedParent.qualifiedName("::")) == validTypes.end()) + throw DependencyMissing(qualifiedParent.qualifiedName("::"), + qualifiedName("::")); +} + +/* ************************************************************************* */ +void Class::appendInheritedMethods(const Class& cls, + const vector& classes) { + + if (!cls.qualifiedParent.empty()) { + + // Find parent + BOOST_FOREACH(const Class& parent, classes) { + // We found a parent class for our parent, TODO improve ! + if (parent.name == cls.qualifiedParent.name) { + methods.insert(parent.methods.begin(), parent.methods.end()); + appendInheritedMethods(parent, classes); + } + } + } +} + +/* ************************************************************************* */ +string Class::getTypedef() const { string result; - BOOST_FOREACH(const string& namesp, namespaces) { + BOOST_FOREACH(Str namesp, namespaces) { result += ("namespace " + namesp + " { "); } result += ("typedef " + typedefName + " " + name + ";"); @@ -379,43 +372,22 @@ std::string Class::getTypedef() const { } /* ************************************************************************* */ - void Class::comment_fragment(FileWriter& proxyFile) const { proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; - if (!constructor.args_list.empty()) - proxyFile.oss << "%\n%-------Constructors-------\n"; - BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - proxyFile.oss << "%"; - argList.emit_prototype(proxyFile, name); - proxyFile.oss << "\n"; - } + constructor.comment_fragment(proxyFile); if (!methods.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - BOOST_FOREACH(ArgumentList argList, m.argLists) { - proxyFile.oss << "%"; - argList.emit_prototype(proxyFile, m.name); - proxyFile.oss << " : returns " - << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; - } - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) + name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) proxyFile.oss << "%\n%-------Static Methods-------\n"; - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - BOOST_FOREACH(ArgumentList argList, m.argLists) { - proxyFile.oss << "%"; - argList.emit_prototype(proxyFile, m.name); - proxyFile.oss << " : returns " - << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; - } - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) + name_m.second.comment_fragment(proxyFile); if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; @@ -429,23 +401,24 @@ void Class::comment_fragment(FileWriter& proxyFile) const { /* ************************************************************************* */ void Class::serialization_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const std::string& wrapperName, - std::vector& functionNames) const { + FileWriter& wrapperFile, Str wrapperName, + vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; // checkArguments("string_serialize",nargout,nargin-1,0); // Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); -// std::ostringstream out_archive_stream; +// ostringstream out_archive_stream; // boost::archive::text_oarchive out_archive(out_archive_stream); // out_archive << *obj; // out[0] = wrap< string >(out_archive_stream.str()); //} int serialize_id = functionNames.size(); - const string matlabQualName = qualifiedName("."), matlabUniqueName = - qualifiedName(), cppClassName = qualifiedName("::"); + const string matlabQualName = qualifiedName("."); + const string matlabUniqueName = qualifiedName(); + const string cppClassName = qualifiedName("::"); const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); @@ -469,7 +442,7 @@ void Class::serialization_fragments(FileWriter& proxyFile, << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate - wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; + wrapperFile.oss << " ostringstream out_archive_stream;\n"; wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; @@ -520,22 +493,23 @@ void Class::serialization_fragments(FileWriter& proxyFile, /* ************************************************************************* */ void Class::deserialization_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const std::string& wrapperName, - std::vector& functionNames) const { + FileWriter& wrapperFile, Str wrapperName, + vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; // checkArguments("Point3.string_deserialize",nargout,nargin,1); // string serialized = unwrap< string >(in[0]); - // std::istringstream in_archive_stream(serialized); + // istringstream in_archive_stream(serialized); // boost::archive::text_iarchive in_archive(in_archive_stream); // Shared output(new Point3()); // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} int deserialize_id = functionNames.size(); - const string matlabQualName = qualifiedName("."), matlabUniqueName = - qualifiedName(), cppClassName = qualifiedName("::"); + const string matlabQualName = qualifiedName("."); + const string matlabUniqueName = qualifiedName(); + const string cppClassName = qualifiedName("::"); const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); functionNames.push_back(wrapFunctionNameDeserialize); @@ -553,7 +527,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile, // string argument with deserialization boilerplate wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss << " istringstream in_archive_stream(serialized);\n"; wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; @@ -604,9 +578,21 @@ void Class::deserialization_fragments(FileWriter& proxyFile, } /* ************************************************************************* */ -std::string Class::getSerializationExport() const { +string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; } + +/* ************************************************************************* */ +void Class::python_wrapper(FileWriter& wrapperFile) const { + wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n"; + constructor.python_wrapper(wrapperFile, name); + BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values) + m.python_wrapper(wrapperFile, name); + BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values) + m.python_wrapper(wrapperFile, name); + wrapperFile.oss << ";\n\n"; +} + /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index 2ca976f66..34323b797 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,67 +19,119 @@ #pragma once -#include -#include - #include "Constructor.h" #include "Deconstructor.h" #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" +#include +#include + +#include +#include + namespace wrap { /// Class has name, constructors, methods -struct Class { +class Class: public Qualified { + typedef std::map Methods; + Methods methods; ///< Class methods + +public: + + typedef const std::string& Str; typedef std::map StaticMethods; - /// Constructor creates an empty class - Class(bool verbose=true) : isVirtual(false), isSerializable(false), hasSerialization(false), verbose_(verbose) {} - // Then the instance variables are set directly by the Module constructor - std::string name; ///< Class name - std::vector templateArgs; ///< Template arguments - std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] - bool isVirtual; ///< Whether the class is part of a virtual inheritance chain - bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports - bool hasSerialization; ///< Whether we should create the serialization functions - std::vector qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack - Methods methods; ///< Class methods - StaticMethods static_methods; ///< Static methods - std::vector namespaces; ///< Stack of namespaces - Constructor constructor; ///< Class constructors - Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object - bool verbose_; ///< verbose flag + std::vector templateArgs; ///< Template arguments + std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] + bool isVirtual; ///< Whether the class is part of a virtual inheritance chain + bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports + bool hasSerialization; ///< Whether we should create the serialization functions + Qualified qualifiedParent; ///< The *single* parent + StaticMethods static_methods; ///< Static methods + Constructor constructor; ///< Class constructors + Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object + bool verbose_; ///< verbose flag + + /// Constructor creates an empty class + Class(bool verbose = true) : + isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor( + verbose), verbose_(verbose) { + } + + size_t nrMethods() const { + return methods.size(); + } + Method& method(Str name) { + return methods.at(name); + } + bool exists(Str name) const { + return methods.find(name) != methods.end(); + } // And finally MATLAB code is emitted, methods below called by Module::matlab_code - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, std::vector& functionNames) const; ///< emit proxy class + void matlab_proxy(Str toolboxPath, Str wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + std::vector& functionNames) const; ///< emit proxy class - std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter + Class expandTemplate(const TemplateSubstitution& ts) const; - std::vector expandTemplate(const std::string& templateArg, const std::vector >& instantiations) const; + std::vector expandTemplate(Str templateArg, + const std::vector& instantiations) const; - Class expandTemplate(const std::string& templateArg, const std::vector& instantiation, const std::vector& expandedClassNamespace, const std::string& expandedClassName) const; + /// Add potentially overloaded, potentially templated method + void addMethod(bool verbose, bool is_const, Str methodName, + const ArgumentList& argumentList, const ReturnValue& returnValue, + Str templateArgName, const std::vector& templateArgValues); - // The typedef line for this class, if this class is a typedef, otherwise returns an empty string. + /// Post-process classes for serialization markers + void erase_serialization(); // non-const ! + + /// verify all of the function arguments + void verifyAll(std::vector& functionNames, + bool& hasSerialiable) const; + + void appendInheritedMethods(const Class& cls, + const std::vector& classes); + + /// The typedef line for this class, if this class is a typedef, otherwise returns an empty string. std::string getTypedef() const; - // Returns the string for an export flag + /// Returns the string for an export flag std::string getSerializationExport() const; - // Creates a member function that performs serialization + /// Creates a member function that performs serialization void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const; + Str wrapperName, std::vector& functionNames) const; - // Creates a static member function that performs deserialization + /// Creates a static member function that performs deserialization void deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const; + Str wrapperName, std::vector& functionNames) const; + + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile) const; + + friend std::ostream& operator<<(std::ostream& os, const Class& cls) { + os << "class " << cls.name << "{\n"; + os << cls.constructor << ";\n"; + BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values) + os << m << ";\n"; + BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values) + os << m << ";\n"; + os << "};" << std::endl; + return os; + } private: - void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector& functionNames) const; - void comment_fragment(FileWriter& proxyFile) const; + + void pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str wrapperName, + std::vector& functionNames) const; + + void comment_fragment(FileWriter& proxyFile) const; }; } // \namespace wrap diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index fdbbf0e42..782c0ca80 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -29,52 +29,55 @@ using namespace std; using namespace wrap; - /* ************************************************************************* */ -string Constructor::matlab_wrapper_name(const string& className) const { +string Constructor::matlab_wrapper_name(Str className) const { string str = "new_" + className; return str; } /* ************************************************************************* */ -void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const { +void Constructor::proxy_fragment(FileWriter& file, + const std::string& wrapperName, bool hasParent, const int id, + const ArgumentList args) const { size_t nrArgs = args.size(); // check for number of arguments... file.oss << " elseif nargin == " << nrArgs; - if (nrArgs>0) file.oss << " && "; + if (nrArgs > 0) + file.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_constructor_" + + boost::lexical_cast(id); - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" + << endl; file.oss << "{\n"; file.oss << " mexAtExit(&_deleteAllObjects);\n"; //Typedef boost::shared_ptr @@ -82,22 +85,29 @@ string Constructor::wrapper_fragment(FileWriter& file, file.oss << "\n"; //Check to see if there will be any arguments and remove {} for consiseness - if(al.size() > 0) + if (al.size() > 0) al.matlab_unwrap(file); // unwrap arguments - file.oss << " Shared *self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl; + file.oss << " Shared *self = new Shared(new " << cppClassName << "(" + << al.names() << "));" << endl; file.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; - if(verbose_) + if (verbose_) file.oss << " std::cout << \"constructed \" << self << std::endl;" << endl; - file.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl; - file.oss << " *reinterpret_cast (mxGetData(out[0])) = self;" << endl; + file.oss + << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" + << endl; + file.oss << " *reinterpret_cast (mxGetData(out[0])) = self;" + << endl; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if(!cppBaseClassName.empty()) { + if (!cppBaseClassName.empty()) { file.oss << "\n"; - file.oss << " typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n"; - file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - file.oss << " *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self);\n"; + file.oss << " typedef boost::shared_ptr<" << cppBaseClassName + << "> SharedBase;\n"; + file.oss + << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + file.oss + << " *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self);\n"; } file.oss << "}" << endl; @@ -106,3 +116,9 @@ string Constructor::wrapper_fragment(FileWriter& file, } /* ************************************************************************* */ +void Constructor::python_wrapper(FileWriter& wrapperFile, Str className) const { + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_ + << ");\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 5438c515c..a026bf423 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,7 +18,7 @@ #pragma once -#include "Argument.h" +#include "OverloadedFunction.h" #include #include @@ -26,42 +26,64 @@ namespace wrap { // Constructor class -struct Constructor { +struct Constructor: public OverloadedFunction { + + typedef const std::string& Str; /// Constructor creates an empty class - Constructor(bool verbose = false) : - verbose_(verbose) { + Constructor(bool verbose = false) { + verbose_ = verbose; } - // Then the instance variables are set directly by the Module constructor - std::vector args_list; - std::string name; - bool verbose_; + Constructor expandTemplate(const TemplateSubstitution& ts) const { + Constructor inst = *this; + inst.argLists_ = expandArgumentListsTemplate(ts); + inst.name_ = ts.expandedClassName(); + return inst; + } // MATLAB code generation // toolboxPath is main toolbox directory, e.g., ../matlab // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m /// wrapper name - std::string matlab_wrapper_name(const std::string& className) const; + std::string matlab_wrapper_name(Str className) const; + + void comment_fragment(FileWriter& proxyFile) const { + if (nrOverloads() > 0) + proxyFile.oss << "%\n%-------Constructors-------\n"; + for (size_t i = 0; i < nrOverloads(); i++) { + proxyFile.oss << "%"; + argumentList(i).emit_prototype(proxyFile, name_); + proxyFile.oss << "\n"; + } + } /** * Create fragment to select constructor in proxy class, e.g., * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ - void proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const; + void proxy_fragment(FileWriter& file, Str wrapperName, bool hasParent, + const int id, const ArgumentList args) const; /// cpp wrapper - std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, const std::string& matlabUniqueName, - const std::string& cppBaseClassName, int id, + std::string wrapper_fragment(FileWriter& file, Str cppClassName, + Str matlabUniqueName, Str cppBaseClassName, int id, const ArgumentList& al) const; /// constructor function - void generate_construct(FileWriter& file, const std::string& cppClassName, + void generate_construct(FileWriter& file, Str cppClassName, std::vector& args_list) const; + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile, Str className) const; + + friend std::ostream& operator<<(std::ostream& os, const Constructor& m) { + for (size_t i = 0; i < m.nrOverloads(); i++) + os << m.name_ << m.argLists_[i]; + return os; + } + }; } // \namespace wrap diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 3f5461078..783661819 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -15,14 +15,15 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -FileWriter::FileWriter(const string& filename, bool verbose, const string& comment_str) -: verbose_(verbose),filename_(filename), comment_str_(comment_str) -{ +FileWriter::FileWriter(const string& filename, bool verbose, + const string& comment_str) : + verbose_(verbose), filename_(filename), comment_str_(comment_str) { } /* ************************************************************************* */ void FileWriter::emit(bool add_header, bool force_overwrite) const { - if (verbose_) cerr << "generating " << filename_ << " "; + if (verbose_) + cerr << "generating " << filename_ << " "; // read in file if it exists string existing_contents; bool file_exists = true; @@ -35,23 +36,17 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { // Only write a file if it is new, an update, or overwrite is forced string new_contents = oss.str(); if (force_overwrite || !file_exists || existing_contents != new_contents) { - ofstream ofs(filename_.c_str(), ios::binary); // Binary to use LF line endings instead of CRLF - if (!ofs) throw CantOpenFile(filename_); + // Binary to use LF line endings instead of CRLF + ofstream ofs(filename_.c_str(), ios::binary); + if (!ofs) + throw CantOpenFile(filename_); // dump in stringstream ofs << new_contents; ofs.close(); - if (verbose_) cerr << " ...complete" << endl; - - // Add small message whenever writing a new file and not running in full verbose mode - if (!verbose_) - cout << "wrap: generating " << filename_ << endl; - } else { - if (verbose_) cerr << " ...no update" << endl; } + if (verbose_) + cerr << " ...no update" << endl; } /* ************************************************************************* */ - - - diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h new file mode 100644 index 000000000..ac22ec3a8 --- /dev/null +++ b/wrap/FullyOverloadedFunction.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 FullyOverloadedFunction.h + * @brief Function that can be fully overloaded: arguments and return values + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#pragma once + +#include "OverloadedFunction.h" + +namespace wrap { + +/** + * Signature Overload (including return value) + */ +class SignatureOverloads: public ArgumentOverloads { + +protected: + + std::vector returnVals_; + +public: + + const ReturnValue& returnValue(size_t i) const { + return returnVals_.at(i); + } + + void push_back(const ArgumentList& args, const ReturnValue& retVal) { + argLists_.push_back(args); + returnVals_.push_back(retVal); + } + + void verifyReturnTypes(const std::vector& validtypes, + const std::string& s) const { + BOOST_FOREACH(const ReturnValue& retval, returnVals_) { + retval.type1.verify(validtypes, s); + if (retval.isPair) + retval.type2.verify(validtypes, s); + } + } + + // TODO use transform ? + std::vector expandReturnValuesTemplate( + const TemplateSubstitution& ts) const { + std::vector result; + BOOST_FOREACH(const ReturnValue& retVal, returnVals_) { + ReturnValue instRetVal = retVal.expandTemplate(ts); + result.push_back(instRetVal); + } + return result; + } + + /// Expand templates, imperative ! + void expandTemplate(const TemplateSubstitution& ts) { + // substitute template in arguments + argLists_ = expandArgumentListsTemplate(ts); + // do the same for return types + returnVals_ = expandReturnValuesTemplate(ts); + } + + // emit a list of comments, one for each overload + void usage_fragment(FileWriter& proxyFile, const std::string& name) const { + unsigned int argLCount = 0; + BOOST_FOREACH(ArgumentList argList, argLists_) { + argList.emit_prototype(proxyFile, name); + if (argLCount != nrOverloads() - 1) + proxyFile.oss << ", "; + else + proxyFile.oss << " : returns " << returnValue(0).return_type(false) + << std::endl; + argLCount++; + } + } + + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile, const std::string& name) const { + size_t i = 0; + BOOST_FOREACH(ArgumentList argList, argLists_) { + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << " : returns " << returnVals_[i++].return_type(false) + << std::endl; + } + } + + friend std::ostream& operator<<(std::ostream& os, + const SignatureOverloads& overloads) { + for (size_t i = 0; i < overloads.nrOverloads(); i++) + os << overloads.returnVals_[i] << overloads.argLists_[i] << std::endl; + return os; + } + +}; + +class FullyOverloadedFunction: public Function, public SignatureOverloads { + +public: + + bool addOverload(const std::string& name, const ArgumentList& args, + const ReturnValue& retVal, const Qualified& instName = Qualified(), + bool verbose = false) { + bool first = initializeOrCheck(name, instName, verbose); + SignatureOverloads::push_back(args, retVal); + return first; + } + +}; + +// Templated checking functions +// TODO: do this via polymorphism, use transform ? + +template +inline void verifyReturnTypes(const std::vector& validTypes, + const std::map& vt) { + typedef typename std::map::value_type NamedMethod; + BOOST_FOREACH(const NamedMethod& namedMethod, vt) + namedMethod.second.verifyReturnTypes(validTypes); +} + +} // \namespace wrap + diff --git a/wrap/Function.cpp b/wrap/Function.cpp new file mode 100644 index 000000000..6faa70fb9 --- /dev/null +++ b/wrap/Function.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * 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 Function.ccp + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#include "Function.h" +#include "utilities.h" + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +bool Function::initializeOrCheck(const std::string& name, + const Qualified& instName, bool verbose) { + + if (name.empty()) + throw std::runtime_error( + "Function::initializeOrCheck called with empty name"); + + // Check if this overload is give to the correct method + if (name_.empty()) { + name_ = name; + templateArgValue_ = instName; + verbose_ = verbose; + return true; + } else { + if (name_ != name || templateArgValue_ != instName || verbose_ != verbose) + throw std::runtime_error( + "Function::initializeOrCheck called with different arguments: with name " + + name + " instead of expected " + name_ + + ", or with template argument " + instName.qualifiedName(":") + + " instead of expected " + templateArgValue_.qualifiedName(":")); + + return false; + } +} + +/* ************************************************************************* */ diff --git a/wrap/Function.h b/wrap/Function.h new file mode 100644 index 000000000..49a26bd8d --- /dev/null +++ b/wrap/Function.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 Function.h + * @brief Base class for global functions and methods + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#pragma once + +#include "Argument.h" + +namespace wrap { + +/// Function class +class Function { + +protected: + + std::string name_; ///< name of method + Qualified templateArgValue_; ///< value of template argument if applicable + bool verbose_; + +public: + + /** + * @brief first time, fill in instance variables, otherwise check if same + * @return true if first time, false thereafter + */ + bool initializeOrCheck(const std::string& name, const Qualified& instName = + Qualified(), bool verbose = false); + + std::string name() const { + return name_; + } + + std::string matlabName() const { + if (templateArgValue_.empty()) + return name_; + else + return name_ + templateArgValue_.name; + } +}; + +} // \namespace wrap + diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 25e1dcedb..e843481a1 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -16,38 +16,31 @@ namespace wrap { using namespace std; /* ************************************************************************* */ -void GlobalFunction::addOverload(bool verbose, const std::string& name, +void GlobalFunction::addOverload(const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const StrVec& ns_stack) { - this->verbose_ = verbose; - this->name = name; - this->argLists.push_back(args); - this->returnVals.push_back(retVal); - this->namespaces.push_back(ns_stack); + const Qualified& instName, bool verbose) { + string name(overload.name); + FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose); + overloads.push_back(overload); } /* ************************************************************************* */ -void GlobalFunction::matlab_proxy(const std::string& toolboxPath, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - FileWriter& file, std::vector& functionNames) const { +void GlobalFunction::matlab_proxy(const string& toolboxPath, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, vector& functionNames) const { // cluster overloads with same namespace // create new GlobalFunction structures around namespaces - same namespaces and names are overloads // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i = 0; i < namespaces.size(); ++i) { - StrVec ns = namespaces.at(i); - string str_ns = qualifiedName("", ns, ""); - ReturnValue ret = returnVals.at(i); - ArgumentList args = argLists.at(i); - - if (!grouped_functions.count(str_ns)) - grouped_functions[str_ns] = GlobalFunction(name, verbose_); - - grouped_functions[str_ns].argLists.push_back(args); - grouped_functions[str_ns].returnVals.push_back(ret); - grouped_functions[str_ns].namespaces.push_back(ns); + for (size_t i = 0; i < overloads.size(); ++i) { + Qualified overload = overloads.at(i); + // use concatenated namespaces as key + string str_ns = qualifiedName("", overload.namespaces); + const ReturnValue& ret = returnValue(i); + const ArgumentList& args = argumentList(i); + grouped_functions[str_ns].addOverload(overload, args, ret); } size_t lastcheck = grouped_functions.size(); @@ -60,37 +53,34 @@ void GlobalFunction::matlab_proxy(const std::string& toolboxPath, } /* ************************************************************************* */ -void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - FileWriter& file, std::vector& functionNames) const { +void GlobalFunction::generateSingleFunction(const string& toolboxPath, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, vector& functionNames) const { // create the folder for the namespace - const StrVec& ns = namespaces.front(); - createNamespaceStructure(ns, toolboxPath); + const Qualified& overload1 = overloads.front(); + createNamespaceStructure(overload1.namespaces, toolboxPath); // open destination mfunctionFileName - string mfunctionFileName = toolboxPath; - if (!ns.empty()) - mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); - mfunctionFileName += "/" + name + ".m"; + string mfunctionFileName = overload1.matlabName(toolboxPath); FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = - qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); + const string matlabQualName = overload1.qualifiedName("."); + const string matlabUniqueName = overload1.qualifiedName(""); + const string cppName = overload1.qualifiedName("::"); - mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; + mfunctionFile.oss << "function varargout = " << name_ << "(varargin)\n"; - for (size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; + for (size_t i = 0; i < nrOverloads(); ++i) { + const ArgumentList& args = argumentList(i); + const ReturnValue& returnVal = returnValue(i); const int id = functionNames.size(); // Output proxy matlab code - mfunctionFile.oss << " " << (overload == 0 ? "" : "else"); - argLists[overload].emit_conditional_call(mfunctionFile, - returnVals[overload], wrapperName, id, true); // true omits "this" + mfunctionFile.oss << " " << (i == 0 ? "" : "else"); + args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this" // Output C++ wrapper code @@ -114,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1 != "void") + if (returnVal.type1.name != "void") returnVal.wrap_result(cppName + "(" + args.names() + ")", file, typeAttributes); else @@ -137,6 +127,11 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, mfunctionFile.emit(true); } +/* ************************************************************************* */ +void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const { + wrapperFile.oss << "def(\"" << name_ << "\", " << name_ << ");\n"; +} + /* ************************************************************************* */ } // \namespace wrap diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 6c8ad0c86..6a49fe5ce 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -9,43 +9,35 @@ #pragma once -#include "Argument.h" -#include "ReturnValue.h" +#include "FullyOverloadedFunction.h" namespace wrap { -struct GlobalFunction { +struct GlobalFunction: public FullyOverloadedFunction { - typedef std::vector StrVec; + std::vector overloads; ///< Stack of qualified names - bool verbose_; - std::string name; + // adds an overloaded version of this function, + void addOverload(const Qualified& overload, const ArgumentList& args, + const ReturnValue& retVal, const Qualified& instName = Qualified(), + bool verbose = false); - // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces - - // Constructor only used in Module - GlobalFunction(bool verbose = true) : - verbose_(verbose) { + void verifyArguments(const std::vector& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); } - // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) : - verbose_(verbose), name(name_) { + void verifyReturnTypes(const std::vector& validtypes) const { + SignatureOverloads::verifyReturnTypes(validtypes, name_); } - // adds an overloaded version of this function - void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, - const StrVec& ns_stack); - // codegen function called from Module to build the cpp and matlab versions of the function void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& file, std::vector& functionNames) const; + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile) const; + private: // Creates a single global function - all in same namespace diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 7b88b9cdc..49d90378d 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -29,155 +29,53 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Method::addOverload(bool verbose, bool is_const, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal) { - this->verbose_ = verbose; - this->is_const_ = is_const; - this->name = name; - this->argLists.push_back(args); - this->returnVals.push_back(retVal); +bool Method::addOverload(Str name, const ArgumentList& args, + const ReturnValue& retVal, bool is_const, const Qualified& instName, + bool verbose) { + bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose); + if (first) + is_const_ = is_const; + else if (is_const && !is_const_) + throw std::runtime_error( + "Method::addOverload now designated as const whereas before it was not"); + else if (!is_const && is_const_) + throw std::runtime_error( + "Method::addOverload now designated as non-const whereas before it was"); + return first; } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, - const string& cppClassName, const std::string& matlabQualName, - const std::string& matlabUniqueName, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { - - // Create function header - file.oss << " function varargout = " << name << "(this, varargin)\n"; - - // Emit comments for documentation - string up_name = boost::to_upper_copy(name); - file.oss << " % " << up_name << " usage: "; - unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(file, name); - if (argLCount != argLists.size() - 1) - file.oss << ", "; - else - file.oss << " : returns " - << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } - - // Emit URL to Doxygen page - file.oss << " % " - << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" - << endl; - - // Document all overloads, if any - if (argLists.size() > 1) { - file.oss << " % " << "" << endl; - file.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) { - file.oss << " % "; - argList.emit_prototype(file, name); - file.oss << endl; - } - } - - // Handle special case of single overload with all numeric arguments - if (argLists.size() == 1 && argLists[0].allScalar()) { - // Output proxy matlab code - file.oss << " "; - const int id = (int) functionNames.size(); - argLists[0].emit_call(file, returnVals[0], wrapperName, id); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, 0, id, typeAttributes); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } else { - // Check arguments for all overloads - for (size_t overload = 0; overload < argLists.size(); ++overload) { - - // Output proxy matlab code - file.oss << " " << (overload == 0 ? "" : "else"); - const int id = (int) functionNames.size(); - argLists[overload].emit_conditional_call(file, returnVals[overload], - wrapperName, id); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, - cppClassName, matlabUniqueName, overload, id, typeAttributes); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } - file.oss << " else\n"; - file.oss - << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name << "');" << endl; - file.oss << " end\n"; - } - - file.oss << " end\n"; +void Method::proxy_header(FileWriter& proxyFile) const { + proxyFile.oss << " function varargout = " << matlabName() + << "(this, varargin)\n"; } /* ************************************************************************* */ -string Method::wrapper_fragment(FileWriter& file, const string& cppClassName, - const string& matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes) const { - - // generate code - - const string wrapFunctionName = matlabUniqueName + "_" + name + "_" - + boost::lexical_cast(id); - - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - - // call - file.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - // start - file.oss << "{\n"; - - if (returnVal.isPair) { - if (returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" - << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 - << ";" << endl; - if (returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" - << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 - << ";" << endl; - } else if (returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") - << "> Shared" << returnVal.type1 << ";" << endl; - - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" - << endl; - +string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args, + const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, + const Qualified& instName) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; - // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file, 1); + + // unwrap arguments, see Argument.cpp, we start at 1 as first is obj + args.matlab_unwrap(wrapperFile, 1); // call method and wrap result - // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1 != "void") - returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, - typeAttributes); - else - file.oss << " obj->" + name + "(" + args.names() + ");\n"; + // example: out[0]=wrap(obj->return_field(t)); + string expanded = "obj->" + name_; + if (!instName.empty()) + expanded += ("<" + instName.qualifiedName("::") + ">"); - // finish - file.oss << "}\n"; - - return wrapFunctionName; + return expanded; } /* ************************************************************************* */ diff --git a/wrap/Method.h b/wrap/Method.h index 9926a5179..db9e6bb9f 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,49 +18,46 @@ #pragma once -#include "Argument.h" -#include "ReturnValue.h" -#include "TypeAttributesTable.h" - -#include -#include +#include "StaticMethod.h" namespace wrap { /// Method class -struct Method { +class Method: public StaticMethod { - /// Constructor creates empty object - Method(bool verbose = true) : - verbose_(verbose), is_const_(false) {} - - // Then the instance variables are set directly by the Module constructor - bool verbose_; bool is_const_; - std::string name; - std::vector argLists; - std::vector returnVals; - // The first time this function is called, it initializes the class members - // with those in rhs, but in subsequent calls it adds additional argument - // lists as function overloads. - void addOverload(bool verbose, bool is_const, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); +public: - // MATLAB code generation - // classPath is class directory, e.g., ../matlab/@Point2 - void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + typedef const std::string& Str; + + bool addOverload(Str name, const ArgumentList& args, + const ReturnValue& retVal, bool is_const, const Qualified& instName = + Qualified(), bool verbose = false); + + virtual bool isStatic() const { + return false; + } + + virtual bool isConst() const { + return is_const_; + } + + friend std::ostream& operator<<(std::ostream& os, const Method& m) { + for (size_t i = 0; i < m.nrOverloads(); i++) + os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; + return os; + } private: - std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + + // Emit method header + void proxy_header(FileWriter& proxyFile) const; + + virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args, + const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, + const Qualified& instName) const; }; } // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 2cb5ea7ed..ac0e0a579 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -62,17 +62,23 @@ typedef rule Rule; /* ************************************************************************* */ /* ************************************************************************* */ -void handle_possible_template(vector& classes, const Class& cls, const string& templateArgument, const vector >& instantiations) { - if(instantiations.empty()) { - classes.push_back(cls); - } else { - vector classInstantiations = cls.expandTemplate(templateArgument, instantiations); - BOOST_FOREACH(const Class& c, classInstantiations) { - classes.push_back(c); - } - } -} - +// If a number of template arguments were given, generate a number of expanded +// class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes +static void handle_possible_template(vector& classes, const Class& cls, + const vector& instantiations) { + if (cls.templateArgs.empty() || instantiations.empty()) { + classes.push_back(cls); + } else { + if (cls.templateArgs.size() != 1) + throw std::runtime_error( + "In-line template instantiations only handle a single template argument"); + vector classInstantiations = // + cls.expandTemplate(cls.templateArgs.front(), instantiations); + BOOST_FOREACH(const Class& c, classInstantiations) + classes.push_back(c); + } +} + /* ************************************************************************* */ Module::Module(const std::string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) @@ -94,28 +100,9 @@ Module::Module(const string& interfacePath, /* ************************************************************************* */ void Module::parseMarkup(const std::string& data) { - // these variables will be imperatively updated to gradually build [cls] + // The parse imperatively :-( updates variables gradually during parse // The one with postfix 0 are used to reset the variables after parse. - string methodName, methodName0; - bool isConst, isConst0 = false; - ReturnValue retVal0, retVal; - Argument arg0, arg; - ArgumentList args0, args; - vector arg_dup; ///keep track of duplicates - Constructor constructor0(verbose), constructor(verbose); - Deconstructor deconstructor0(verbose), deconstructor(verbose); - StaticMethod static_method0(verbose), static_method(verbose); - Class cls0(verbose),cls(verbose); - GlobalFunction globalFunc0(verbose), globalFunc(verbose); - ForwardDeclaration fwDec0, fwDec; - vector namespaces, /// current namespace tag - namespaces_return; /// namespace for current return type - string templateArgument; - vector templateInstantiationNamespace; - vector > templateInstantiations; - TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; - string include_path = ""; - const string null_str = ""; + vector namespaces; // current namespace tag //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -144,51 +131,63 @@ void Module::parseMarkup(const std::string& data) { 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("::"); + Argument arg0, arg; + Rule namespace_arg_p = namespace_name_p + [push_back_a(arg.type.namespaces)] >> str_p("::"); Rule argEigenType_p = - eigenType_p[assign_a(arg.type)]; + eigenType_p[assign_a(arg.type.name)]; Rule eigenRef_p = !str_p("const") [assign_a(arg.is_const,true)] >> - eigenType_p [assign_a(arg.type)] >> + eigenType_p [assign_a(arg.type.name)] >> ch_p('&') [assign_a(arg.is_ref,true)]; Rule classArg_p = !str_p("const") [assign_a(arg.is_const,true)] >> *namespace_arg_p >> - className_p[assign_a(arg.type)] >> + className_p[assign_a(arg.type.name)] >> !ch_p('&')[assign_a(arg.is_ref,true)]; Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; - + + // TODO, do we really need cls here? Non-local + Class cls0(verbose),cls(verbose); Rule classParent_p = - *(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >> - className_p[push_back_a(cls.qualifiedParent)]; + *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >> + className_p[assign_a(cls.qualifiedParent.name)]; - Rule templateInstantiation_p = - (*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> - className_p[push_back_a(templateInstantiationNamespace)]) - [push_back_a(templateInstantiations, templateInstantiationNamespace)] - [clear_a(templateInstantiationNamespace)]; + // parse "gtsam::Pose2" and add to templateArgValues + Qualified templateArgValue; + vector templateArgValues; + Rule templateArgValue_p = + (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> + className_p[assign_a(templateArgValue.name)]) + [push_back_a(templateArgValues, templateArgValue)] + [clear_a(templateArgValue)]; - Rule templateInstantiations_p = + // template + string templateArgName; + Rule templateArgValues_p = (str_p("template") >> - '<' >> name_p[assign_a(templateArgument)] >> '=' >> '{' >> - !(templateInstantiation_p >> *(',' >> templateInstantiation_p)) >> - '}' >> '>') - [push_back_a(cls.templateArgs, templateArgument)]; + '<' >> name_p[assign_a(templateArgName)] >> '=' >> + '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >> + '>'); + // parse "gtsam::Pose2" and add to singleInstantiation.typeList + TemplateInstantiationTypedef singleInstantiation; Rule templateSingleInstantiationArg_p = - (*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> - className_p[push_back_a(templateInstantiationNamespace)]) - [push_back_a(singleInstantiation.typeList, templateInstantiationNamespace)] - [clear_a(templateInstantiationNamespace)]; + (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> + className_p[assign_a(templateArgValue.name)]) + [push_back_a(singleInstantiation.typeList, templateArgValue)] + [clear_a(templateArgValue)]; + // typedef gtsam::RangeFactor RangeFactorPosePoint2; + TemplateInstantiationTypedef singleInstantiation0; Rule templateSingleInstantiation_p = (str_p("typedef") >> - *(namespace_name_p[push_back_a(singleInstantiation.classNamespaces)] >> str_p("::")) >> - className_p[assign_a(singleInstantiation.className)] >> + *(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >> + className_p[assign_a(singleInstantiation.class_.name)] >> '<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >> '>' >> className_p[assign_a(singleInstantiation.name)] >> @@ -197,129 +196,128 @@ void Module::parseMarkup(const std::string& data) { [push_back_a(templateInstantiationTypedefs, singleInstantiation)] [assign_a(singleInstantiation, singleInstantiation0)]; + // template Rule templateList_p = (str_p("template") >> '<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >> '>'); // NOTE: allows for pointers to all types + ArgumentList args; Rule argument_p = - ((basisType_p[assign_a(arg.type)] | argEigenType_p | eigenRef_p | classArg_p) + ((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p) >> !ch_p('*')[assign_a(arg.is_ptr,true)] >> name_p[assign_a(arg.name)]) [push_back_a(args, arg)] [assign_a(arg,arg0)]; Rule argumentList_p = !argument_p >> * (',' >> argument_p); - + + // parse class constructor + Constructor constructor0(verbose), constructor(verbose); Rule constructor_p = (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) - [push_back_a(constructor.args_list, args)] - [assign_a(args,args0)]; - //[assign_a(constructor.args,args)] - //[assign_a(constructor.name,cls.name)] - //[push_back_a(cls.constructors, constructor)] - //[assign_a(constructor,constructor0)]; + [bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))] + [clear_a(args)]; + vector namespaces_return; /// namespace for current return type Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish - static const ReturnValue::return_category RETURN_EIGEN = ReturnValue::EIGEN; - static const ReturnValue::return_category RETURN_BASIS = ReturnValue::BASIS; - static const ReturnValue::return_category RETURN_CLASS = ReturnValue::CLASS; - static const ReturnValue::return_category RETURN_VOID = ReturnValue::VOID; + static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN; + static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS; + static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS; + static const ReturnType::return_category RETURN_VOID = ReturnType::VOID; - Rule returnType1_p = - (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_CLASS)]) >> - !ch_p('*')[assign_a(retVal.isPtr1,true)]) | - (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_EIGEN)]); + ReturnType retType0, retType; + Rule returnType_p = + (basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) | + ((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >> + !ch_p('*')[assign_a(retType.isPtr,true)]) | + (eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]); - Rule returnType2_p = - (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_CLASS)]) >> - !ch_p('*') [assign_a(retVal.isPtr2,true)]) | - (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_EIGEN)]); + ReturnValue retVal0, retVal; + Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)]; + Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)]; Rule pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') [assign_a(retVal.isPair,true)]; - Rule void_p = str_p("void")[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_VOID)]; + Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)]; - Rule returnType_p = void_p | pair_p | returnType1_p; + Rule returnValue_p = void_p | pair_p | returnType1_p; Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + // gtsam::Values retract(const gtsam::VectorValues& delta) const; + string methodName; + bool isConst, isConst0 = false; Rule method_p = - (returnType_p >> methodName_p[assign_a(methodName)] >> + !templateArgValues_p >> + (returnValue_p >> methodName_p[assign_a(methodName)] >> '(' >> argumentList_p >> ')' >> !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) - [bl::bind(&Method::addOverload, - bl::var(cls.methods)[bl::var(methodName)], - verbose, - bl::var(isConst), - bl::var(methodName), - bl::var(args), - bl::var(retVal))] - [assign_a(isConst,isConst0)] - [assign_a(methodName,methodName0)] - [assign_a(args,args0)] - [assign_a(retVal,retVal0)]; + [bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst), + bl::var(methodName), bl::var(args), bl::var(retVal), + bl::var(templateArgName), bl::var(templateArgValues))] + [assign_a(retVal,retVal0)] + [clear_a(args)] + [clear_a(templateArgValues)] + [assign_a(isConst,isConst0)]; Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; Rule static_method_p = - (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(methodName)] >> + (str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >> '(' >> argumentList_p >> ')' >> ';' >> *comments_p) [bl::bind(&StaticMethod::addOverload, bl::var(cls.static_methods)[bl::var(methodName)], - verbose, - bl::var(methodName), - bl::var(args), - bl::var(retVal))] - [assign_a(methodName,methodName0)] - [assign_a(args,args0)] - [assign_a(retVal,retVal0)]; + bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)] + [assign_a(retVal,retVal0)] + [clear_a(args)]; Rule functions_p = constructor_p | method_p | static_method_p; + // parse a full class + vector templateInstantiations; Rule class_p = - (str_p("")[assign_a(cls,cls0)]) - >> (!(templateInstantiations_p | templateList_p) + eps_p[assign_a(cls,cls0)] + >> (!(templateArgValues_p + [push_back_a(cls.templateArgs, templateArgName)] + [assign_a(templateInstantiations,templateArgValues)] + [clear_a(templateArgValues)] + | templateList_p) >> !(str_p("virtual")[assign_a(cls.isVirtual, true)]) >> str_p("class") >> className_p[assign_a(cls.name)] >> ((':' >> classParent_p >> '{') | '{') >> *(functions_p | comments_p) >> str_p("};")) - [assign_a(constructor.name, cls.name)] + [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), + bl::var(cls.name), Qualified(), verbose)] [assign_a(cls.constructor, constructor)] [assign_a(cls.namespaces, namespaces)] - [assign_a(deconstructor.name,cls.name)] - [assign_a(cls.deconstructor, deconstructor)] - [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), bl::var(templateArgument), bl::var(templateInstantiations))] - [assign_a(deconstructor,deconstructor0)] + [assign_a(cls.deconstructor.name,cls.name)] + [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), + bl::var(templateInstantiations))] + [clear_a(templateInstantiations)] [assign_a(constructor, constructor0)] - [assign_a(cls,cls0)] - [clear_a(templateArgument)] - [clear_a(templateInstantiations)]; + [assign_a(cls,cls0)]; + // parse a global function + Qualified globalFunction; Rule global_function_p = - (returnType_p >> staticMethodName_p[assign_a(methodName)] >> + (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >> '(' >> argumentList_p >> ')' >> ';' >> *comments_p) + [assign_a(globalFunction.namespaces,namespaces)] [bl::bind(&GlobalFunction::addOverload, - bl::var(global_functions)[bl::var(methodName)], - verbose, - bl::var(methodName), - bl::var(args), - bl::var(retVal), - bl::var(namespaces))] - [assign_a(methodName,methodName0)] - [assign_a(args,args0)] - [assign_a(retVal,retVal0)]; + bl::var(global_functions)[bl::var(globalFunction.name)], + bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)] + [assign_a(retVal,retVal0)] + [clear_a(globalFunction)] + [clear_a(args)]; Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); @@ -327,19 +325,21 @@ void Module::parseMarkup(const std::string& data) { #pragma clang diagnostic push #pragma clang diagnostic ignored "-Wuninitialized" #endif - - Rule namespace_def_p = - (str_p("namespace") - >> namespace_name_p[push_back_a(namespaces)] - >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) - >> ch_p('}')) - [pop_a(namespaces)]; + + Rule namespace_def_p = + (str_p("namespace") + >> namespace_name_p[push_back_a(namespaces)] + >> ch_p('{') + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) + >> ch_p('}')) + [pop_a(namespaces)]; #ifdef __clang__ #pragma clang diagnostic pop #endif + // parse forward declaration + ForwardDeclaration fwDec0, fwDec; Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") @@ -367,7 +367,7 @@ void Module::parseMarkup(const std::string& data) { BOOST_SPIRIT_DEBUG_NODE(returnType2_p); BOOST_SPIRIT_DEBUG_NODE(pair_p); BOOST_SPIRIT_DEBUG_NODE(void_p); - BOOST_SPIRIT_DEBUG_NODE(returnType_p); + BOOST_SPIRIT_DEBUG_NODE(returnValue_p); BOOST_SPIRIT_DEBUG_NODE(methodName_p); BOOST_SPIRIT_DEBUG_NODE(method_p); BOOST_SPIRIT_DEBUG_NODE(class_p); @@ -388,125 +388,41 @@ void Module::parseMarkup(const std::string& data) { } // Post-process classes for serialization markers - BOOST_FOREACH(Class& cls, classes) { - Class::Methods::iterator serializable_it = cls.methods.find("serializable"); - if (serializable_it != cls.methods.end()) { -#ifndef WRAP_DISABLE_SERIALIZE - cls.isSerializable = true; -#else - cout << "Ignoring serializable() flag in class " << cls.name << endl; -#endif - cls.methods.erase(serializable_it); - } - - Class::Methods::iterator serialize_it = cls.methods.find("serialize"); - if (serialize_it != cls.methods.end()) { -#ifndef WRAP_DISABLE_SERIALIZE - cls.isSerializable = true; - cls.hasSerialization= true; -#else - cout << "Ignoring serialize() flag in class " << cls.name << endl; -#endif - cls.methods.erase(serialize_it); - } - } + BOOST_FOREACH(Class& cls, classes) + cls.erase_serialization(); // Explicitly add methods to the classes from parents so it shows in documentation BOOST_FOREACH(Class& cls, classes) - { - map inhereted = appendInheretedMethods(cls, classes); - cls.methods.insert(inhereted.begin(), inhereted.end()); - } -} - -/* ************************************************************************* */ -template -void verifyArguments(const vector& validArgs, const map& vt) { - typedef typename map::value_type Name_Method; - BOOST_FOREACH(const Name_Method& name_method, vt) { - const T& t = name_method.second; - BOOST_FOREACH(const ArgumentList& argList, t.argLists) { - BOOST_FOREACH(Argument arg, argList) { - string fullType = arg.qualifiedType("::"); - if(find(validArgs.begin(), validArgs.end(), fullType) - == validArgs.end()) - throw DependencyMissing(fullType, t.name); - } - } - } -} - -/* ************************************************************************* */ -template -void verifyReturnTypes(const vector& validtypes, const map& vt) { - typedef typename map::value_type Name_Method; - BOOST_FOREACH(const Name_Method& name_method, vt) { - const T& t = name_method.second; - BOOST_FOREACH(const ReturnValue& retval, t.returnVals) { - if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end()) - throw DependencyMissing(retval.qualifiedType1("::"), t.name); - if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end()) - throw DependencyMissing(retval.qualifiedType2("::"), t.name); - } - } -} - -/* ************************************************************************* */ -void Module::generateIncludes(FileWriter& file) const { - - // collect includes - vector all_includes(includes); - - // sort and remove duplicates - sort(all_includes.begin(), all_includes.end()); - vector::const_iterator last_include = unique(all_includes.begin(), all_includes.end()); - vector::const_iterator it = all_includes.begin(); - // add includes to file - for (; it != last_include; ++it) - file.oss << "#include <" << *it << ">" << endl; - file.oss << "\n"; -} - - -/* ************************************************************************* */ -void Module::matlab_code(const string& toolboxPath, const string& headerPath) const { - - fs::create_directories(toolboxPath); + cls.appendInheritedMethods(cls, classes); // Expand templates - This is done first so that template instantiations are // counted in the list of valid types, have their attributes and dependencies // checked, etc. - vector expandedClasses = ExpandTypedefInstantiations(classes, templateInstantiationTypedefs); + expandedClasses = ExpandTypedefInstantiations(classes, + templateInstantiationTypedefs); // Dependency check list - vector validTypes = GenerateValidTypes(expandedClasses, forward_declarations); + vector validTypes = GenerateValidTypes(expandedClasses, + forward_declarations); // Check that all classes have been defined somewhere verifyArguments(validTypes, global_functions); verifyReturnTypes(validTypes, global_functions); - bool hasSerialiable = false; - BOOST_FOREACH(const Class& cls, expandedClasses) { - hasSerialiable |= cls.isSerializable; - // verify all of the function arguments - //TODO:verifyArguments(validTypes, cls.constructor.args_list); - verifyArguments(validTypes, cls.static_methods); - verifyArguments(validTypes, cls.methods); - - // verify function return types - verifyReturnTypes(validTypes, cls.static_methods); - verifyReturnTypes(validTypes, cls.methods); - - // verify parents - if(!cls.qualifiedParent.empty() && std::find(validTypes.begin(), validTypes.end(), wrap::qualifiedName("::", cls.qualifiedParent)) == validTypes.end()) - throw DependencyMissing(wrap::qualifiedName("::", cls.qualifiedParent), cls.qualifiedName("::")); - } + hasSerialiable = false; + BOOST_FOREACH(const Class& cls, expandedClasses) + cls.verifyAll(validTypes,hasSerialiable); // Create type attributes table and check validity - TypeAttributesTable typeAttributes; typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); typeAttributes.checkValidity(expandedClasses); +} + +/* ************************************************************************* */ +void Module::matlab_code(const string& toolboxPath) const { + + fs::create_directories(toolboxPath); // create the unified .cpp switch file const string wrapperName = name + "_wrapper"; @@ -527,19 +443,18 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co // Generate includes while avoiding redundant includes generateIncludes(wrapperFile); - // create typedef classes - we put this at the top of the wrap file so that collectors and method arguments can use these typedefs - BOOST_FOREACH(const Class& cls, expandedClasses) { + // create typedef classes - we put this at the top of the wrap file so that + // collectors and method arguments can use these typedefs + BOOST_FOREACH(const Class& cls, expandedClasses) if(!cls.typedefName.empty()) wrapperFile.oss << cls.getTypedef() << "\n"; - } wrapperFile.oss << "\n"; // Generate boost.serialization export flags (needs typedefs from above) if (hasSerialiable) { - BOOST_FOREACH(const Class& cls, expandedClasses) { + BOOST_FOREACH(const Class& cls, expandedClasses) if(cls.isSerializable) wrapperFile.oss << cls.getSerializationExport() << "\n"; - } wrapperFile.oss << "\n"; } @@ -552,14 +467,12 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co vector functionNames; // Function names stored by index for switch // create proxy class and wrapper code - BOOST_FOREACH(const Class& cls, expandedClasses) { + BOOST_FOREACH(const Class& cls, expandedClasses) cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames); - } // create matlab files and wrapper code for global functions - BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) { + BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames); - } // finish wrapper file wrapperFile.oss << "\n"; @@ -567,29 +480,25 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co wrapperFile.emit(true); } -/* ************************************************************************* */ -map Module::appendInheretedMethods(const Class& cls, const vector& classes) -{ - map methods; - if(!cls.qualifiedParent.empty()) - { - //Find Class - BOOST_FOREACH(const Class& parent, classes) { - //We found the class for our parent - if(parent.name == cls.qualifiedParent.back()) - { - Methods inhereted = appendInheretedMethods(parent, classes); - methods.insert(inhereted.begin(), inhereted.end()); - } - } - } else { - methods.insert(cls.methods.begin(), cls.methods.end()); - } - return methods; -} - /* ************************************************************************* */ +void Module::generateIncludes(FileWriter& file) const { + + // collect includes + vector all_includes(includes); + + // sort and remove duplicates + sort(all_includes.begin(), all_includes.end()); + vector::const_iterator last_include = unique(all_includes.begin(), all_includes.end()); + vector::const_iterator it = all_includes.begin(); + // add includes to file + for (; it != last_include; ++it) + file.oss << "#include <" << *it << ">" << endl; + file.oss << "\n"; +} + + +/* ************************************************************************* */ void Module::finish_wrapper(FileWriter& file, const std::vector& functionNames) const { file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; file.oss << "{\n"; @@ -741,3 +650,31 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul } /* ************************************************************************* */ +void Module::python_wrapper(const string& toolboxPath) const { + + fs::create_directories(toolboxPath); + + // create the unified .cpp switch file + const string wrapperName = name + "_python"; + string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp"; + FileWriter wrapperFile(wrapperFileName, verbose, "//"); + wrapperFile.oss << "#include \n\n"; + wrapperFile.oss << "using namespace boost::python;\n"; + wrapperFile.oss << "BOOST_PYTHON_MODULE(" + name + ")\n"; + wrapperFile.oss << "{\n"; + + // write out classes + BOOST_FOREACH(const Class& cls, expandedClasses) + cls.python_wrapper(wrapperFile); + + // write out global functions + BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) + p.second.python_wrapper(wrapperFile); + + // finish wrapper file + wrapperFile.oss << "}\n"; + + wrapperFile.emit(true); +} + +/* ************************************************************************* */ diff --git a/wrap/Module.h b/wrap/Module.h index 93f699e14..a4659dc3a 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -37,43 +37,53 @@ struct Module { typedef std::map GlobalFunctions; typedef std::map Methods; - std::string name; ///< module name - bool verbose; ///< verbose flag + // Filled during parsing: + std::string name; ///< module name + bool verbose; ///< verbose flag std::vector classes; ///< list of classes std::vector templateInstantiationTypedefs; ///< list of template instantiations std::vector forward_declarations; - std::vector includes; ///< Include statements + std::vector includes; ///< Include statements GlobalFunctions global_functions; + // After parsing: + std::vector expandedClasses; + bool hasSerialiable; + TypeAttributesTable typeAttributes; + /// constructor that parses interface file - Module(const std::string& interfacePath, - const std::string& moduleName, - bool enable_verbose=true); + Module(const std::string& interfacePath, const std::string& moduleName, + bool enable_verbose = true); /// Dummy constructor that does no parsing - use only for testing - Module(const std::string& moduleName, bool enable_verbose=true); - - //Recursive method to append all methods inhereted from parent classes - std::map appendInheretedMethods(const Class& cls, const std::vector& classes); - - /// MATLAB code generation: - void matlab_code( - const std::string& path, - const std::string& headerPath) const; // FIXME: headerPath not actually used? - - void finish_wrapper(FileWriter& file, const std::vector& functionNames) const; - - void generateIncludes(FileWriter& file) const; + Module(const std::string& moduleName, bool enable_verbose = true); /// non-const function that performs parsing - typically called by constructor /// Throws exception on failure void parseMarkup(const std::string& data); + /// MATLAB code generation: + void matlab_code(const std::string& path) const; + + void generateIncludes(FileWriter& file) const; + + void finish_wrapper(FileWriter& file, + const std::vector& functionNames) const; + + /// Python code generation: + void python_wrapper(const std::string& path) const; + private: - static std::vector ExpandTypedefInstantiations(const std::vector& classes, const std::vector instantiations); - static std::vector GenerateValidTypes(const std::vector& classes, const std::vector forwardDeclarations); - static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes); - static void WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes); + static std::vector ExpandTypedefInstantiations( + const std::vector& classes, + const std::vector instantiations); + static std::vector GenerateValidTypes( + const std::vector& classes, + const std::vector forwardDeclarations); + static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, + const std::string& moduleName, const std::vector& classes); + static void WriteRTTIRegistry(FileWriter& wrapperFile, + const std::string& moduleName, const std::vector& classes); }; } // \namespace wrap diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h new file mode 100644 index 000000000..47c418748 --- /dev/null +++ b/wrap/OverloadedFunction.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 OverloadedFunction.h + * @brief Function that can overload its arguments only + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#pragma once + +#include "Function.h" +#include "Argument.h" + +namespace wrap { + +/** + * ArgumentList Overloads + */ +class ArgumentOverloads { + +protected: + + std::vector argLists_; + +public: + + size_t nrOverloads() const { + return argLists_.size(); + } + + const ArgumentList& argumentList(size_t i) const { + return argLists_.at(i); + } + + void push_back(const ArgumentList& args) { + argLists_.push_back(args); + } + + std::vector expandArgumentListsTemplate( + const TemplateSubstitution& ts) const { + std::vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists_) { + ArgumentList instArgList = argList.expandTemplate(ts); + result.push_back(instArgList); + } + return result; + } + + /// Expand templates, imperative ! + virtual void ExpandTemplate(const TemplateSubstitution& ts) { + argLists_ = expandArgumentListsTemplate(ts); + } + + void verifyArguments(const std::vector& validArgs, + const std::string s) const { + BOOST_FOREACH(const ArgumentList& argList, argLists_) { + BOOST_FOREACH(Argument arg, argList) { + std::string fullType = arg.type.qualifiedName("::"); + if (find(validArgs.begin(), validArgs.end(), fullType) + == validArgs.end()) + throw DependencyMissing(fullType, "checking argument of " + s); + } + } + } + + friend std::ostream& operator<<(std::ostream& os, + const ArgumentOverloads& overloads) { + BOOST_FOREACH(const ArgumentList& argList, overloads.argLists_) + os << argList << std::endl; + return os; + } + +}; + +class OverloadedFunction: public Function, public ArgumentOverloads { + +public: + + bool addOverload(const std::string& name, const ArgumentList& args, + const Qualified& instName = Qualified(), bool verbose = false) { + bool first = initializeOrCheck(name, instName, verbose); + ArgumentOverloads::push_back(args); + return first; + } + +private: + +}; + +// Templated checking functions +// TODO: do this via polymorphism, use transform ? + +template +static std::map expandMethodTemplate( + const std::map& methods, const TemplateSubstitution& ts) { + std::map result; + typedef std::pair NamedMethod; + BOOST_FOREACH(NamedMethod namedMethod, methods) { + F instMethod = namedMethod.second; + instMethod.expandTemplate(ts); + namedMethod.second = instMethod; + result.insert(namedMethod); + } + return result; +} + +template +inline void verifyArguments(const std::vector& validArgs, + const std::map& vt) { + typedef typename std::map::value_type NamedMethod; + BOOST_FOREACH(const NamedMethod& namedMethod, vt) + namedMethod.second.verifyArguments(validArgs); +} + +} // \namespace wrap + diff --git a/wrap/Qualified.h b/wrap/Qualified.h new file mode 100644 index 000000000..b23e9020d --- /dev/null +++ b/wrap/Qualified.h @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * 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 Qualified.h + * @brief Qualified name + * @author Frank Dellaert + * @date Nov 11, 2014 + **/ + +#pragma once + +#include +#include + +namespace wrap { + +/** + * Class to encapuslate a qualified name, i.e., with (nested) namespaces + */ +struct Qualified { + + std::vector namespaces; ///< Stack of namespaces + std::string name; ///< type name + + Qualified(const std::string& name_ = "") : + name(name_) { + } + + bool empty() const { + return namespaces.empty() && name.empty(); + } + + void clear() { + namespaces.clear(); + name.clear(); + } + + bool operator!=(const Qualified& other) const { + return other.name != name || other.namespaces != namespaces; + } + + /// Return a qualified string using given delimiter + std::string qualifiedName(const std::string& delimiter = "") const { + std::string result; + for (std::size_t i = 0; i < namespaces.size(); ++i) + result += (namespaces[i] + delimiter); + result += name; + return result; + } + + /// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m" + std::string matlabName(const std::string& toolboxPath) const { + std::string result = toolboxPath; + for (std::size_t i = 0; i < namespaces.size(); ++i) + result += ("/+" + namespaces[i]); + result += "/" + name + ".m"; + return result; + } + + friend std::ostream& operator<<(std::ostream& os, const Qualified& q) { + os << q.qualifiedName("::"); + return os; + } + +}; + +} // \namespace wrap + diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp new file mode 100644 index 000000000..a317a5420 --- /dev/null +++ b/wrap/ReturnType.cpp @@ -0,0 +1,61 @@ +/** + * @file ReturnType.cpp + * @date Nov 13, 2014 + * @author Frank Dellaert + */ + +#include "ReturnType.h" +#include "utilities.h" +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +string ReturnType::str(bool add_ptr) const { + return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name); +} + +/* ************************************************************************* */ +void ReturnType::wrap_result(const string& out, const string& result, + FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const { + + string cppType = qualifiedName("::"), matlabType = qualifiedName("."); + + if (category == CLASS) { + string objCopy, ptrType; + ptrType = "Shared" + name; + const bool isVirtual = typeAttributes.at(cppType).isVirtual; + if (isVirtual) { + if (isPtr) + objCopy = result; + else + objCopy = result + ".clone()"; + } else { + if (isPtr) + objCopy = result; + else + objCopy = ptrType + "(new " + cppType + "(" + result + "))"; + } + wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" + << matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n"; + } else if (isPtr) { + wrapperFile.oss << " Shared" << name << "* ret = new Shared" << name << "(" + << result << ");" << endl; + wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType + << "\");\n"; + } else if (matlabType != "void") + wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result + << ");\n"; +} + +/* ************************************************************************* */ +void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { + if (category == CLASS) + wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") + << "> Shared" << name << ";" << endl; +} + +/* ************************************************************************* */ + diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h new file mode 100644 index 000000000..1829fbc81 --- /dev/null +++ b/wrap/ReturnType.h @@ -0,0 +1,67 @@ +/** + * @file ReturnValue.h + * @brief Encapsulates a return type of a method + * @date Nov 13, 2014 + * @author Frank Dellaert + */ + +#include "Qualified.h" +#include "FileWriter.h" +#include "TypeAttributesTable.h" +#include "utilities.h" + +#pragma once + +namespace wrap { + +/** + * Encapsulates return value of a method or function + */ +struct ReturnType: Qualified { + + /// the different supported return value categories + typedef enum { + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 + } return_category; + + bool isPtr; + return_category category; + + ReturnType() : + isPtr(false), category(CLASS) { + } + + ReturnType(const std::string& name) : + isPtr(false), category(CLASS) { + Qualified::name = name; + } + + void rename(const Qualified& q) { + name = q.name; + namespaces = q.namespaces; + } + + /// Check if this type is in a set of valid types + template + void verify(TYPES validtypes, const std::string& s) const { + std::string key = qualifiedName("::"); + if (find(validtypes.begin(), validtypes.end(), key) == validtypes.end()) + throw DependencyMissing(key, "checking return type of " + s); + } + +private: + + friend struct ReturnValue; + + std::string str(bool add_ptr) const; + + /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false); + void wrap_result(const std::string& out, const std::string& result, + FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; + + /// Creates typedef + void wrapTypeUnwrap(FileWriter& wrapperFile) const; + +}; + +} // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 78e36d4da..54e585cad 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -1,153 +1,72 @@ /** * @file ReturnValue.cpp - * * @date Dec 1, 2011 * @author Alex Cunningham * @author Andrew Melim * @author Richard Roberts */ -#include - #include "ReturnValue.h" #include "utilities.h" +#include +#include 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("::"), type1) + ", " + - maybe_shared_ptr(add_ptr || isPtr2, qualifiedType2("::"), type2) + " >"; - return str; - } else - return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1); +ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { + ReturnValue instRetVal = *this; + instRetVal.type1 = ts(type1); + if (isPair) + instRetVal.type2 = ts(type2); + return instRetVal; +} + +/* ************************************************************************* */ +string ReturnValue::return_type(bool add_ptr) const { + if (isPair) + return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >"; + else + return type1.str(add_ptr); } /* ************************************************************************* */ string ReturnValue::matlab_returnType() const { - return isPair? "[first,second]" : "result"; + 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; -} - -/* ************************************************************************* */ -//TODO:Fix this -void ReturnValue::wrap_result(const string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const { - string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1("."); - string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2("."); - +void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, + const TypeAttributesTable& typeAttributes) const { if (isPair) { // For a pair, store the returned pair so we do not evaluate the function twice - file.oss << " " << return_type(false, pair) << " pairResult = " << result << ";\n"; - - // first return value in pair - if (category1 == ReturnValue::CLASS) { // if we are going to make one - string objCopy, ptrType; - ptrType = "Shared" + type1; - const bool isVirtual = typeAttributes.at(cppType1).isVirtual; - if(isVirtual) { - if(isPtr1) - objCopy = "pairResult.first"; - else - objCopy = "pairResult.first.clone()"; - } else { - if(isPtr1) - objCopy = "pairResult.first"; - else - objCopy = ptrType + "(new " + cppType1 + "(pairResult.first))"; - } - file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n"; - } else if(isPtr1) { - file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(pairResult.first);" << endl; - file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\", false);\n"; - } else // if basis type - file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(pairResult.first);\n"; - - // second return value in pair - if (category2 == ReturnValue::CLASS) { // if we are going to make one - string objCopy, ptrType; - ptrType = "Shared" + type2; - const bool isVirtual = typeAttributes.at(cppType2).isVirtual; - if(isVirtual) { - if(isPtr2) - objCopy = "pairResult.second"; - else - objCopy = "pairResult.second.clone()"; - } else { - if(isPtr2) - objCopy = "pairResult.second"; - else - objCopy = ptrType + "(new " + cppType2 + "(pairResult.second))"; - } - file.oss << " out[1] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType2 << "\", " << (isVirtual ? "true" : "false") << ");\n"; - } else if(isPtr2) { - file.oss << " Shared" << type2 <<"* ret = new Shared" << type2 << "(pairResult.second);" << endl; - file.oss << " out[1] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n"; - } else - file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(pairResult.second);\n"; + wrapperFile.oss << " " << return_type(true) << " pairResult = " << result + << ";\n"; + type1.wrap_result(" out[0]", "pairResult.first", wrapperFile, + typeAttributes); + type2.wrap_result(" out[1]", "pairResult.second", wrapperFile, + typeAttributes); } else { // Not a pair - - if (category1 == ReturnValue::CLASS) { - string objCopy, ptrType; - ptrType = "Shared" + type1; - const bool isVirtual = typeAttributes.at(cppType1).isVirtual; - if(isVirtual) { - if(isPtr1) - objCopy = result; - else - objCopy = result + ".clone()"; - } else { - if(isPtr1) - objCopy = result; - else - objCopy = ptrType + "(new " + cppType1 + "(" + result + "))"; - } - file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n"; - } else if(isPtr1) { - file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ");" << endl; - file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n"; - } else if (matlabType1!="void") - file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(" << result << ");\n"; + type1.wrap_result(" out[0]", result, wrapperFile, typeAttributes); } } /* ************************************************************************* */ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { - if(isPair) - { - if(category1 == ReturnValue::CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType1("::") << "> Shared" << type1 << ";"<< endl; - if(category2 == ReturnValue::CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType2("::") << "> Shared" << type2 << ";"<< endl; - } - else { - if (category1 == ReturnValue::CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType1("::") << "> Shared" << type1 << ";"<< endl; - } + type1.wrapTypeUnwrap(wrapperFile); + if (isPair) + type2.wrapTypeUnwrap(wrapperFile); } + /* ************************************************************************* */ -void ReturnValue::emit_matlab(FileWriter& file) const { +void ReturnValue::emit_matlab(FileWriter& proxyFile) const { string output; if (isPair) - file.oss << "[ varargout{1} varargout{2} ] = "; - else if (category1 != ReturnValue::VOID) - file.oss << "varargout{1} = "; + proxyFile.oss << "[ varargout{1} varargout{2} ] = "; + else if (type1.category != ReturnType::VOID) + proxyFile.oss << "varargout{1} = "; } + /* ************************************************************************* */ - diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 989f81109..abfcec2b0 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -2,59 +2,61 @@ * @file ReturnValue.h * * @brief Encapsulates a return value from a method - * * @date Dec 1, 2011 * @author Alex Cunningham * @author Richard Roberts */ +#include "ReturnType.h" +#include "TemplateSubstitution.h" #include "FileWriter.h" #include "TypeAttributesTable.h" - -#include +#include "utilities.h" #pragma once namespace wrap { /** - * Encapsulates return value of a method or function + * Encapsulates return type of a method or function, possibly a pair */ struct ReturnValue { - /// the different supported return value categories - typedef enum { - CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 - } return_category; - - bool isPtr1, isPtr2, isPair; - return_category category1, category2; - std::string type1, type2; - std::vector namespaces1, namespaces2; + bool isPair; + ReturnType type1, type2; /// Constructor ReturnValue() : - isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( - CLASS) { + isPair(false) { } - typedef enum { - arg1, arg2, pair - } pairing; + /// Constructor + ReturnValue(const ReturnType& type) : + isPair(false), type1(type) { + } - std::string return_type(bool add_ptr, pairing p) const; + /// Substitute template argument + ReturnValue expandTemplate(const TemplateSubstitution& ts) const; - std::string qualifiedType1(const std::string& delim = "") const; - std::string qualifiedType2(const std::string& delim = "") const; + std::string return_type(bool add_ptr) const; std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, + void wrap_result(const std::string& result, FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; - void emit_matlab(FileWriter& file) const; + void emit_matlab(FileWriter& proxyFile) const; + + friend std::ostream& operator<<(std::ostream& os, const ReturnValue& r) { + if (!r.isPair && r.type1.category == ReturnType::VOID) + os << "void"; + else + os << r.return_type(true); + return os; + } + }; } // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 0c4cc7d75..5f91a15b4 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,7 +16,7 @@ * @author Richard Roberts **/ -#include "StaticMethod.h" +#include "Method.h" #include "utilities.h" #include @@ -30,117 +30,145 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void StaticMethod::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal) { - this->verbose = verbose; - this->name = name; - this->argLists.push_back(args); - this->returnVals.push_back(retVal); +void StaticMethod::proxy_header(FileWriter& proxyFile) const { + string upperName = matlabName(); + upperName[0] = toupper(upperName[0], locale()); + proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& file, - FileWriter& wrapperFile, const string& cppClassName, - const std::string& matlabQualName, const std::string& matlabUniqueName, - const string& wrapperName, const TypeAttributesTable& typeAttributes, +void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, + Str matlabUniqueName, Str wrapperName, + const TypeAttributesTable& typeAttributes, vector& functionNames) const { - string upperName = name; - upperName[0] = std::toupper(upperName[0], std::locale()); + // emit header, e.g., function varargout = templatedMethod(this, varargin) + proxy_header(proxyFile); - file.oss << " function varargout = " << upperName << "(varargin)\n"; - //Comments for documentation - string up_name = boost::to_upper_copy(name); - file.oss << " % " << up_name << " usage: "; - unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(file, name); - if (argLCount != argLists.size() - 1) - file.oss << ", "; - else - file.oss << " : returns " - << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } + // Emit comments for documentation + string up_name = boost::to_upper_copy(matlabName()); + proxyFile.oss << " % " << up_name << " usage: "; + usage_fragment(proxyFile, matlabName()); - file.oss << " % " + // Emit URL to Doxygen page + proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - file.oss << " % " << "" << endl; - file.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) { - file.oss << " % "; - argList.emit_prototype(file, up_name); - file.oss << endl; - } - - // Check arguments for all overloads - for (size_t overload = 0; overload < argLists.size(); ++overload) { + // Handle special case of single overload with all numeric arguments + if (nrOverloads() == 1 && argumentList(0).allScalar()) { // Output proxy matlab code - file.oss << " " << (overload == 0 ? "" : "else"); + // TODO: document why is it OK to not check arguments in this case + proxyFile.oss << " "; const int id = (int) functionNames.size(); - argLists[overload].emit_conditional_call(file, returnVals[overload], - wrapperName, id, true); // last bool is to indicate static ! + argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id, + isStatic()); // Output C++ wrapper code const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, (int) overload, id, typeAttributes); + matlabUniqueName, 0, id, typeAttributes, templateArgValue_); // Add to function list functionNames.push_back(wrapFunctionName); - } - file.oss << " else\n"; - file.oss << " error('Arguments do not match any overload of function " - << matlabQualName << "." << upperName << "');" << endl; - file.oss << " end\n"; + } else { + // Check arguments for all overloads + for (size_t i = 0; i < nrOverloads(); ++i) { - file.oss << " end\n"; + // Output proxy matlab code + proxyFile.oss << " " << (i == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argumentList(i).emit_conditional_call(proxyFile, returnValue(i), + wrapperName, id, isStatic()); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, i, id, typeAttributes, + templateArgValue_); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name_ << "');" << endl; + proxyFile.oss << " end\n"; + } + + proxyFile.oss << " end\n"; } /* ************************************************************************* */ -string StaticMethod::wrapper_fragment(FileWriter& file, - const string& cppClassName, const string& matlabUniqueName, int overload, - int id, const TypeAttributesTable& typeAttributes) const { +string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes, + const Qualified& instName) const { // generate code - const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" + boost::lexical_cast(id); - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; + const ArgumentList& args = argumentList(overload); + const ReturnValue& returnVal = returnValue(overload); // call - file.oss << "void " << wrapFunctionName + wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - file.oss << "{\n"; + wrapperFile.oss << "{\n"; - returnVal.wrapTypeUnwrap(file); + returnVal.wrapTypeUnwrap(wrapperFile); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" - << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name - << "\",nargout,nargin," << args.size() << ");\n"; + // get call + // for static methods: cppClassName::staticMethod + // for instance methods: obj->instanceMethod + string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, + args, returnVal, typeAttributes, instName); - // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file, 0); // We start at 0 because there is no self object - - // call method with default type and wrap result - if (returnVal.type1 != "void") - returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", - file, typeAttributes); + expanded += ("(" + args.names() + ")"); + if (returnVal.type1.name != "void") + returnVal.wrap_result(expanded, wrapperFile, typeAttributes); else - file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; + wrapperFile.oss << " " + expanded + ";\n"; // finish - file.oss << "}\n"; + wrapperFile.oss << "}\n"; return wrapFunctionName; } /* ************************************************************************* */ +string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args, + const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, + const Qualified& instName) const { + // check arguments + // NOTE: for static functions, there is no object passed + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_ + << "\",nargout,nargin," << args.size() << ");\n"; + + // unwrap arguments, see Argument.cpp + args.matlab_unwrap(wrapperFile, 0); // We start at 0 because there is no self object + + // call method and wrap result + // example: out[0]=wrap(staticMethod(t)); + string expanded = cppClassName + "::" + name_; + if (!instName.empty()) + expanded += ("<" + instName.qualifiedName("::") + ">"); + + return expanded; +} + +/* ************************************************************************* */ +void StaticMethod::python_wrapper(FileWriter& wrapperFile, + Str className) const { + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_ + << ");\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index e1855f4c2..de8e4a94e 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,44 +19,61 @@ #pragma once -#include "Argument.h" -#include "ReturnValue.h" -#include "TypeAttributesTable.h" +#include "FullyOverloadedFunction.h" namespace wrap { /// StaticMethod class -struct StaticMethod { +struct StaticMethod: public FullyOverloadedFunction { - /// Constructor creates empty object - StaticMethod(bool verbosity = true) : - verbose(verbosity) { + typedef const std::string& Str; + + virtual bool isStatic() const { + return true; } - // Then the instance variables are set directly by the Module constructor - bool verbose; - std::string name; - std::vector argLists; - std::vector returnVals; + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile) const { + SignatureOverloads::comment_fragment(proxyFile, matlabName()); + } - // The first time this function is called, it initializes the class members - // with those in rhs, but in subsequent calls it adds additional argument - // lists as function overloads. - void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + void verifyArguments(const std::vector& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); + } + + void verifyReturnTypes(const std::vector& validtypes) const { + SignatureOverloads::verifyReturnTypes(validtypes, name_); + } // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, - const std::string& matlabUniqueName, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, + Str cppClassName, Str matlabQualName, Str matlabUniqueName, + Str wrapperName, const TypeAttributesTable& typeAttributes, std::vector& functionNames) const; -private: - std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, const std::string& matlabUniqueName, - int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile, Str className) const; + + friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) { + for (size_t i = 0; i < m.nrOverloads(); i++) + os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; + return os; + } + +protected: + + virtual void proxy_header(FileWriter& proxyFile) const; + + std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes, const Qualified& instName = + Qualified()) const; ///< cpp wrapper + + virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args, + const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, + const Qualified& instName) const; }; } // \namespace wrap diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index 85615cef4..2007acdf1 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -19,43 +19,50 @@ #include "TemplateInstantiationTypedef.h" #include "utilities.h" +#include +#include +#include using namespace std; namespace wrap { - Class TemplateInstantiationTypedef::findAndExpand(const vector& classes) const { - // Find matching class - std::vector::const_iterator clsIt = classes.end(); - for(std::vector::const_iterator it = classes.begin(); it != classes.end(); ++it) { - if(it->name == className && it->namespaces == classNamespaces && it->templateArgs.size() == typeList.size()) { - clsIt = it; - break; - } +Class TemplateInstantiationTypedef::findAndExpand( + const vector& classes) const { + // Find matching class + boost::optional matchedClass; + BOOST_FOREACH(const Class& cls, classes) { + if (cls.name == class_.name && cls.namespaces == class_.namespaces + && cls.templateArgs.size() == typeList.size()) { + matchedClass.reset(cls); + break; } - - if(clsIt == classes.end()) - throw DependencyMissing(wrap::qualifiedName("::", classNamespaces, className), - "instantiation into typedef name " + wrap::qualifiedName("::", namespaces, name) + - ". Ensure that the typedef provides the correct number of template arguments."); - - // Instantiate it - Class classInst = *clsIt; - for(size_t i = 0; i < typeList.size(); ++i) - classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], namespaces, name); - - // Fix class properties - classInst.name = name; - classInst.templateArgs.clear(); - classInst.typedefName = clsIt->qualifiedName("::") + "<"; - if(typeList.size() > 0) - classInst.typedefName += wrap::qualifiedName("::", typeList[0]); - for(size_t i = 1; i < typeList.size(); ++i) - classInst.typedefName += (", " + wrap::qualifiedName("::", typeList[i])); - classInst.typedefName += ">"; - classInst.namespaces = namespaces; - - return classInst; } -} \ No newline at end of file + if (!matchedClass) + throw DependencyMissing(class_.qualifiedName("::"), + "instantiation into typedef name " + qualifiedName("::") + + ". Ensure that the typedef provides the correct number of template arguments."); + + // Instantiate it + Class classInst = *matchedClass; + for (size_t i = 0; i < typeList.size(); ++i) { + TemplateSubstitution ts(classInst.templateArgs[i], typeList[i], *this); + classInst = classInst.expandTemplate(ts); + } + + // Fix class properties + classInst.name = name; + classInst.templateArgs.clear(); + classInst.typedefName = matchedClass->qualifiedName("::") + "<"; + if (typeList.size() > 0) + classInst.typedefName += typeList[0].qualifiedName("::"); + for (size_t i = 1; i < typeList.size(); ++i) + classInst.typedefName += (", " + typeList[i].qualifiedName("::")); + classInst.typedefName += ">"; + classInst.namespaces = namespaces; + + return classInst; +} + +} diff --git a/wrap/TemplateInstantiationTypedef.h b/wrap/TemplateInstantiationTypedef.h index 2292f16ee..2a08d7a94 100644 --- a/wrap/TemplateInstantiationTypedef.h +++ b/wrap/TemplateInstantiationTypedef.h @@ -26,14 +26,11 @@ namespace wrap { -struct TemplateInstantiationTypedef { - std::vector classNamespaces; - std::string className; - std::vector namespaces; - std::string name; - std::vector > typeList; +struct TemplateInstantiationTypedef : public Qualified { + Qualified class_; + std::vector typeList; Class findAndExpand(const std::vector& classes) const; }; -} \ No newline at end of file +} diff --git a/wrap/TemplateSubstitution.h b/wrap/TemplateSubstitution.h new file mode 100644 index 000000000..b20a1af6f --- /dev/null +++ b/wrap/TemplateSubstitution.h @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * 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 TemplateSubstitution.h + * @brief Auxiliary class for template substitutions + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#pragma once + +#include "ReturnType.h" +#include +#include + +namespace wrap { + +/** + * e.g. TemplateSubstitution("T", gtsam::Point2, gtsam::PriorFactorPoint2) + */ +class TemplateSubstitution { + + std::string templateArg_; + Qualified qualifiedType_, expandedClass_; + +public: + + TemplateSubstitution(const std::string& a, const Qualified& t, + const Qualified& e) : + templateArg_(a), qualifiedType_(t), expandedClass_(e) { + } + + std::string expandedClassName() const { + return expandedClass_.name; + } + + // Substitute if needed + Qualified operator()(const Qualified& type) const { + if (type.name == templateArg_ && type.namespaces.empty()) + return qualifiedType_; + else if (type.name == "This") + return expandedClass_; + else + return type; + } + + // Substitute if needed + ReturnType operator()(const ReturnType& type) const { + ReturnType instType = type; + if (type.name == templateArg_ && type.namespaces.empty()) + instType.rename(qualifiedType_); + else if (type.name == "This") + instType.rename(expandedClass_); + return instType; + } + + friend std::ostream& operator<<(std::ostream& os, + const TemplateSubstitution& ts) { + os << ts.templateArg_ << '/' << ts.qualifiedType_.qualifiedName("::") + << " (" << ts.expandedClass_.qualifiedName("::") << ")"; + return os; + } + +}; + +} // \namespace wrap + diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp index 449b88aab..16055fca1 100644 --- a/wrap/TypeAttributesTable.cpp +++ b/wrap/TypeAttributesTable.cpp @@ -46,13 +46,17 @@ namespace wrap { void TypeAttributesTable::checkValidity(const vector& classes) const { BOOST_FOREACH(const Class& cls, classes) { // Check that class is virtual if it has a parent - if(!cls.qualifiedParent.empty() && !cls.isVirtual) - throw AttributeError(cls.qualifiedName("::"), "Has a base class so needs to be declared virtual, change to 'virtual class "+cls.name+" ...'"); + if (!cls.qualifiedParent.empty() && !cls.isVirtual) + throw AttributeError(cls.qualifiedName("::"), + "Has a base class so needs to be declared virtual, change to 'virtual class " + + cls.name + " ...'"); // Check that parent is virtual as well - if(!cls.qualifiedParent.empty() && !at(wrap::qualifiedName("::", cls.qualifiedParent)).isVirtual) - throw AttributeError(wrap::qualifiedName("::", cls.qualifiedParent), - "Is the base class of " + cls.qualifiedName("::") + ", so needs to be declared virtual"); + Qualified parent = cls.qualifiedParent; + if (!parent.empty() && !at(parent.qualifiedName("::")).isVirtual) + throw AttributeError(parent.qualifiedName("::"), + "Is the base class of " + cls.qualifiedName("::") + + ", so needs to be declared virtual"); } } -} \ No newline at end of file +} diff --git a/wrap/TypeAttributesTable.h b/wrap/TypeAttributesTable.h index 1ff6c121c..d2a346bfc 100644 --- a/wrap/TypeAttributesTable.h +++ b/wrap/TypeAttributesTable.h @@ -28,7 +28,7 @@ namespace wrap { // Forward declarations - struct Class; + class Class; /** Attributes about valid classes, both for classes defined in this module and * also those forward-declared from others. At the moment this only contains @@ -52,4 +52,4 @@ public: void checkValidity(const std::vector& classes) const; }; -} \ No newline at end of file +} diff --git a/wrap/tests/expected-python/geometry_python.cpp b/wrap/tests/expected-python/geometry_python.cpp new file mode 100644 index 000000000..f500f2984 --- /dev/null +++ b/wrap/tests/expected-python/geometry_python.cpp @@ -0,0 +1,83 @@ +#include + +using namespace boost::python; +BOOST_PYTHON_MODULE(geometry) +{ +class_("Point2") + .def("Point2", &Point2::Point2); + .def("argChar", &Point2::argChar); + .def("argUChar", &Point2::argUChar); + .def("dim", &Point2::dim); + .def("returnChar", &Point2::returnChar); + .def("vectorConfusion", &Point2::vectorConfusion); + .def("x", &Point2::x); + .def("y", &Point2::y); +; + +class_("Point3") + .def("Point3", &Point3::Point3); + .def("StaticFunctionRet", &Point3::StaticFunctionRet); + .def("staticFunction", &Point3::staticFunction); + .def("norm", &Point3::norm); +; + +class_("Test") + .def("Test", &Test::Test); + .def("arg_EigenConstRef", &Test::arg_EigenConstRef); + .def("create_MixedPtrs", &Test::create_MixedPtrs); + .def("create_ptrs", &Test::create_ptrs); + .def("print", &Test::print); + .def("return_Point2Ptr", &Test::return_Point2Ptr); + .def("return_Test", &Test::return_Test); + .def("return_TestPtr", &Test::return_TestPtr); + .def("return_bool", &Test::return_bool); + .def("return_double", &Test::return_double); + .def("return_field", &Test::return_field); + .def("return_int", &Test::return_int); + .def("return_matrix1", &Test::return_matrix1); + .def("return_matrix2", &Test::return_matrix2); + .def("return_pair", &Test::return_pair); + .def("return_ptrs", &Test::return_ptrs); + .def("return_size_t", &Test::return_size_t); + .def("return_string", &Test::return_string); + .def("return_vector1", &Test::return_vector1); + .def("return_vector2", &Test::return_vector2); +; + +class_("MyBase") + .def("MyBase", &MyBase::MyBase); +; + +class_("MyTemplatePoint2") + .def("MyTemplatePoint2", &MyTemplatePoint2::MyTemplatePoint2); + .def("accept_T", &MyTemplatePoint2::accept_T); + .def("accept_Tptr", &MyTemplatePoint2::accept_Tptr); + .def("create_MixedPtrs", &MyTemplatePoint2::create_MixedPtrs); + .def("create_ptrs", &MyTemplatePoint2::create_ptrs); + .def("return_T", &MyTemplatePoint2::return_T); + .def("return_Tptr", &MyTemplatePoint2::return_Tptr); + .def("return_ptrs", &MyTemplatePoint2::return_ptrs); + .def("templatedMethod", &MyTemplatePoint2::templatedMethod); + .def("templatedMethod", &MyTemplatePoint2::templatedMethod); +; + +class_("MyTemplatePoint3") + .def("MyTemplatePoint3", &MyTemplatePoint3::MyTemplatePoint3); + .def("accept_T", &MyTemplatePoint3::accept_T); + .def("accept_Tptr", &MyTemplatePoint3::accept_Tptr); + .def("create_MixedPtrs", &MyTemplatePoint3::create_MixedPtrs); + .def("create_ptrs", &MyTemplatePoint3::create_ptrs); + .def("return_T", &MyTemplatePoint3::return_T); + .def("return_Tptr", &MyTemplatePoint3::return_Tptr); + .def("return_ptrs", &MyTemplatePoint3::return_ptrs); + .def("templatedMethod", &MyTemplatePoint3::templatedMethod); + .def("templatedMethod", &MyTemplatePoint3::templatedMethod); +; + +class_("MyFactorPosePoint2") + .def("MyFactorPosePoint2", &MyFactorPosePoint2::MyFactorPosePoint2); +; + +def("aGlobalFunction", aGlobalFunction); +def("overloadedGlobalFunction", overloadedGlobalFunction); +} diff --git a/wrap/tests/expected2/+gtsam/Point2.m b/wrap/tests/expected2/+gtsam/Point2.m new file mode 100644 index 000000000..308b35d9a --- /dev/null +++ b/wrap/tests/expected2/+gtsam/Point2.m @@ -0,0 +1,90 @@ +%class Point2, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%Point2() +%Point2(double x, double y) +% +%-------Methods------- +%argChar(char a) : returns void +%argUChar(unsigned char a) : returns void +%dim() : returns int +%returnChar() : returns char +%vectorConfusion() : returns VectorNotEigen +%x() : returns double +%y() : returns double +% +classdef Point2 < handle + properties + ptr_gtsamPoint2 = 0 + end + methods + function obj = Point2(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(0, my_ptr); + elseif nargin == 0 + my_ptr = geometry_wrapper(1); + elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') + my_ptr = geometry_wrapper(2, varargin{1}, varargin{2}); + else + error('Arguments do not match any overload of gtsam.Point2 constructor'); + end + obj.ptr_gtsamPoint2 = my_ptr; + end + + function delete(obj) + geometry_wrapper(3, obj.ptr_gtsamPoint2); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + function varargout = argChar(this, varargin) + % ARGCHAR usage: argChar(char a) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + geometry_wrapper(4, this, varargin{:}); + end + + function varargout = argUChar(this, varargin) + % ARGUCHAR usage: argUChar(unsigned char a) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + geometry_wrapper(5, this, varargin{:}); + end + + function varargout = dim(this, varargin) + % DIM usage: dim() : returns int + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(6, this, varargin{:}); + end + + function varargout = returnChar(this, varargin) + % RETURNCHAR usage: returnChar() : returns char + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(7, this, varargin{:}); + end + + function varargout = vectorConfusion(this, varargin) + % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(8, this, varargin{:}); + end + + function varargout = x(this, varargin) + % X usage: x() : returns double + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(9, this, varargin{:}); + end + + function varargout = y(this, varargin) + % Y usage: y() : returns double + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(10, this, varargin{:}); + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/+gtsam/Point3.m b/wrap/tests/expected2/+gtsam/Point3.m new file mode 100644 index 000000000..3ef336ff1 --- /dev/null +++ b/wrap/tests/expected2/+gtsam/Point3.m @@ -0,0 +1,61 @@ +%class Point3, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%Point3(double x, double y, double z) +% +%-------Methods------- +%norm() : returns double +% +%-------Static Methods------- +%StaticFunctionRet(double z) : returns gtsam::Point3 +%staticFunction() : returns double +% +classdef Point3 < handle + properties + ptr_gtsamPoint3 = 0 + end + methods + function obj = Point3(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(11, my_ptr); + elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double') + my_ptr = geometry_wrapper(12, varargin{1}, varargin{2}, varargin{3}); + else + error('Arguments do not match any overload of gtsam.Point3 constructor'); + end + obj.ptr_gtsamPoint3 = my_ptr; + end + + function delete(obj) + geometry_wrapper(13, obj.ptr_gtsamPoint3); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + function varargout = norm(this, varargin) + % NORM usage: norm() : returns double + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(14, this, varargin{:}); + end + + end + + methods(Static = true) + function varargout = StaticFunctionRet(varargin) + % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3 + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(15, varargin{:}); + end + + function varargout = StaticFunction(varargin) + % STATICFUNCTION usage: staticFunction() : returns double + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(16, varargin{:}); + end + + end +end diff --git a/wrap/tests/expected2/MyBase.m b/wrap/tests/expected2/MyBase.m new file mode 100644 index 000000000..d7bfe7e81 --- /dev/null +++ b/wrap/tests/expected2/MyBase.m @@ -0,0 +1,35 @@ +%class MyBase, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +classdef MyBase < handle + properties + ptr_MyBase = 0 + end + methods + function obj = MyBase(varargin) + if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + if nargin == 2 + my_ptr = varargin{2}; + else + my_ptr = geometry_wrapper(41, varargin{2}); + end + geometry_wrapper(40, my_ptr); + else + error('Arguments do not match any overload of MyBase constructor'); + end + obj.ptr_MyBase = my_ptr; + end + + function delete(obj) + geometry_wrapper(42, obj.ptr_MyBase); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/MyFactorPosePoint2.m b/wrap/tests/expected2/MyFactorPosePoint2.m new file mode 100644 index 000000000..166e1514d --- /dev/null +++ b/wrap/tests/expected2/MyFactorPosePoint2.m @@ -0,0 +1,36 @@ +%class MyFactorPosePoint2, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%MyFactorPosePoint2(size_t key1, size_t key2, double measured, Base noiseModel) +% +classdef MyFactorPosePoint2 < handle + properties + ptr_MyFactorPosePoint2 = 0 + end + methods + function obj = MyFactorPosePoint2(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(69, my_ptr); + elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') + my_ptr = geometry_wrapper(70, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + else + error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); + end + obj.ptr_MyFactorPosePoint2 = my_ptr; + end + + function delete(obj) + geometry_wrapper(71, obj.ptr_MyFactorPosePoint2); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/MyTemplatePoint2.m b/wrap/tests/expected2/MyTemplatePoint2.m new file mode 100644 index 000000000..5f1c69480 --- /dev/null +++ b/wrap/tests/expected2/MyTemplatePoint2.m @@ -0,0 +1,134 @@ +%class MyTemplatePoint2, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%MyTemplatePoint2() +% +%-------Methods------- +%accept_T(Point2 value) : returns void +%accept_Tptr(Point2 value) : returns void +%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 > +%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 > +%return_T(Point2 value) : returns gtsam::Point2 +%return_Tptr(Point2 value) : returns gtsam::Point2 +%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 > +%templatedMethodPoint2(Point2 t) : returns void +%templatedMethodPoint3(Point3 t) : returns void +% +classdef MyTemplatePoint2 < MyBase + properties + ptr_MyTemplatePoint2 = 0 + end + methods + function obj = MyTemplatePoint2(varargin) + if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + if nargin == 2 + my_ptr = varargin{2}; + else + my_ptr = geometry_wrapper(44, varargin{2}); + end + base_ptr = geometry_wrapper(43, my_ptr); + elseif nargin == 0 + [ my_ptr, base_ptr ] = geometry_wrapper(45); + else + error('Arguments do not match any overload of MyTemplatePoint2 constructor'); + end + obj = obj@MyBase(uint64(5139824614673773682), base_ptr); + obj.ptr_MyTemplatePoint2 = my_ptr; + end + + function delete(obj) + geometry_wrapper(46, obj.ptr_MyTemplatePoint2); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + function varargout = accept_T(this, varargin) + % ACCEPT_T usage: accept_T(Point2 value) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2') + geometry_wrapper(47, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.accept_T'); + end + end + + function varargout = accept_Tptr(this, varargin) + % ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2') + geometry_wrapper(48, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr'); + end + end + + function varargout = create_MixedPtrs(this, varargin) + % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + [ varargout{1} varargout{2} ] = geometry_wrapper(49, this, varargin{:}); + end + + function varargout = create_ptrs(this, varargin) + % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + [ varargout{1} varargout{2} ] = geometry_wrapper(50, this, varargin{:}); + end + + function varargout = return_T(this, varargin) + % RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2 + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2') + varargout{1} = geometry_wrapper(51, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.return_T'); + end + end + + function varargout = return_Tptr(this, varargin) + % RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2 + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2') + varargout{1} = geometry_wrapper(52, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr'); + end + end + + function varargout = return_ptrs(this, varargin) + % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2') + [ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs'); + end + end + + function varargout = templatedMethodPoint2(this, varargin) + % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2') + geometry_wrapper(54, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod'); + end + end + + function varargout = templatedMethodPoint3(this, varargin) + % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3') + geometry_wrapper(55, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod'); + end + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/MyTemplatePoint3.m b/wrap/tests/expected2/MyTemplatePoint3.m new file mode 100644 index 000000000..848e224fd --- /dev/null +++ b/wrap/tests/expected2/MyTemplatePoint3.m @@ -0,0 +1,134 @@ +%class MyTemplatePoint3, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%MyTemplatePoint3() +% +%-------Methods------- +%accept_T(Point3 value) : returns void +%accept_Tptr(Point3 value) : returns void +%create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 > +%create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 > +%return_T(Point3 value) : returns gtsam::Point3 +%return_Tptr(Point3 value) : returns gtsam::Point3 +%return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 > +%templatedMethodPoint2(Point2 t) : returns void +%templatedMethodPoint3(Point3 t) : returns void +% +classdef MyTemplatePoint3 < MyBase + properties + ptr_MyTemplatePoint3 = 0 + end + methods + function obj = MyTemplatePoint3(varargin) + if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + if nargin == 2 + my_ptr = varargin{2}; + else + my_ptr = geometry_wrapper(57, varargin{2}); + end + base_ptr = geometry_wrapper(56, my_ptr); + elseif nargin == 0 + [ my_ptr, base_ptr ] = geometry_wrapper(58); + else + error('Arguments do not match any overload of MyTemplatePoint3 constructor'); + end + obj = obj@MyBase(uint64(5139824614673773682), base_ptr); + obj.ptr_MyTemplatePoint3 = my_ptr; + end + + function delete(obj) + geometry_wrapper(59, obj.ptr_MyTemplatePoint3); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + function varargout = accept_T(this, varargin) + % ACCEPT_T usage: accept_T(Point3 value) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3') + geometry_wrapper(60, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.accept_T'); + end + end + + function varargout = accept_Tptr(this, varargin) + % ACCEPT_TPTR usage: accept_Tptr(Point3 value) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3') + geometry_wrapper(61, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.accept_Tptr'); + end + end + + function varargout = create_MixedPtrs(this, varargin) + % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + [ varargout{1} varargout{2} ] = geometry_wrapper(62, this, varargin{:}); + end + + function varargout = create_ptrs(this, varargin) + % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + [ varargout{1} varargout{2} ] = geometry_wrapper(63, this, varargin{:}); + end + + function varargout = return_T(this, varargin) + % RETURN_T usage: return_T(Point3 value) : returns gtsam::Point3 + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3') + varargout{1} = geometry_wrapper(64, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.return_T'); + end + end + + function varargout = return_Tptr(this, varargin) + % RETURN_TPTR usage: return_Tptr(Point3 value) : returns gtsam::Point3 + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3') + varargout{1} = geometry_wrapper(65, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.return_Tptr'); + end + end + + function varargout = return_ptrs(this, varargin) + % RETURN_PTRS usage: return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 2 && isa(varargin{1},'gtsam.Point3') && isa(varargin{2},'gtsam.Point3') + [ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.return_ptrs'); + end + end + + function varargout = templatedMethodPoint2(this, varargin) + % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2') + geometry_wrapper(67, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod'); + end + end + + function varargout = templatedMethodPoint3(this, varargin) + % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3') + geometry_wrapper(68, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod'); + end + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/Test.m b/wrap/tests/expected2/Test.m new file mode 100644 index 000000000..ada5a868d --- /dev/null +++ b/wrap/tests/expected2/Test.m @@ -0,0 +1,218 @@ +%class Test, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%Test() +%Test(double a, Matrix b) +% +%-------Methods------- +%arg_EigenConstRef(Matrix value) : returns void +%create_MixedPtrs() : returns pair< Test, Test > +%create_ptrs() : returns pair< Test, Test > +%print() : returns void +%return_Point2Ptr(bool value) : returns gtsam::Point2 +%return_Test(Test value) : returns Test +%return_TestPtr(Test value) : returns Test +%return_bool(bool value) : returns bool +%return_double(double value) : returns double +%return_field(Test t) : returns bool +%return_int(int value) : returns int +%return_matrix1(Matrix value) : returns Matrix +%return_matrix2(Matrix value) : returns Matrix +%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > +%return_ptrs(Test p1, Test p2) : returns pair< Test, Test > +%return_size_t(size_t value) : returns size_t +%return_string(string value) : returns string +%return_vector1(Vector value) : returns Vector +%return_vector2(Vector value) : returns Vector +% +classdef Test < handle + properties + ptr_Test = 0 + end + methods + function obj = Test(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(17, my_ptr); + elseif nargin == 0 + my_ptr = geometry_wrapper(18); + elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') + my_ptr = geometry_wrapper(19, varargin{1}, varargin{2}); + else + error('Arguments do not match any overload of Test constructor'); + end + obj.ptr_Test = my_ptr; + end + + function delete(obj) + geometry_wrapper(20, obj.ptr_Test); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + function varargout = arg_EigenConstRef(this, varargin) + % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'double') + geometry_wrapper(21, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.arg_EigenConstRef'); + end + end + + function varargout = create_MixedPtrs(this, varargin) + % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + [ varargout{1} varargout{2} ] = geometry_wrapper(22, this, varargin{:}); + end + + function varargout = create_ptrs(this, varargin) + % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + [ varargout{1} varargout{2} ] = geometry_wrapper(23, this, varargin{:}); + end + + function varargout = print(this, varargin) + % PRINT usage: print() : returns void + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + geometry_wrapper(24, this, varargin{:}); + end + + function varargout = return_Point2Ptr(this, varargin) + % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns gtsam::Point2 + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(25, this, varargin{:}); + end + + function varargout = return_Test(this, varargin) + % RETURN_TEST usage: return_Test(Test value) : returns Test + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'Test') + varargout{1} = geometry_wrapper(26, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_Test'); + end + end + + function varargout = return_TestPtr(this, varargin) + % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'Test') + varargout{1} = geometry_wrapper(27, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_TestPtr'); + end + end + + function varargout = return_bool(this, varargin) + % RETURN_BOOL usage: return_bool(bool value) : returns bool + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(28, this, varargin{:}); + end + + function varargout = return_double(this, varargin) + % RETURN_DOUBLE usage: return_double(double value) : returns double + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(29, this, varargin{:}); + end + + function varargout = return_field(this, varargin) + % RETURN_FIELD usage: return_field(Test t) : returns bool + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'Test') + varargout{1} = geometry_wrapper(30, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_field'); + end + end + + function varargout = return_int(this, varargin) + % RETURN_INT usage: return_int(int value) : returns int + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(31, this, varargin{:}); + end + + function varargout = return_matrix1(this, varargin) + % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'double') + varargout{1} = geometry_wrapper(32, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_matrix1'); + end + end + + function varargout = return_matrix2(this, varargin) + % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'double') + varargout{1} = geometry_wrapper(33, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_matrix2'); + end + end + + function varargout = return_pair(this, varargin) + % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') + [ varargout{1} varargout{2} ] = geometry_wrapper(34, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_pair'); + end + end + + function varargout = return_ptrs(this, varargin) + % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test > + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') + [ varargout{1} varargout{2} ] = geometry_wrapper(35, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_ptrs'); + end + end + + function varargout = return_size_t(this, varargin) + % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + varargout{1} = geometry_wrapper(36, this, varargin{:}); + end + + function varargout = return_string(this, varargin) + % RETURN_STRING usage: return_string(string value) : returns string + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'char') + varargout{1} = geometry_wrapper(37, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_string'); + end + end + + function varargout = return_vector1(this, varargin) + % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'double') + varargout{1} = geometry_wrapper(38, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_vector1'); + end + end + + function varargout = return_vector2(this, varargin) + % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector + % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html + if length(varargin) == 1 && isa(varargin{1},'double') + varargout{1} = geometry_wrapper(39, this, varargin{:}); + else + error('Arguments do not match any overload of function Test.return_vector2'); + end + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/aGlobalFunction.m b/wrap/tests/expected2/aGlobalFunction.m new file mode 100644 index 000000000..5cf9aafa1 --- /dev/null +++ b/wrap/tests/expected2/aGlobalFunction.m @@ -0,0 +1,6 @@ +function varargout = aGlobalFunction(varargin) + if length(varargin) == 0 + varargout{1} = geometry_wrapper(72, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp new file mode 100644 index 000000000..ab6ae5aa7 --- /dev/null +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -0,0 +1,1127 @@ +#include +#include +#include + +#include + +typedef MyTemplate MyTemplatePoint2; +typedef MyTemplate MyTemplatePoint3; +typedef MyFactor MyFactorPosePoint2; + +typedef std::set*> Collector_gtsamPoint2; +static Collector_gtsamPoint2 collector_gtsamPoint2; +typedef std::set*> Collector_gtsamPoint3; +static Collector_gtsamPoint3 collector_gtsamPoint3; +typedef std::set*> Collector_Test; +static Collector_Test collector_Test; +typedef std::set*> Collector_MyBase; +static Collector_MyBase collector_MyBase; +typedef std::set*> Collector_MyTemplatePoint2; +static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; +typedef std::set*> Collector_MyTemplatePoint3; +static Collector_MyTemplatePoint3 collector_MyTemplatePoint3; +typedef std::set*> Collector_MyFactorPosePoint2; +static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); + iter != collector_gtsamPoint2.end(); ) { + delete *iter; + collector_gtsamPoint2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); + iter != collector_gtsamPoint3.end(); ) { + delete *iter; + collector_gtsamPoint3.erase(iter++); + anyDeleted = true; + } } + { for(Collector_Test::iterator iter = collector_Test.begin(); + iter != collector_Test.end(); ) { + delete *iter; + collector_Test.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); + iter != collector_MyBase.end(); ) { + delete *iter; + collector_MyBase.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin(); + iter != collector_MyTemplatePoint2.end(); ) { + delete *iter; + collector_MyTemplatePoint2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyTemplatePoint3::iterator iter = collector_MyTemplatePoint3.begin(); + iter != collector_MyTemplatePoint3.end(); ) { + delete *iter; + collector_MyTemplatePoint3.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); + iter != collector_MyFactorPosePoint2.end(); ) { + delete *iter; + collector_MyFactorPosePoint2.erase(iter++); + anyDeleted = true; + } } + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _geometry_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_geometry_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); + types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); + types.insert(std::make_pair(typeid(MyTemplatePoint3).name(), "MyTemplatePoint3")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + BOOST_FOREACH(const StringPair& rtti_matlab, types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + mxDestroyArray(newAlreadyCreated); + } +} + +void gtsamPoint2_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamPoint2.insert(self); +} + +void gtsamPoint2_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Point2()); + collector_gtsamPoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamPoint2_constructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double x = unwrap< double >(in[0]); + double y = unwrap< double >(in[1]); + Shared *self = new Shared(new gtsam::Point2(x,y)); + collector_gtsamPoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamPoint2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamPoint2::iterator item; + item = collector_gtsamPoint2.find(self); + if(item != collector_gtsamPoint2.end()) { + delete self; + collector_gtsamPoint2.erase(item); + } +} + +void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("argChar",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + char a = unwrap< char >(in[1]); + obj->argChar(a); +} + +void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("argUChar",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + unsigned char a = unwrap< unsigned char >(in[1]); + obj->argUChar(a); +} + +void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("dim",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< int >(obj->dim()); +} + +void gtsamPoint2_returnChar_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("returnChar",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< char >(obj->returnChar()); +} + +void gtsamPoint2_vectorConfusion_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedVectorNotEigen; + typedef boost::shared_ptr Shared; + checkArguments("vectorConfusion",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(SharedVectorNotEigen(new VectorNotEigen(obj->vectorConfusion())),"VectorNotEigen", false); +} + +void gtsamPoint2_x_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("x",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< double >(obj->x()); +} + +void gtsamPoint2_y_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("y",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< double >(obj->y()); +} + +void gtsamPoint3_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamPoint3.insert(self); +} + +void gtsamPoint3_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double x = unwrap< double >(in[0]); + double y = unwrap< double >(in[1]); + double z = unwrap< double >(in[2]); + Shared *self = new Shared(new gtsam::Point3(x,y,z)); + collector_gtsamPoint3.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamPoint3_deconstructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamPoint3",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamPoint3::iterator item; + item = collector_gtsamPoint3.find(self); + if(item != collector_gtsamPoint3.end()) { + delete self; + collector_gtsamPoint3.erase(item); + } +} + +void gtsamPoint3_norm_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("norm",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); + out[0] = wrap< double >(obj->norm()); +} + +void gtsamPoint3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1); + double z = unwrap< double >(in[0]); + out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(gtsam::Point3::StaticFunctionRet(z))),"gtsam.Point3", false); +} + +void gtsamPoint3_staticFunction_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0); + out[0] = wrap< double >(gtsam::Point3::staticFunction()); +} + +void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_Test.insert(self); +} + +void Test_constructor_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new Test()); + collector_Test.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double a = unwrap< double >(in[0]); + Matrix b = unwrap< Matrix >(in[1]); + Shared *self = new Shared(new Test(a,b)); + collector_Test.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void Test_deconstructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_Test",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_Test::iterator item; + item = collector_Test.find(self); + if(item != collector_Test.end()) { + delete self; + collector_Test.erase(item); + } +} + +void Test_arg_EigenConstRef_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("arg_EigenConstRef",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + obj->arg_EigenConstRef(value); +} + +void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + pair< Test, SharedTest > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); +} + +void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("create_ptrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + pair< SharedTest, SharedTest > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); +} + +void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("print",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + obj->print(); +} + +void Test_return_Point2Ptr_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_Point2Ptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + bool value = unwrap< bool >(in[1]); + out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false); +} + +void Test_return_Test_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("return_Test",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false); +} + +void Test_return_TestPtr_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("return_TestPtr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); +} + +void Test_return_bool_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_bool",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + bool value = unwrap< bool >(in[1]); + out[0] = wrap< bool >(obj->return_bool(value)); +} + +void Test_return_double_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_double",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + double value = unwrap< double >(in[1]); + out[0] = wrap< double >(obj->return_double(value)); +} + +void Test_return_field_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_field",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + out[0] = wrap< bool >(obj->return_field(t)); +} + +void Test_return_int_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_int",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + int value = unwrap< int >(in[1]); + out[0] = wrap< int >(obj->return_int(value)); +} + +void Test_return_matrix1_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_matrix1",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Matrix value = unwrap< Matrix >(in[1]); + out[0] = wrap< Matrix >(obj->return_matrix1(value)); +} + +void Test_return_matrix2_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_matrix2",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Matrix value = unwrap< Matrix >(in[1]); + out[0] = wrap< Matrix >(obj->return_matrix2(value)); +} + +void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_pair",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Vector v = unwrap< Vector >(in[1]); + Matrix A = unwrap< Matrix >(in[2]); + pair< Vector, Matrix > pairResult = obj->return_pair(v,A); + out[0] = wrap< Vector >(pairResult.first); + out[1] = wrap< Matrix >(pairResult.second); +} + +void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("return_ptrs",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test"); + pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); +} + +void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_size_t",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + size_t value = unwrap< size_t >(in[1]); + out[0] = wrap< size_t >(obj->return_size_t(value)); +} + +void Test_return_string_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_string",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + string value = unwrap< string >(in[1]); + out[0] = wrap< string >(obj->return_string(value)); +} + +void Test_return_vector1_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_vector1",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Vector value = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->return_vector1(value)); +} + +void Test_return_vector2_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_vector2",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Vector value = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->return_vector2(value)); +} + +void MyBase_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyBase.insert(self); +} + +void MyBase_upcastFromVoid_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyBase_deconstructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyBase",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyBase::iterator item; + item = collector_MyBase.find(self); + if(item != collector_MyBase.end()) { + delete self; + collector_MyBase.erase(item); + } +} + +void MyTemplatePoint2_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyTemplatePoint2.insert(self); + + typedef boost::shared_ptr SharedBase; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); +} + +void MyTemplatePoint2_upcastFromVoid_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyTemplatePoint2_constructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new MyTemplatePoint2()); + collector_MyTemplatePoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; + + typedef boost::shared_ptr SharedBase; + out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self); +} + +void MyTemplatePoint2_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyTemplatePoint2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyTemplatePoint2::iterator item; + item = collector_MyTemplatePoint2.find(self); + if(item != collector_MyTemplatePoint2.end()) { + delete self; + collector_MyTemplatePoint2.erase(item); + } +} + +void MyTemplatePoint2_accept_T_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->accept_T(value); +} + +void MyTemplatePoint2_accept_Tptr_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->accept_Tptr(value); +} + +void MyTemplatePoint2_create_MixedPtrs_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + pair< gtsam::Point2, SharedPoint2 > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(pairResult.first)),"gtsam.Point2", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); +} + +void MyTemplatePoint2_create_ptrs_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("create_ptrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + pair< SharedPoint2, SharedPoint2 > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); +} + +void MyTemplatePoint2_return_T_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->return_T(value))),"gtsam.Point2", false); +} + +void MyTemplatePoint2_return_Tptr_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false); +} + +void MyTemplatePoint2_return_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_ptrs",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2"); + pair< SharedPoint2, SharedPoint2 > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); +} + +void MyTemplatePoint2_templatedMethod_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->templatedMethod(t); +} + +void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->templatedMethod(t); +} + +void MyTemplatePoint3_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyTemplatePoint3.insert(self); + + typedef boost::shared_ptr SharedBase; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); +} + +void MyTemplatePoint3_upcastFromVoid_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyTemplatePoint3_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new MyTemplatePoint3()); + collector_MyTemplatePoint3.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; + + typedef boost::shared_ptr SharedBase; + out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self); +} + +void MyTemplatePoint3_deconstructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyTemplatePoint3",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyTemplatePoint3::iterator item; + item = collector_MyTemplatePoint3.find(self); + if(item != collector_MyTemplatePoint3.end()) { + delete self; + collector_MyTemplatePoint3.erase(item); + } +} + +void MyTemplatePoint3_accept_T_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + gtsam::Point3& value = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->accept_T(value); +} + +void MyTemplatePoint3_accept_Tptr_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->accept_Tptr(value); +} + +void MyTemplatePoint3_create_MixedPtrs_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + pair< gtsam::Point3, SharedPoint3 > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(pairResult.first)),"gtsam.Point3", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); +} + +void MyTemplatePoint3_create_ptrs_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("create_ptrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + pair< SharedPoint3, SharedPoint3 > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point3", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); +} + +void MyTemplatePoint3_return_T_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("return_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->return_T(value))),"gtsam.Point3", false); +} + +void MyTemplatePoint3_return_Tptr_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("return_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point3", false); +} + +void MyTemplatePoint3_return_ptrs_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("return_ptrs",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point3 >(in[2], "ptr_gtsamPoint3"); + pair< SharedPoint3, SharedPoint3 > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point3", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); +} + +void MyTemplatePoint3_templatedMethod_67(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->templatedMethod(t); +} + +void MyTemplatePoint3_templatedMethod_68(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->templatedMethod(t); +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_69(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyFactorPosePoint2.insert(self); +} + +void MyFactorPosePoint2_constructor_70(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + size_t key1 = unwrap< size_t >(in[0]); + size_t key2 = unwrap< size_t >(in[1]); + double measured = unwrap< double >(in[2]); + boost::shared_ptr noiseModel = unwrap_shared_ptr< gtsam::noiseModel::Base >(in[3], "ptr_gtsamnoiseModelBase"); + Shared *self = new Shared(new MyFactorPosePoint2(key1,key2,measured,noiseModel)); + collector_MyFactorPosePoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void MyFactorPosePoint2_deconstructor_71(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyFactorPosePoint2::iterator item; + item = collector_MyFactorPosePoint2.find(self); + if(item != collector_MyFactorPosePoint2.end()) { + delete self; + collector_MyFactorPosePoint2.erase(item); + } +} + +void aGlobalFunction_72(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("aGlobalFunction",nargout,nargin,0); + out[0] = wrap< Vector >(aGlobalFunction()); +} +void overloadedGlobalFunction_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _geometry_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + gtsamPoint2_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + gtsamPoint2_constructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1); + break; + case 4: + gtsamPoint2_argChar_4(nargout, out, nargin-1, in+1); + break; + case 5: + gtsamPoint2_argUChar_5(nargout, out, nargin-1, in+1); + break; + case 6: + gtsamPoint2_dim_6(nargout, out, nargin-1, in+1); + break; + case 7: + gtsamPoint2_returnChar_7(nargout, out, nargin-1, in+1); + break; + case 8: + gtsamPoint2_vectorConfusion_8(nargout, out, nargin-1, in+1); + break; + case 9: + gtsamPoint2_x_9(nargout, out, nargin-1, in+1); + break; + case 10: + gtsamPoint2_y_10(nargout, out, nargin-1, in+1); + break; + case 11: + gtsamPoint3_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1); + break; + case 12: + gtsamPoint3_constructor_12(nargout, out, nargin-1, in+1); + break; + case 13: + gtsamPoint3_deconstructor_13(nargout, out, nargin-1, in+1); + break; + case 14: + gtsamPoint3_norm_14(nargout, out, nargin-1, in+1); + break; + case 15: + gtsamPoint3_StaticFunctionRet_15(nargout, out, nargin-1, in+1); + break; + case 16: + gtsamPoint3_staticFunction_16(nargout, out, nargin-1, in+1); + break; + case 17: + Test_collectorInsertAndMakeBase_17(nargout, out, nargin-1, in+1); + break; + case 18: + Test_constructor_18(nargout, out, nargin-1, in+1); + break; + case 19: + Test_constructor_19(nargout, out, nargin-1, in+1); + break; + case 20: + Test_deconstructor_20(nargout, out, nargin-1, in+1); + break; + case 21: + Test_arg_EigenConstRef_21(nargout, out, nargin-1, in+1); + break; + case 22: + Test_create_MixedPtrs_22(nargout, out, nargin-1, in+1); + break; + case 23: + Test_create_ptrs_23(nargout, out, nargin-1, in+1); + break; + case 24: + Test_print_24(nargout, out, nargin-1, in+1); + break; + case 25: + Test_return_Point2Ptr_25(nargout, out, nargin-1, in+1); + break; + case 26: + Test_return_Test_26(nargout, out, nargin-1, in+1); + break; + case 27: + Test_return_TestPtr_27(nargout, out, nargin-1, in+1); + break; + case 28: + Test_return_bool_28(nargout, out, nargin-1, in+1); + break; + case 29: + Test_return_double_29(nargout, out, nargin-1, in+1); + break; + case 30: + Test_return_field_30(nargout, out, nargin-1, in+1); + break; + case 31: + Test_return_int_31(nargout, out, nargin-1, in+1); + break; + case 32: + Test_return_matrix1_32(nargout, out, nargin-1, in+1); + break; + case 33: + Test_return_matrix2_33(nargout, out, nargin-1, in+1); + break; + case 34: + Test_return_pair_34(nargout, out, nargin-1, in+1); + break; + case 35: + Test_return_ptrs_35(nargout, out, nargin-1, in+1); + break; + case 36: + Test_return_size_t_36(nargout, out, nargin-1, in+1); + break; + case 37: + Test_return_string_37(nargout, out, nargin-1, in+1); + break; + case 38: + Test_return_vector1_38(nargout, out, nargin-1, in+1); + break; + case 39: + Test_return_vector2_39(nargout, out, nargin-1, in+1); + break; + case 40: + MyBase_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1); + break; + case 41: + MyBase_upcastFromVoid_41(nargout, out, nargin-1, in+1); + break; + case 42: + MyBase_deconstructor_42(nargout, out, nargin-1, in+1); + break; + case 43: + MyTemplatePoint2_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1); + break; + case 44: + MyTemplatePoint2_upcastFromVoid_44(nargout, out, nargin-1, in+1); + break; + case 45: + MyTemplatePoint2_constructor_45(nargout, out, nargin-1, in+1); + break; + case 46: + MyTemplatePoint2_deconstructor_46(nargout, out, nargin-1, in+1); + break; + case 47: + MyTemplatePoint2_accept_T_47(nargout, out, nargin-1, in+1); + break; + case 48: + MyTemplatePoint2_accept_Tptr_48(nargout, out, nargin-1, in+1); + break; + case 49: + MyTemplatePoint2_create_MixedPtrs_49(nargout, out, nargin-1, in+1); + break; + case 50: + MyTemplatePoint2_create_ptrs_50(nargout, out, nargin-1, in+1); + break; + case 51: + MyTemplatePoint2_return_T_51(nargout, out, nargin-1, in+1); + break; + case 52: + MyTemplatePoint2_return_Tptr_52(nargout, out, nargin-1, in+1); + break; + case 53: + MyTemplatePoint2_return_ptrs_53(nargout, out, nargin-1, in+1); + break; + case 54: + MyTemplatePoint2_templatedMethod_54(nargout, out, nargin-1, in+1); + break; + case 55: + MyTemplatePoint2_templatedMethod_55(nargout, out, nargin-1, in+1); + break; + case 56: + MyTemplatePoint3_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + break; + case 57: + MyTemplatePoint3_upcastFromVoid_57(nargout, out, nargin-1, in+1); + break; + case 58: + MyTemplatePoint3_constructor_58(nargout, out, nargin-1, in+1); + break; + case 59: + MyTemplatePoint3_deconstructor_59(nargout, out, nargin-1, in+1); + break; + case 60: + MyTemplatePoint3_accept_T_60(nargout, out, nargin-1, in+1); + break; + case 61: + MyTemplatePoint3_accept_Tptr_61(nargout, out, nargin-1, in+1); + break; + case 62: + MyTemplatePoint3_create_MixedPtrs_62(nargout, out, nargin-1, in+1); + break; + case 63: + MyTemplatePoint3_create_ptrs_63(nargout, out, nargin-1, in+1); + break; + case 64: + MyTemplatePoint3_return_T_64(nargout, out, nargin-1, in+1); + break; + case 65: + MyTemplatePoint3_return_Tptr_65(nargout, out, nargin-1, in+1); + break; + case 66: + MyTemplatePoint3_return_ptrs_66(nargout, out, nargin-1, in+1); + break; + case 67: + MyTemplatePoint3_templatedMethod_67(nargout, out, nargin-1, in+1); + break; + case 68: + MyTemplatePoint3_templatedMethod_68(nargout, out, nargin-1, in+1); + break; + case 69: + MyFactorPosePoint2_collectorInsertAndMakeBase_69(nargout, out, nargin-1, in+1); + break; + case 70: + MyFactorPosePoint2_constructor_70(nargout, out, nargin-1, in+1); + break; + case 71: + MyFactorPosePoint2_deconstructor_71(nargout, out, nargin-1, in+1); + break; + case 72: + aGlobalFunction_72(nargout, out, nargin-1, in+1); + break; + case 73: + overloadedGlobalFunction_73(nargout, out, nargin-1, in+1); + break; + case 74: + overloadedGlobalFunction_74(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/wrap/tests/expected2/overloadedGlobalFunction.m b/wrap/tests/expected2/overloadedGlobalFunction.m new file mode 100644 index 000000000..24758ed6e --- /dev/null +++ b/wrap/tests/expected2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(73, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(74, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index 9c064734e..14095899c 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -65,14 +65,7 @@ classdef ClassA < handle function varargout = Afunction(varargin) % AFUNCTION usage: afunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Usage - % AFUNCTION() - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(12, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.Afunction'); - end + varargout{1} = testNamespaces_wrapper(12, varargin{:}); end end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index bc233763d..5a6cee1a5 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -3,6 +3,8 @@ class VectorNotEigen; class ns::OtherClass; +namespace gtsam { + class Point2 { Point2(); Point2(double x, double y); @@ -23,12 +25,13 @@ class Point3 { // static functions - use static keyword and uppercase static double staticFunction(); - static Point3 StaticFunctionRet(double z); + static gtsam::Point3 StaticFunctionRet(double z); // enabling serialization functionality void serialize() const; // Just triggers a flag internally and removes actual function }; +} // another comment // another comment @@ -71,7 +74,7 @@ class Test { Test* return_TestPtr(Test* value) const; Test return_Test(Test* value) const; - Point2* return_Point2Ptr(bool value) const; + gtsam::Point2* return_Point2Ptr(bool value) const; pair create_ptrs () const; pair create_MixedPtrs () const; @@ -91,6 +94,38 @@ Vector aGlobalFunction(); Vector overloadedGlobalFunction(int a); Vector overloadedGlobalFunction(int a, double b); +// A base class +virtual class MyBase { + +}; + +// A templated class +template +virtual class MyTemplate : MyBase { + MyTemplate(); + + template + void templatedMethod(const ARG& t); + + // Stress test templates and pointer combinations + void accept_T(const T& value) const; + void accept_Tptr(T* value) const; + T* return_Tptr(T* value) const; + T return_T(T* value) const; + pair create_ptrs () const; + pair create_MixedPtrs () const; + pair return_ptrs (T* p1, T* p2) const; +}; + +// A doubly templated class +template +class MyFactor { + MyFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); +}; + +// and a typedef specializing it +typedef MyFactor MyFactorPosePoint2; + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp new file mode 100644 index 000000000..d68daf4ba --- /dev/null +++ b/wrap/tests/testClass.cpp @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * 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 testClass.cpp + * @brief Unit test for Class class + * @author Frank Dellaert + * @date Nov 12, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +// Constructor +TEST( Class, Constructor ) { + Class cls; +} + +/* ************************************************************************* */ +// test method overloading +TEST( Class, OverloadingMethod ) { + Class cls; + const string name = "method1"; + EXPECT(!cls.exists(name)); + + bool verbose = true, is_const = true; + ArgumentList args; + const ReturnValue retVal; + const string templateArgName; + vector templateArgValues; + cls.addMethod(verbose, is_const, name, args, retVal, templateArgName, + templateArgValues); + EXPECT_LONGS_EQUAL(1, cls.nrMethods()); + EXPECT(cls.exists(name)); + Method& method = cls.method(name); + EXPECT_LONGS_EQUAL(1, method.nrOverloads()); + + cls.addMethod(verbose, is_const, name, args, retVal, templateArgName, + templateArgValues); + EXPECT_LONGS_EQUAL(1, cls.nrMethods()); + EXPECT_LONGS_EQUAL(2, method.nrOverloads()); +} + +/* ************************************************************************* */ +// test templated methods +TEST( Class, TemplatedMethods ) { + Class cls; + const string name = "method"; + EXPECT(!cls.exists(name)); + + bool verbose = true, is_const = true; + ArgumentList args; + Argument arg; + arg.type.name = "T"; + args.push_back(arg); + const ReturnValue retVal(ReturnType("T")); + const string templateArgName("T"); + vector templateArgValues; + templateArgValues.push_back(Qualified("Point2")); + templateArgValues.push_back(Qualified("Point3")); + cls.addMethod(verbose, is_const, name, args, retVal, templateArgName, + templateArgValues); + EXPECT_LONGS_EQUAL(2, cls.nrMethods()); + EXPECT(cls.exists(name+"Point2")); + EXPECT(cls.exists(name+"Point3")); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/wrap/tests/testMethod.cpp b/wrap/tests/testMethod.cpp new file mode 100644 index 000000000..8050f0d3c --- /dev/null +++ b/wrap/tests/testMethod.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 testMethod.cpp + * @brief Unit test for Method class + * @author Frank Dellaert + * @date Nov 12, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +// Constructor +TEST( Method, Constructor ) { + Method method; +} + +/* ************************************************************************* */ +// addOverload +TEST( Method, addOverload ) { + Method method; + method.initializeOrCheck("myName"); + bool is_const = true; + ArgumentList args; + const ReturnValue retVal(ReturnType("return_type")); + method.addOverload("myName", args, retVal, is_const); + EXPECT_LONGS_EQUAL(1, method.nrOverloads()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index c6ce0903a..0645fb407 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -48,9 +48,9 @@ static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; - Argument arg1; arg1.type = "double"; arg1.name = "x"; - Argument arg2; arg2.type = "double"; arg2.name = "y"; - Argument arg3; arg3.type = "double"; arg3.name = "z"; + Argument arg1; arg1.type.name = "double"; arg1.name = "x"; + Argument arg2; arg2.type.name = "double"; arg2.name = "y"; + Argument arg3; arg3.type.name = "double"; arg3.name = "z"; args.push_back(arg1); args.push_back(arg2); args.push_back(arg3); @@ -64,16 +64,15 @@ TEST( wrap, check_exception ) { THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); - // clean out previous generated code - fs::remove_all("actual_deps"); - - string path = topdir + "/wrap/tests"; - Module module(path.c_str(), "testDependencies",enable_verbose); - CHECK_EXCEPTION(module.matlab_code("actual_deps", headerPath), DependencyMissing); +// // TODO: matlab_code does not throw this anymore, so check constructor +// fs::remove_all("actual_deps"); // clean out previous generated code +// string path = topdir + "/wrap/tests"; +// Module module(path.c_str(), "testDependencies",enable_verbose); +// CHECK_EXCEPTION(module.matlab_code("actual_deps"), DependencyMissing); } /* ************************************************************************* */ -TEST( wrap, small_parse ) { +TEST( wrap, Small ) { string moduleName("gtsam"); Module module(moduleName, true); @@ -92,68 +91,64 @@ TEST( wrap, small_parse ) { EXPECT(assert_equal("Point2", cls.name)); EXPECT(!cls.isVirtual); EXPECT(cls.namespaces.empty()); - LONGS_EQUAL(3, cls.methods.size()); + LONGS_EQUAL(3, cls.nrMethods()); LONGS_EQUAL(1, cls.static_methods.size()); // Method 1 - Method m1 = cls.methods.at("x"); - EXPECT(assert_equal("x", m1.name)); - EXPECT(m1.is_const_); - LONGS_EQUAL(1, m1.argLists.size()); - LONGS_EQUAL(1, m1.returnVals.size()); + Method m1 = cls.method("x"); + EXPECT(assert_equal("x", m1.name())); + EXPECT(m1.isConst()); + LONGS_EQUAL(1, m1.nrOverloads()); - ReturnValue rv1 = m1.returnVals.front(); + ReturnValue rv1 = m1.returnValue(0); EXPECT(!rv1.isPair); - EXPECT(!rv1.isPtr1); - EXPECT(assert_equal("double", rv1.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::BASIS, rv1.category1); + EXPECT(!rv1.type1.isPtr); + EXPECT(assert_equal("double", rv1.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, rv1.type1.category); // Method 2 - Method m2 = cls.methods.at("returnMatrix"); - EXPECT(assert_equal("returnMatrix", m2.name)); - EXPECT(m2.is_const_); - LONGS_EQUAL(1, m2.argLists.size()); - LONGS_EQUAL(1, m2.returnVals.size()); + Method m2 = cls.method("returnMatrix"); + EXPECT(assert_equal("returnMatrix", m2.name())); + EXPECT(m2.isConst()); + LONGS_EQUAL(1, m2.nrOverloads()); - ReturnValue rv2 = m2.returnVals.front(); + ReturnValue rv2 = m2.returnValue(0); EXPECT(!rv2.isPair); - EXPECT(!rv2.isPtr1); - EXPECT(assert_equal("Matrix", rv2.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, rv2.category1); + EXPECT(!rv2.type1.isPtr); + EXPECT(assert_equal("Matrix", rv2.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv2.type1.category); // Method 3 - Method m3 = cls.methods.at("returnPoint2"); - EXPECT(assert_equal("returnPoint2", m3.name)); - EXPECT(m3.is_const_); - LONGS_EQUAL(1, m3.argLists.size()); - LONGS_EQUAL(1, m3.returnVals.size()); + Method m3 = cls.method("returnPoint2"); + EXPECT(assert_equal("returnPoint2", m3.name())); + EXPECT(m3.isConst()); + LONGS_EQUAL(1, m3.nrOverloads()); - ReturnValue rv3 = m3.returnVals.front(); + ReturnValue rv3 = m3.returnValue(0); EXPECT(!rv3.isPair); - EXPECT(!rv3.isPtr1); - EXPECT(assert_equal("Point2", rv3.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::CLASS, rv3.category1); + EXPECT(!rv3.type1.isPtr); + EXPECT(assert_equal("Point2", rv3.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv3.type1.category); // Static Method 1 // static Vector returnVector(); StaticMethod sm1 = cls.static_methods.at("returnVector"); - EXPECT(assert_equal("returnVector", sm1.name)); - LONGS_EQUAL(1, sm1.argLists.size()); - LONGS_EQUAL(1, sm1.returnVals.size()); + EXPECT(assert_equal("returnVector", sm1.name())); + LONGS_EQUAL(1, sm1.nrOverloads()); - ReturnValue rv4 = sm1.returnVals.front(); + ReturnValue rv4 = sm1.returnValue(0); EXPECT(!rv4.isPair); - EXPECT(!rv4.isPtr1); - EXPECT(assert_equal("Vector", rv4.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, rv4.category1); + EXPECT(!rv4.type1.isPtr); + EXPECT(assert_equal("Vector", rv4.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category); } /* ************************************************************************* */ -TEST( wrap, parse_geometry ) { +TEST( wrap, Geometry ) { string markup_header_path = topdir + "/wrap/tests"; Module module(markup_header_path.c_str(), "geometry",enable_verbose); - EXPECT_LONGS_EQUAL(3, module.classes.size()); + EXPECT_LONGS_EQUAL(7, module.classes.size()); // forward declarations LONGS_EQUAL(2, module.forward_declarations.size()); @@ -164,9 +159,9 @@ TEST( wrap, parse_geometry ) { strvec exp_includes; exp_includes += "folder/path/to/Test.h"; EXPECT(assert_equal(exp_includes, module.includes)); - LONGS_EQUAL(3, module.classes.size()); + LONGS_EQUAL(7, module.classes.size()); - // Key for ReturnValue::return_category + // Key for ReturnType::return_category // CLASS = 1, // EIGEN = 2, // BASIS = 3, @@ -188,122 +183,126 @@ TEST( wrap, parse_geometry ) { Class cls = module.classes.at(0); EXPECT(assert_equal("Point2", cls.name)); - EXPECT_LONGS_EQUAL(2, cls.constructor.args_list.size()); - EXPECT_LONGS_EQUAL(7, cls.methods.size()); + EXPECT_LONGS_EQUAL(2, cls.constructor.nrOverloads()); + EXPECT_LONGS_EQUAL(7, cls.nrMethods()); { // char returnChar() const; - CHECK(cls.methods.find("returnChar") != cls.methods.end()); - Method m1 = cls.methods.find("returnChar")->second; - LONGS_EQUAL(1, m1.returnVals.size()); - EXPECT(assert_equal("char", m1.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::BASIS, m1.returnVals.front().category1); - EXPECT(!m1.returnVals.front().isPair); - EXPECT(assert_equal("returnChar", m1.name)); - LONGS_EQUAL(1, m1.argLists.size()); - EXPECT_LONGS_EQUAL(0, m1.argLists.front().size()); - EXPECT(m1.is_const_); + CHECK(cls.exists("returnChar")); + Method m1 = cls.method("returnChar"); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT(assert_equal("char", m1.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category); + EXPECT(!m1.returnValue(0).isPair); + EXPECT(assert_equal("returnChar", m1.name())); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size()); + EXPECT(m1.isConst()); } { // VectorNotEigen vectorConfusion(); - CHECK(cls.methods.find("vectorConfusion") != cls.methods.end()); - Method m1 = cls.methods.find("vectorConfusion")->second; - LONGS_EQUAL(1, m1.returnVals.size()); - EXPECT(assert_equal("VectorNotEigen", m1.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::CLASS, m1.returnVals.front().category1); - EXPECT(!m1.returnVals.front().isPair); - EXPECT(assert_equal("vectorConfusion", m1.name)); - LONGS_EQUAL(1, m1.argLists.size()); - EXPECT_LONGS_EQUAL(0, m1.argLists.front().size()); - EXPECT(!m1.is_const_); + CHECK(cls.exists("vectorConfusion")); + Method m1 = cls.method("vectorConfusion"); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT(assert_equal("VectorNotEigen", m1.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::CLASS, m1.returnValue(0).type1.category); + EXPECT(!m1.returnValue(0).isPair); + EXPECT(assert_equal("vectorConfusion", m1.name())); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size()); + EXPECT(!m1.isConst()); } EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); - EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + EXPECT_LONGS_EQUAL(1, cls.namespaces.size()); +#ifndef WRAP_DISABLE_SERIALIZE // check serialization flag EXPECT(cls.isSerializable); EXPECT(!cls.hasSerialization); - } +#endif + } // check second class, Point3 { Class cls = module.classes.at(1); EXPECT(assert_equal("Point3", cls.name)); - EXPECT_LONGS_EQUAL(1, cls.constructor.args_list.size()); - EXPECT_LONGS_EQUAL(1, cls.methods.size()); + EXPECT_LONGS_EQUAL(1, cls.constructor.nrOverloads()); + EXPECT_LONGS_EQUAL(1, cls.nrMethods()); EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); - EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + EXPECT_LONGS_EQUAL(1, cls.namespaces.size()); // first constructor takes 3 doubles - ArgumentList c1 = cls.constructor.args_list.front(); + ArgumentList c1 = cls.constructor.argumentList(0); EXPECT_LONGS_EQUAL(3, c1.size()); // check first double argument Argument a1 = c1.front(); EXPECT(!a1.is_const); - EXPECT(assert_equal("double", a1.type)); + EXPECT(assert_equal("double", a1.type.name)); EXPECT(!a1.is_ref); EXPECT(assert_equal("x", a1.name)); // check method - CHECK(cls.methods.find("norm") != cls.methods.end()); - Method m1 = cls.methods.find("norm")->second; - LONGS_EQUAL(1, m1.returnVals.size()); - EXPECT(assert_equal("double", m1.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::BASIS, m1.returnVals.front().category1); - EXPECT(assert_equal("norm", m1.name)); - LONGS_EQUAL(1, m1.argLists.size()); - EXPECT_LONGS_EQUAL(0, m1.argLists.front().size()); - EXPECT(m1.is_const_); + CHECK(cls.exists("norm")); + Method m1 = cls.method("norm"); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT(assert_equal("double", m1.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category); + EXPECT(assert_equal("norm", m1.name())); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size()); + EXPECT(m1.isConst()); +#ifndef WRAP_DISABLE_SERIALIZE // check serialization flag EXPECT(cls.isSerializable); EXPECT(cls.hasSerialization); - } +#endif + } // Test class is the third one { Class testCls = module.classes.at(2); - EXPECT_LONGS_EQUAL( 2, testCls.constructor.args_list.size()); - EXPECT_LONGS_EQUAL(19, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 2, testCls.constructor.nrOverloads()); + EXPECT_LONGS_EQUAL(19, testCls.nrMethods()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); // function to parse: pair return_pair (Vector v, Matrix A) const; - CHECK(testCls.methods.find("return_pair") != testCls.methods.end()); - Method m2 = testCls.methods.find("return_pair")->second; - LONGS_EQUAL(1, m2.returnVals.size()); - EXPECT(m2.returnVals.front().isPair); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, m2.returnVals.front().category1); - EXPECT(assert_equal("Vector", m2.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, m2.returnVals.front().category2); - EXPECT(assert_equal("Matrix", m2.returnVals.front().type2)); + CHECK(testCls.exists("return_pair")); + Method m2 = testCls.method("return_pair"); + LONGS_EQUAL(1, m2.nrOverloads()); + EXPECT(m2.returnValue(0).isPair); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type1.category); + EXPECT(assert_equal("Vector", m2.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type2.category); + EXPECT(assert_equal("Matrix", m2.returnValue(0).type2.name)); // checking pointer args and return values // pair return_ptrs (Test* p1, Test* p2) const; - CHECK(testCls.methods.find("return_ptrs") != testCls.methods.end()); - Method m3 = testCls.methods.find("return_ptrs")->second; - LONGS_EQUAL(1, m3.argLists.size()); - ArgumentList args = m3.argLists.front(); + CHECK(testCls.exists("return_ptrs")); + Method m3 = testCls.method("return_ptrs"); + LONGS_EQUAL(1, m3.nrOverloads()); + ArgumentList args = m3.argumentList(0); LONGS_EQUAL(2, args.size()); Argument arg1 = args.at(0); EXPECT(arg1.is_ptr); EXPECT(!arg1.is_const); EXPECT(!arg1.is_ref); - EXPECT(assert_equal("Test", arg1.type)); + EXPECT(assert_equal("Test", arg1.type.name)); EXPECT(assert_equal("p1", arg1.name)); - EXPECT(arg1.namespaces.empty()); + EXPECT(arg1.type.namespaces.empty()); Argument arg2 = args.at(1); EXPECT(arg2.is_ptr); EXPECT(!arg2.is_const); EXPECT(!arg2.is_ref); - EXPECT(assert_equal("Test", arg2.type)); + EXPECT(assert_equal("Test", arg2.type.name)); EXPECT(assert_equal("p2", arg2.name)); - EXPECT(arg2.namespaces.empty()); + EXPECT(arg2.type.namespaces.empty()); } // evaluate global functions @@ -312,12 +311,12 @@ TEST( wrap, parse_geometry ) { CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); - EXPECT(assert_equal("aGlobalFunction", gfunc.name)); - LONGS_EQUAL(1, gfunc.returnVals.size()); - EXPECT(assert_equal("Vector", gfunc.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(1, gfunc.argLists.size()); - LONGS_EQUAL(1, gfunc.namespaces.size()); - EXPECT(gfunc.namespaces.front().empty()); + EXPECT(assert_equal("aGlobalFunction", gfunc.name())); + LONGS_EQUAL(1, gfunc.nrOverloads()); + EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(1, gfunc.nrOverloads()); + LONGS_EQUAL(1, gfunc.overloads.size()); + EXPECT(gfunc.overloads.front().namespaces.empty()); } } @@ -386,18 +385,17 @@ TEST( wrap, parse_namespaces ) { CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); - EXPECT(assert_equal("aGlobalFunction", gfunc.name)); - LONGS_EQUAL(2, gfunc.returnVals.size()); - EXPECT(assert_equal("Vector", gfunc.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(2, gfunc.argLists.size()); + EXPECT(assert_equal("aGlobalFunction", gfunc.name())); + LONGS_EQUAL(2, gfunc.nrOverloads()); + EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name)); // check namespaces - LONGS_EQUAL(2, gfunc.namespaces.size()); + LONGS_EQUAL(2, gfunc.overloads.size()); strvec exp_namespaces1; exp_namespaces1 += "ns1"; - EXPECT(assert_equal(exp_namespaces1, gfunc.namespaces.at(0))); + EXPECT(assert_equal(exp_namespaces1, gfunc.overloads.at(0).namespaces)); strvec exp_namespaces2; exp_namespaces2 += "ns2"; - EXPECT(assert_equal(exp_namespaces2, gfunc.namespaces.at(1))); + EXPECT(assert_equal(exp_namespaces2, gfunc.overloads.at(1).namespaces)); } } @@ -414,7 +412,7 @@ TEST( wrap, matlab_code_namespaces ) { // emit MATLAB code string exp_path = path + "/tests/expected_namespaces/"; string act_path = "actual_namespaces/"; - module.matlab_code("actual_namespaces", headerPath); + module.matlab_code("actual_namespaces"); EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); @@ -442,18 +440,45 @@ TEST( wrap, matlab_code_geometry ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual", headerPath); + module.matlab_code("actual"); +#ifndef WRAP_DISABLE_SERIALIZE string epath = path + "/tests/expected/"; +#else + string epath = path + "/tests/expected2/"; +#endif string apath = "actual/"; EXPECT(files_equal(epath + "geometry_wrapper.cpp" , apath + "geometry_wrapper.cpp" )); - EXPECT(files_equal(epath + "Point2.m" , apath + "Point2.m" )); - EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); + EXPECT(files_equal(epath + "+gtsam/Point2.m" , apath + "+gtsam/Point2.m" )); + EXPECT(files_equal(epath + "+gtsam/Point3.m" , apath + "+gtsam/Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); + EXPECT(files_equal(epath + "MyBase.m" , apath + "MyBase.m" )); + EXPECT(files_equal(epath + "MyTemplatePoint2.m" , apath + "MyTemplatePoint2.m" )); + EXPECT(files_equal(epath + "MyTemplatePoint3.m" , apath + "MyTemplatePoint3.m" )); + EXPECT(files_equal(epath + "MyFactorPosePoint2.m" , apath + "MyFactorPosePoint2.m")); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } +/* ************************************************************************* */ +TEST( wrap, python_code_geometry ) { + // Parse into class object + string header_path = topdir + "/wrap/tests"; + Module module(header_path,"geometry",enable_verbose); + string path = topdir + "/wrap"; + + // clean out previous generated code + fs::remove_all("actual-python"); + + // emit MATLAB code + // make_geometry will not compile, use make testwrap to generate real make + module.python_wrapper("actual-python"); + string epath = path + "/tests/expected-python/"; + string apath = "actual-python/"; + + EXPECT(files_equal(epath + "geometry_python.cpp", apath + "geometry_python.cpp" )); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 47f7d10a6..51dc6f4c3 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -112,29 +112,23 @@ string maybe_shared_ptr(bool add, const string& qtype, const string& type) { string str = add? "Shared" : ""; if (add) str += type; else str += qtype; - - //if (add) str += ">"; return str; } /* ************************************************************************* */ -string qualifiedName(const string& separator, const vector& names, const string& finalName) { +string qualifiedName(const string& separator, const vector& names) { string result; - if(!names.empty()) { - for(size_t i = 0; i < names.size() - 1; ++i) + if (!names.empty()) { + for (size_t i = 0; i < names.size() - 1; ++i) result += (names[i] + separator); - if(finalName.empty()) - result += names.back(); - else - result += (names.back() + separator + finalName); - } else if(!finalName.empty()) { - result = finalName; + result += names.back(); } return result; } /* ************************************************************************* */ -void createNamespaceStructure(const std::vector& namespaces, const std::string& toolboxPath) { +void createNamespaceStructure(const std::vector& namespaces, + const std::string& toolboxPath) { using namespace boost::filesystem; path curPath = toolboxPath; BOOST_FOREACH(const string& subdir, namespaces) { diff --git a/wrap/utilities.h b/wrap/utilities.h index 7280dd297..a4f71b6ad 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -18,17 +18,20 @@ #pragma once +#include "FileWriter.h" + +#include +#include + +#include #include #include #include #include + //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC -#include -#include - -#include "FileWriter.h" namespace wrap { @@ -123,12 +126,12 @@ bool assert_equal(const std::vector& expected, const std::vector& names, const std::string& finalName = ""); +std::string qualifiedName(const std::string& separator, const std::vector& names); /** creates the necessary folders for namespaces, as specified by a namespace stack */ -void createNamespaceStructure(const std::vector& namespaces, const std::string& toolboxPath); +void createNamespaceStructure(const std::vector& namespaces, + const std::string& toolboxPath); } // \namespace wrap diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 37b8e36b1..2e5ac1612 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -40,7 +40,7 @@ void generate_matlab_toolbox( wrap::Module module(interfacePath, moduleName, false); // Then emit MATLAB code - module.matlab_code(toolboxPath,headerPath); + module.matlab_code(toolboxPath); } /** Displays usage information */