diff --git a/.cproject b/.cproject index 9c03c5b7d..241afea81 100644 --- a/.cproject +++ b/.cproject @@ -2393,6 +2393,38 @@ true true + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + make -j5 diff --git a/gtsam.h b/gtsam.h index d63563028..96d51117a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1738,6 +1738,8 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::EssentialMatrix& t); void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); @@ -1750,9 +1752,10 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& t); void update(size_t j, const gtsam::EssentialMatrix& t); void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); - template + template T at(size_t j); }; @@ -2150,7 +2153,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; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index a3a5b029b..1190822ed 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -73,7 +73,6 @@ template struct dimension: public Dynamic { }; - /** * zero::value is intended to be the origin of a canonical coordinate system * with canonical(t) == DefaultChart::local(zero::value, t) @@ -111,14 +110,16 @@ struct is_group > : publi }; template -struct is_manifold > : public boost::true_type{ +struct is_manifold > : public boost::true_type { }; template -struct dimension > : public boost::integral_constant { - //TODO after switch to c++11 : the above should should be extracted to a constexpr function - // for readability and to reduce code duplication +struct dimension > : public boost::integral_constant< + int, + M == Eigen::Dynamic ? Eigen::Dynamic : + (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { + //TODO after switch to c++11 : the above should should be extracted to a constexpr function + // for readability and to reduce code duplication }; template @@ -131,10 +132,10 @@ struct zero > { }; template -struct identity > : public zero > { +struct identity > : public zero< + Eigen::Matrix > { }; - template struct is_chart: public boost::false_type { }; @@ -248,12 +249,16 @@ struct DefaultChart > { * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. */ - typedef Eigen::Matrix type; + typedef Eigen::Matrix type; typedef type T; - 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"); + typedef Eigen::Matrix::value, 1> vector; + + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "Internal error: DefaultChart for Dynamic should be handled by template below"); + 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); @@ -266,20 +271,36 @@ struct DefaultChart > { // Dynamically sized Vector template<> struct DefaultChart { - typedef Vector T; - typedef T type; - typedef T vector; - static vector local(const T& origin, const T& other) { + typedef Vector type; + typedef Vector vector; + static vector local(const Vector& origin, const Vector& other) { return other - origin; } - static T retract(const T& origin, const vector& d) { + static Vector retract(const Vector& origin, const vector& d) { return origin + d; } - static int getDimension(const T& origin) { + static int getDimension(const Vector& origin) { return origin.size(); } }; +// Dynamically sized Matrix +template<> +struct DefaultChart { + typedef Matrix type; + typedef Vector vector; + static vector local(const Matrix& origin, const Matrix& other) { + Matrix d = other - origin; + return Eigen::Map(d.data(),getDimension(d)); + } + static Matrix retract(const Matrix& origin, const vector& d) { + return origin + Eigen::Map(d.data(),origin.rows(),origin.cols()); + } + static int getDimension(const Matrix& m) { + return m.size(); + } +}; + /** * Old Concept check class for Manifold types * Requires a mapping between a linear tangent space and the underlying diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 50bed2e3b..7299d7c8a 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -26,8 +26,8 @@ namespace gtsam { /* ************************************************************************* */ template void MetisIndex::augment(const FactorGraph& factors) { - std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first - std::map >::iterator iAdjMapIt; + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ @@ -36,7 +36,7 @@ void MetisIndex::augment(const FactorGraph& factors) { // starting at index xadj[i] and ending at(but not including) // index xadj[i + 1](i.e., adjncy[xadj[i]] through // and including adjncy[xadj[i + 1] - 1]). - idx_t keyCounter = 0; + int32_t keyCounter = 0; // First: Record a copy of each key inside the factorgraph and create a // key to integer mapping. This is referenced during the adjaceny step @@ -58,7 +58,7 @@ void MetisIndex::augment(const FactorGraph& factors) { BOOST_FOREACH(const Key& k1, *factors[i]) BOOST_FOREACH(const Key& k2, *factors[i]) if (k1 != k2) { - // Store both in Key and idx_t format + // Store both in Key and int32_t format int i = intKeyBMap_.left.at(k1); int j = intKeyBMap_.left.at(k2); iAdjMap[i].insert(iAdjMap[i].end(), j); @@ -71,14 +71,14 @@ void MetisIndex::augment(const FactorGraph& factors) { xadj_.push_back(0); // Always set the first index to zero for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { - std::vector temp; + std::vector temp; // Copy from the FastSet into a temporary vector std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); - xadj_.push_back((idx_t) adj_.size()); + xadj_.push_back((int32_t) adj_.size()); } } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 51624e29e..22b03ee5d 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -22,7 +22,6 @@ #include #include #include -#include // Boost bimap generates many ugly warnings in CLANG #ifdef __clang__ @@ -47,13 +46,13 @@ namespace gtsam { class GTSAM_EXPORT MetisIndex { public: typedef boost::shared_ptr shared_ptr; - typedef boost::bimap bm_type; + typedef boost::bimap bm_type; private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; - boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; + boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship size_t nKeys_; public: @@ -84,16 +83,16 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { + std::vector xadj() const { return xadj_; } - std::vector adj() const { + std::vector adj() const { return adj_; } size_t nValues() const { return nKeys_; } - Key intToKey(idx_t value) const { + Key intToKey(int32_t value) const { assert(value >= 0); return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 3c397c9e9..d6e1b6e98 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -106,9 +106,9 @@ namespace gtsam { typedef boost::tuple Base; KeyInfoEntry(){} KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - const size_t index() const { return this->get<0>(); } - const size_t dim() const { return this->get<1>(); } - const size_t colstart() const { return this->get<2>(); } + size_t index() const { return this->get<0>(); } + size_t dim() const { return this->get<1>(); } + size_t colstart() const { return this->get<2>(); } }; /************************************************************************************/ diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 08dd18ee3..332c23ca7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -106,7 +106,7 @@ struct UseBlockIf { }; } -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { @@ -186,28 +186,28 @@ public: } } /** - * *** This is the main entry point for reverseAD, called from Expression *** + * *** This is the main entry point for reverse AD, called from Expression *** * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) */ typedef Eigen::Matrix JacobianTT; - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD1(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors static const JacobianTT I = JacobianTT::Identity(); handleLeafCase(I, jacobians, content.key); } else if (kind == Function) // This is the more typical entry point, starting the AD pipeline - // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD(jacobians); + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) template - void reverseAD(const Eigen::MatrixBase & dTdA, + void reverseAD1(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { if (kind == Leaf) handleLeafCase(dTdA, jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA, jacobians); + content.ptr->reverseAD2(dTdA, jacobians); } /// Define type so we can apply it as a meta-function @@ -470,10 +470,10 @@ struct FunctionalBase: ExpressionNode { struct Record { void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { } }; /// Construct an execution trace for reverse AD @@ -505,9 +505,9 @@ struct JacobianTrace { typename Jacobian::type dTdA; }; -/** - * Recursive Definition of Functional ExpressionNode - */ +// Recursive Definition of Functional ExpressionNode +// The reason we inherit from Argument is because we can then +// case to this unique signature to retrieve the expression at any level template struct GenerateFunctionalNode: Argument, Base { @@ -528,7 +528,9 @@ struct GenerateFunctionalNode: Argument, Base { This::expression->dims(map); } - /// Recursive Record Class for Functional Expressions + // Recursive Record Class for Functional Expressions + // The reason we inherit from JacobianTrace is because we can then + // case to this unique signature to retrieve the value/trace at any level struct Record: JacobianTrace, Base::Record { typedef T return_type; @@ -543,17 +545,26 @@ struct GenerateFunctionalNode: Argument, Base { } /// Start the reverse AD process - void startReverseAD(JacobianMap& jacobians) const { - Base::Record::startReverseAD(jacobians); - This::trace.reverseAD(This::dTdA, jacobians); + void startReverseAD4(JacobianMap& jacobians) const { + Base::Record::startReverseAD4(jacobians); + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + This::trace.reverseAD1(This::dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process + // Cols is always known at compile time template - void reverseAD(const Eigen::Matrix & dFdT, + void reverseAD4(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const { - Base::Record::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); + Base::Record::reverseAD4(dFdT, jacobians); + This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } }; @@ -614,8 +625,8 @@ struct FunctionalNode { struct Record: public internal::CallRecordImplementor::value>, public Base::Record { using Base::Record::print; - using Base::Record::startReverseAD; - using Base::Record::reverseAD; + using Base::Record::startReverseAD4; + using Base::Record::reverseAD4; virtual ~Record() { } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index d44d21cd7..c7f022724 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -209,7 +209,7 @@ private: ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD(jacobians); + trace.startReverseAD1(jacobians); return value; } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 09fe0f253..941728d8c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -12,6 +12,8 @@ /** * @file testValues.cpp * @author Richard Roberts + * @author Frank Dellaert + * @author Mike Bosse */ #include @@ -168,9 +170,9 @@ TEST(Values, basic_functions) Values values; const Values& values_c = values; values.insert(2, Vector3()); - values.insert(4, Vector3()); - values.insert(6, Vector3()); - values.insert(8, Vector3()); + values.insert(4, Vector(3)); + values.insert(6, Matrix23()); + values.insert(8, Matrix(2,3)); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h index f1ac0b044..159a841e5 100644 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -32,12 +32,6 @@ class JacobianMap; // forward declaration //----------------------------------------------------------------------------- -/** - * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific - * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface. - */ -const int MaxVirtualStaticRows = 4; - namespace internal { /** @@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType { template<> struct ConvertToVirtualFunctionSupportedMatrixType { template - static const Eigen::Matrix convert( + static const Eigen::Matrix convert( const Eigen::MatrixBase & x) { return x; } @@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -/** - * Recursive definition of an interface having several purely - * virtual _reverseAD(const Eigen::Matrix &, JacobianMap&) - * with Rows in 1..MaxSupportedStaticRows - */ -template -struct ReverseADInterface: ReverseADInterface { - using ReverseADInterface::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -template -struct ReverseADInterface<0, Cols> { - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; - virtual void _reverseAD(const Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -/** - * ReverseADImplementor is a utility class used by CallRecordImplementor to - * implementing the recursive ReverseADInterface interface. - */ -template -struct ReverseADImplementor: ReverseADImplementor { -private: - using ReverseADImplementor::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - static_cast(this)->reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - -template -struct ReverseADImplementor : virtual internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { -private: - using internal::ReverseADInterface::_reverseAD; - const Derived & derived() const { - return static_cast(*this); - } - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - } // namespace internal /** - * The CallRecord class stores the Jacobians of applying a function - * with respect to each of its arguments. It also stores an execution trace - * (defined below) for each of its arguments. - * - * It is implemented in the function-style ExpressionNode's nested Record class below. + * The CallRecord is an abstract base class for the any class that stores + * the Jacobians of applying a function with respect to each of its arguments, + * as well as an execution trace for each of its arguments. */ template -struct CallRecord: virtual private internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { +struct CallRecord { + // Print entire record, recursively inline void print(const std::string& indent) const { _print(indent); } - inline void startReverseAD(JacobianMap& jacobians) const { - _startReverseAD(jacobians); + // Main entry point for the reverse AD process of a functional expression. + // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 + // This function then calls ExecutionTrace::reverseAD for every argument + // which will in turn call the reverseAD method below. + // This non-virtual function _startReverseAD3, implemented in derived + inline void startReverseAD2(JacobianMap& jacobians) const { + _startReverseAD3(jacobians); } + // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 + // Here we convert to dynamic if the template - inline void reverseAD(const Eigen::MatrixBase & dFdT, + inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { - _reverseAD( - internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( - dFdT), jacobians); + _reverseAD3( + internal::ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), + jacobians); } - inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - _reverseAD(dFdT, jacobians); + // This overload supports matrices with both rows and columns dynamically sized. + // The template version above would be slower by introducing an extra conversion + // to statically sized columns. + inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const { + _reverseAD3(dFdT, jacobians); } virtual ~CallRecord() { } private: + virtual void _print(const std::string& indent) const = 0; - virtual void _startReverseAD(JacobianMap& jacobians) const = 0; - using internal::ReverseADInterface::_reverseAD; + virtual void _startReverseAD3(JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; }; +/** + * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * interface. It is used to keep the testCallRecord unit test in sync. + */ +const int CallRecordMaxVirtualStaticRows = 5; + namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. */ template -struct CallRecordImplementor: public CallRecord, - private ReverseADImplementor { +struct CallRecordImplementor: public CallRecord { private: + const Derived & derived() const { return static_cast(*this); } + virtual void _print(const std::string& indent) const { derived().print(indent); } - virtual void _startReverseAD(JacobianMap& jacobians) const { - derived().startReverseAD(jacobians); + + virtual void _startReverseAD3(JacobianMap& jacobians) const { + derived().startReverseAD4(jacobians); + } + + virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); } - template friend struct ReverseADImplementor; }; } // namespace internal diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 95c4d71a8..de6e4983a 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -35,7 +35,6 @@ class ExpressionFactor: public NoiseModelFactor { T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices - size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -55,15 +54,6 @@ public: // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now boost::tie(keys_, dims_) = expression_.keysAndDims(); - - // Add sizes to know how much memory to allocate on stack in linearize - augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1); - -#ifdef DEBUG_ExpressionFactor - BOOST_FOREACH(size_t d, dims_) - std::cout << d << " "; - std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; -#endif } /** diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam_unstable/nonlinear/ceres_rotation.h index b02c10211..6ed3b88cb 100644 --- a/gtsam_unstable/nonlinear/ceres_rotation.h +++ b/gtsam_unstable/nonlinear/ceres_rotation.h @@ -47,6 +47,7 @@ #include #include +#include #include #define DCHECK assert diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt index 5b1fd07d4..ad14fcd9b 100644 --- a/gtsam_unstable/nonlinear/tests/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "" "gtsam_unstable") +gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable") diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp index f113a4f64..5d6ea2168 100644 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -16,22 +16,9 @@ * @brief unit tests for Basis Decompositions w Expressions */ -#include -#include -#include -#include -#include -#include +#include -#include - -#include -using boost::assign::list_of; - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); +namespace gtsam { /// Fourier template @@ -67,6 +54,80 @@ public: } }; +} + +#include +#include +#include +#include + +namespace gtsam { + +/// For now, this is our sequence representation +typedef std::map Sequence; + +/** + * Class that does Fourier Decomposition via least squares + */ +class FourierDecomposition { +public: + + typedef Vector3 Coefficients; ///< Fourier coefficients + +private: + Coefficients c_; + +public: + + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model) { + NonlinearFactorGraph graph; + Expression c(0); + typedef std::pair Sample; + BOOST_FOREACH(Sample sample, sequence) { + Expression expression(Fourier<3>(sample.first), c); + ExpressionFactor factor(model, sample.second, expression); + graph.add(factor); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence, + const SharedNoiseModel& model) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model); + Values values; + values.insert(0, Coefficients::Zero()); // does not matter + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /// Constructor + FourierDecomposition(const Sequence& sequence, + const SharedNoiseModel& model) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model); + VectorValues solution = gfg->optimize(); + c_ = solution.at(0); + } + + /// Return Fourier coefficients + Coefficients coefficients() { + return c_; + } +}; + +} + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + //****************************************************************************** TEST(BasisDecompositions, Fourier) { Fourier<3> fx(0); @@ -79,12 +140,14 @@ TEST(BasisDecompositions, Fourier) { } //****************************************************************************** -TEST(BasisDecompositions, FourierExpression) { +TEST(BasisDecompositions, ManualFourier) { // Create linear factor graph GaussianFactorGraph g; Key key(1); - Vector3_ c(key); + Expression c(key); + Values values; + values.insert(key, Vector3::Zero()); // does not matter for (size_t i = 0; i < 16; i++) { double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); @@ -93,13 +156,44 @@ TEST(BasisDecompositions, FourierExpression) { A << 1, cos(x), sin(x); Vector b(1); b << y; - JacobianFactor f1(key, A, b, model); + JacobianFactor f1(key, A, b); + g.add(f1); // With ExpressionFactor Expression expression(Fourier<3>(x), c); - ExpressionFactor f2(model, y, expression); - - g.add(f1); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9); + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } + { + ExpressionFactor f2(model, y, expression); + boost::shared_ptr gf = f2.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + CHECK(jf); + EXPECT( assert_equal(f1, *jf, 1e-9)); + } } // Solve @@ -110,6 +204,24 @@ TEST(BasisDecompositions, FourierExpression) { EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); } +//****************************************************************************** +TEST(BasisDecompositions, FourierDecomposition) { + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); + sequence[x] = y; + } + + // Do Fourier Decomposition + FourierDecomposition actual(sequence, model); + + // Check + Vector3 expected(1.5661, 1.2717, 1.2717); + EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp index a4561b349..1cc674901 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -33,7 +33,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > MaxVirtualStaticRows){ + if(i > CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -43,7 +43,6 @@ struct CallConfig { int compTimeCols; int runTimeRows; int runTimeCols; - CallConfig() {} CallConfig(int rows, int cols): compTimeRows(dynamicIfAboveMax(rows)), compTimeCols(cols), @@ -72,25 +71,26 @@ struct CallConfig { }; struct Record: public internal::CallRecordImplementor { + Record() : cc(0, 0) {} virtual ~Record() { } void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); cc.runTimeCols = dFdT.cols(); } - template - friend struct internal::ReverseADImplementor; + template + friend struct internal::CallRecordImplementor; }; JacobianMap & NJM= *static_cast(NULL); @@ -102,56 +102,56 @@ TEST(CallRecord, virtualReverseAdDispatching) { Record record; { const int Rows = 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 3; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 4; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 5; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 6; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d146ea945..09cad558a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) { // Concise version ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ @@ -124,6 +127,38 @@ TEST(ExpressionFactor, Unary) { EXPECT( assert_equal(expected, *jf, 1e-9)); } +/* ************************************************************************* */ +// Unary(Leaf)) and Unary(Unary(Leaf))) +// wide version (not handled in fixed-size pipeline) +typedef Eigen::Matrix Matrix93; +Vector9 wide(const Point3& p, boost::optional H) { + Vector9 v; + v << p.vector(), p.vector(), p.vector(); + if (H) *H << eye(3), eye(3), eye(3); + return v; +} +typedef Eigen::Matrix Matrix9; +Vector9 id9(const Vector9& v, boost::optional H) { + if (H) *H = Matrix9::Identity(); + return v; +} +TEST(ExpressionFactor, Wide) { + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + Point3_ point(2); + Vector9 measured; + Expression expression(wide,point); + SharedNoiseModel model = noiseModel::Unit::Create(9); + + ExpressionFactor f1(model, measured, expression); + EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9); + + Expression expression2(id9,expression); + ExpressionFactor f2(model, measured, expression2); + EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9); +} + /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, boost::optional Dcal, boost::optional Dp) { diff --git a/matlab/gtsam_tests/.gitignore b/matlab/gtsam_tests/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/gtsam_tests/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/matlab/gtsam_tests/testValues.m b/matlab/gtsam_tests/testValues.m new file mode 100644 index 000000000..fe2cd30fe --- /dev/null +++ b/matlab/gtsam_tests/testValues.m @@ -0,0 +1,40 @@ +% test wrapping of Values +import gtsam.*; + +values = Values; +E = EssentialMatrix(Rot3,Unit3); +tol = 1e-9; + +values.insert(0, Point2); +values.insert(1, Point3); +values.insert(2, Rot2); +values.insert(3, Pose2); +values.insert(4, Rot3); +values.insert(5, Pose3); +values.insert(6, Cal3_S2); +values.insert(7, Cal3DS2); +values.insert(8, Cal3Bundler); +values.insert(9, E); +values.insert(10, imuBias.ConstantBias); + +% special cases for Vector and Matrix: +values.insert(11, [1;2;3]); +values.insert(12, [1 2;3 4]); + +EXPECT('at',values.atPoint2(0).equals(Point2,tol)); +EXPECT('at',values.atPoint3(1).equals(Point3,tol)); +EXPECT('at',values.atRot2(2).equals(Rot2,tol)); +EXPECT('at',values.atPose2(3).equals(Pose2,tol)); +EXPECT('at',values.atRot3(4).equals(Rot3,tol)); +EXPECT('at',values.atPose3(5).equals(Pose3,tol)); +EXPECT('at',values.atCal3_S2(6).equals(Cal3_S2,tol)); +EXPECT('at',values.atCal3DS2(7).equals(Cal3DS2,tol)); +EXPECT('at',values.atCal3Bundler(8).equals(Cal3Bundler,tol)); +EXPECT('at',values.atEssentialMatrix(9).equals(E,tol)); +EXPECT('at',values.atConstantBias(10).equals(imuBias.ConstantBias,tol)); + +% special cases for Vector and Matrix: +actualVector = values.atVector(11); +EQUALITY('at',[1;2;3],actualVector,tol); +actualMatrix = values.atMatrix(12); +EQUALITY('at',[1 2;3 4],actualMatrix,tol); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e3705948d..e08019610 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,5 +1,8 @@ % Test runner script - runs each test +display 'Starting: testValues' +testValues + display 'Starting: testJacobianFactor' testJacobianFactor diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 4989afb0d..1f57917d8 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -31,12 +32,13 @@ using namespace wrap; /* ************************************************************************* */ Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { Argument instArg = *this; - instArg.type = ts(type); + instArg.type = ts.tryToSubstitite(type); return instArg; } /* ************************************************************************* */ -ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const { +ArgumentList ArgumentList::expandTemplate( + const TemplateSubstitution& ts) const { ArgumentList instArgList; BOOST_FOREACH(const Argument& arg, *this) { Argument instArg = arg.expandTemplate(ts); @@ -48,25 +50,25 @@ ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, type.namespaces) + BOOST_FOREACH(const string& ns, type.namespaces()) result += ns + delim; - if (type.name == "string" || type.name == "unsigned char" - || type.name == "char") + if (type.name() == "string" || type.name() == "unsigned char" + || type.name() == "char") return result + "char"; - if (type.name == "Vector" || type.name == "Matrix") + if (type.name() == "Vector" || type.name() == "Matrix") return result + "double"; - if (type.name == "int" || type.name == "size_t") + if (type.name() == "int" || type.name() == "size_t") return result + "numeric"; - if (type.name == "bool") + if (type.name() == "bool") return result + "logical"; - return result + type.name; + return result + type.name(); } /* ************************************************************************* */ bool Argument::isScalar() const { - return (type.name == "bool" || type.name == "char" - || type.name == "unsigned char" || type.name == "int" - || type.name == "size_t" || type.name == "double"); + return (type.name() == "bool" || type.name() == "char" + || type.name() == "unsigned char" || type.name() == "int" + || type.name() == "size_t" || type.name() == "double"); } /* ************************************************************************* */ @@ -97,6 +99,13 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << ");" << endl; } +/* ************************************************************************* */ +void Argument::proxy_check(FileWriter& proxyFile, const string& s) const { + proxyFile.oss << "isa(" << s << ",'" << matlabClass(".") << "')"; + if (type.name() == "Vector") + proxyFile.oss << " && size(" << s << ",2)==1"; +} + /* ************************************************************************* */ string ArgumentList::types() const { string str; @@ -104,7 +113,7 @@ string ArgumentList::types() const { BOOST_FOREACH(Argument arg, *this) { if (!first) str += ","; - str += arg.type.name; + str += arg.type.name(); first = false; } return str; @@ -116,14 +125,14 @@ string ArgumentList::signature() const { bool cap = false; BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type.name) + 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.name[0]; + sig += arg.type.name()[0]; //Reset to default cap = false; } @@ -170,25 +179,14 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { BOOST_FOREACH(Argument arg, *this) { if (!first) file.oss << ", "; - file.oss << arg.type.name << " " << arg.name; + file.oss << arg.type.name() << " " << arg.name; first = false; } file.oss << ")"; } + /* ************************************************************************* */ -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) - proxyFile.oss << ", this"; - proxyFile.oss << ", varargin{:});\n"; -} -/* ************************************************************************* */ -void ArgumentList::emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { +void ArgumentList::proxy_check(FileWriter& proxyFile) const { // Check nr of arguments proxyFile.oss << "if length(varargin) == " << size(); if (size() > 0) @@ -198,15 +196,12 @@ void ArgumentList::emit_conditional_call(FileWriter& proxyFile, for (size_t i = 0; i < size(); i++) { if (!first) proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << (*this)[i].matlabClass(".") << "')"; + string s = "varargin{" + boost::lexical_cast(i + 1) + "}"; + (*this)[i].proxy_check(proxyFile, s); first = false; } proxyFile.oss << "\n"; - - // output call to C++ wrapper - proxyFile.oss << " "; - emit_call(proxyFile, returnVal, wrapperName, id, staticMethod); } + /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 3d8d7288f..3b7a13ee3 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -28,13 +28,23 @@ namespace wrap { /// Argument class struct Argument { Qualified type; - bool is_const, is_ref, is_ptr; std::string name; + bool is_const, is_ref, is_ptr; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + Argument(const Qualified& t, const std::string& n) : + type(t), name(n), is_const(false), is_ref(false), is_ptr(false) { + } + + bool operator==(const Argument& other) const { + return type == other.type && name == other.name + && is_const == other.is_const && is_ref == other.is_ref + && is_ptr == other.is_ptr; + } + Argument expandTemplate(const TemplateSubstitution& ts) const; /// return MATLAB class for use in isa(x,class) @@ -46,6 +56,12 @@ struct Argument { /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; + /** + * emit checking argument to MATLAB proxy + * @param proxyFile output stream + */ + void proxy_check(FileWriter& proxyFile, const std::string& s) const; + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") << (arg.is_ref ? "&" : ""); @@ -88,26 +104,12 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit emit MATLAB call to proxy + * emit checking arguments to MATLAB 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& proxyFile, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod = false) const; - - /** - * 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& proxyFile, - const ReturnValue& returnVal, const std::string& wrapperName, int id, - bool staticMethod = false) const; + void proxy_check(FileWriter& proxyFile) const; + /// Output stream operator friend std::ostream& operator<<(std::ostream& os, const ArgumentList& argList) { os << "("; @@ -122,5 +124,87 @@ struct ArgumentList: public std::vector { }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentGrammar: public classic::grammar { + + wrap::Argument& result_; ///< successful parse will be placed in here + TypeGrammar argument_type_g; ///< Type parser for Argument::type + + /// Construct type grammar and specify where result is placed + ArgumentGrammar(wrap::Argument& result) : + result_(result), argument_type_g(result.type) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule argument_p; + + definition(ArgumentGrammar const& self) { + using namespace classic; + + // NOTE: allows for pointers to all types + // Slightly more permissive than before on basis/eigen type qualification + // Also, currently parses Point2*&, can't make it work otherwise :-( + argument_p = !str_p("const")[assign_a(self.result_.is_const, T)] // + >> self.argument_type_g // + >> !ch_p('*')[assign_a(self.result_.is_ptr, T)] + >> !ch_p('&')[assign_a(self.result_.is_ref, T)] + >> BasicRules::name_p[assign_a(self.result_.name)]; + } + + Rule const& start() const { + return argument_p; + } + + }; +}; +// ArgumentGrammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentListGrammar: public classic::grammar { + + wrap::ArgumentList& result_; ///< successful parse will be placed in here + + const Argument arg0; ///< used to reset arg + mutable Argument arg; ///< temporary argument for use during parsing + ArgumentGrammar argument_g; ///< single Argument parser + + /// Construct type grammar and specify where result is placed + ArgumentListGrammar(wrap::ArgumentList& result) : + result_(result), argument_g(arg) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule argument_p, argumentList_p; + + definition(ArgumentListGrammar const& self) { + using namespace classic; + + argument_p = self.argument_g // + [classic::push_back_a(self.result_, self.arg)] // + [assign_a(self.arg, self.arg0)]; + + argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')'; + } + + classic::rule const& start() const { + return argumentList_p; + } + + }; +}; +// ArgumentListGrammar + +/* ************************************************************************* */ + +}// \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 0e480f0fd..6e415065d 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -22,23 +22,68 @@ #include #include +#include +#include #include #include #include +#include // std::ostream_iterator //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC using namespace std; using namespace wrap; +/* ************************************************************************* */ +void Class::assignParent(const Qualified& parent) { + parentClass.reset(parent); +} + +/* ************************************************************************* */ +boost::optional Class::qualifiedParent() const { + boost::optional result = boost::none; + if (parentClass) + result = parentClass->qualifiedName("::"); + return result; +} + +/* ************************************************************************* */ +static void handleException(const out_of_range& oor, + const Class::Methods& methods) { + cerr << "Class::method: key not found: " << oor.what() << ", methods are:\n"; + using boost::adaptors::map_keys; + ostream_iterator out_it(cerr, "\n"); + boost::copy(methods | map_keys, out_it); +} + +/* ************************************************************************* */ +Method& Class::mutableMethod(Str key) { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + +/* ************************************************************************* */ +const Method& Class::method(Str key) const { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + /* ************************************************************************* */ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); + createNamespaceStructure(namespaces(), toolboxPath); // open destination classFile string classFile = matlabName(toolboxPath); @@ -48,21 +93,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, 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 - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = + parentClass ? parentClass->qualifiedName(".") : "handle"; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name() << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; // Constructor - proxyFile.oss << " function obj = " << name << "(varargin)\n"; + proxyFile.oss << " function obj = " << name() << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and // assign a pointer returned from a C++ function. In turn this MATLAB // constructor calls a special C++ function that just adds the object to @@ -75,11 +119,12 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, wrapperFile.oss << "\n"; // Regular constructors + boost::optional cppBaseName = qualifiedParent(); 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, args); + constructor.proxy_fragment(proxyFile, wrapperName, (bool) parentClass, id, + args); const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, cppBaseName, id, args); wrapperFile.oss << "\n"; @@ -89,9 +134,9 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; proxyFile.oss << " end\n"; - if (!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" - << ptr_constructor_key << "), base_ptr);\n"; + if (parentClass) + proxyFile.oss << " obj = obj@" << parentClass->qualifiedName(".") + << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; proxyFile.oss << " end\n\n"; @@ -111,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { + BOOST_FOREACH(const Methods::value_type& name_m, methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); @@ -151,7 +196,6 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string baseCppName = qualifiedParent.qualifiedName("::"); const int collectorInsertId = (int) functionNames.size(); const string collectorInsertFunctionName = matlabUniqueName @@ -188,7 +232,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } else { proxyFile.oss << " my_ptr = varargin{2};\n"; } - if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + if (!parentClass) // If this class has a base class, we'll get a base class pointer back proxyFile.oss << " "; else proxyFile.oss << " base_ptr = "; @@ -211,9 +255,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, // Add to collector wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // 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 (!qualifiedParent.empty()) { + boost::optional cppBaseName = qualifiedParent(); + if (cppBaseName) { wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + wrapperFile.oss << " typedef boost::shared_ptr<" << *cppBaseName << "> SharedBase;\n"; wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; @@ -244,10 +289,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, /* ************************************************************************* */ Class Class::expandTemplate(const TemplateSubstitution& ts) const { Class inst = *this; - inst.methods = expandMethodTemplate(methods, ts); + inst.methods_ = expandMethodTemplate(methods_, ts); inst.static_methods = expandMethodTemplate(static_methods, ts); inst.constructor = constructor.expandTemplate(ts); - inst.deconstructor.name = inst.name; + inst.deconstructor.name = inst.name(); return inst; } @@ -257,10 +302,10 @@ vector Class::expandTemplate(Str templateArg, vector result; BOOST_FOREACH(const Qualified& instName, instantiations) { Qualified expandedClass = (Qualified) (*this); - expandedClass.name += instName.name; + expandedClass.expand(instName.name()); const TemplateSubstitution ts(templateArg, instName, expandedClass); Class inst = expandTemplate(ts); - inst.name = expandedClass.name; + inst.name_ = expandedClass.name(); inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + ">"; @@ -272,50 +317,50 @@ vector Class::expandTemplate(Str templateArg, /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const vector& templateArgValues) { + const Template& tmplate) { // Check if templated - if (!templateArgName.empty() && templateArgValues.size() > 0) { + if (tmplate.valid()) { // 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); + BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) { + const TemplateSubstitution ts(tmplate.argName(), instName, *this); // 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, + 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); + methods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); } /* ************************************************************************* */ void Class::erase_serialization() { - Methods::iterator it = methods.find("serializable"); - if (it != methods.end()) { + 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); + methods_.erase(it); } - it = methods.find("serialize"); - if (it != methods.end()) { + 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); + methods_.erase(it); } } @@ -327,31 +372,31 @@ void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { // verify all of the function arguments //TODO:verifyArguments(validTypes, constructor.args_list); verifyArguments(validTypes, static_methods); - verifyArguments(validTypes, methods); + verifyArguments(validTypes, methods_); // verify function return types verifyReturnTypes(validTypes, static_methods); - verifyReturnTypes(validTypes, methods); + verifyReturnTypes(validTypes, methods_); // verify parents - if (!qualifiedParent.empty() - && find(validTypes.begin(), validTypes.end(), - qualifiedParent.qualifiedName("::")) == validTypes.end()) - throw DependencyMissing(qualifiedParent.qualifiedName("::"), - qualifiedName("::")); + boost::optional parent = qualifiedParent(); + if (parent + && find(validTypes.begin(), validTypes.end(), *parent) + == validTypes.end()) + throw DependencyMissing(*parent, qualifiedName("::")); } /* ************************************************************************* */ void Class::appendInheritedMethods(const Class& cls, const vector& classes) { - if (!cls.qualifiedParent.empty()) { + if (cls.parentClass) { // 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()); + if (parent.name() == cls.parentClass->name()) { + methods_.insert(parent.methods_.begin(), parent.methods_.end()); appendInheritedMethods(parent, classes); } } @@ -361,11 +406,11 @@ void Class::appendInheritedMethods(const Class& cls, /* ************************************************************************* */ string Class::getTypedef() const { string result; - BOOST_FOREACH(Str namesp, namespaces) { + BOOST_FOREACH(Str namesp, namespaces()) { result += ("namespace " + namesp + " { "); } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i < namespaces.size(); ++i) { + result += ("typedef " + typedefName + " " + name() + ";"); + for (size_t i = 0; i < namespaces().size(); ++i) { result += " }"; } return result; @@ -373,15 +418,15 @@ string Class::getTypedef() const { /* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; + 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"; constructor.comment_fragment(proxyFile); - if (!methods.empty()) + if (!methods_.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods) + BOOST_FOREACH(const Methods::value_type& name_m, methods_) name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) @@ -393,7 +438,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; proxyFile.oss << "%string_deserialize(string serialized) : returns " - << this->name << "\n"; + << name() << "\n"; } proxyFile.oss << "%\n"; @@ -586,12 +631,12 @@ string Class::getSerializationExport() const { /* ************************************************************************* */ void Class::python_wrapper(FileWriter& wrapperFile) const { - wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n"; - constructor.python_wrapper(wrapperFile, name); + 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); + 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 34323b797..8faf7ab77 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,6 +19,7 @@ #pragma once +#include "Template.h" #include "Constructor.h" #include "Deconstructor.h" #include "Method.h" @@ -27,6 +28,7 @@ #include #include +#include #include #include @@ -36,13 +38,20 @@ namespace wrap { /// Class has name, constructors, methods class Class: public Qualified { +public: + typedef const std::string& Str; typedef std::map Methods; - Methods methods; ///< Class methods + typedef std::map StaticMethods; + +private: + + boost::optional parentClass; ///< The *single* parent + Methods methods_; ///< Class methods + Method& mutableMethod(Str key); public: - typedef const std::string& Str; - typedef std::map StaticMethods; + StaticMethods static_methods; ///< Static methods // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments @@ -50,26 +59,28 @@ public: 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) { + parentClass(boost::none), isVirtual(false), isSerializable(false), hasSerialization( + false), deconstructor(verbose), verbose_(verbose) { } + void assignParent(const Qualified& parent); + + boost::optional qualifiedParent() const; + size_t nrMethods() const { - return methods.size(); - } - Method& method(Str name) { - return methods.at(name); + return methods_.size(); } + + const Method& method(Str key) const; + bool exists(Str name) const { - return methods.find(name) != methods.end(); + return methods_.find(name) != methods_.end(); } // And finally MATLAB code is emitted, methods below called by Module::matlab_code @@ -85,7 +96,7 @@ public: /// 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); + const Template& tmplate); /// Post-process classes for serialization markers void erase_serialization(); // non-const ! @@ -115,11 +126,11 @@ public: void python_wrapper(FileWriter& wrapperFile) const; friend std::ostream& operator<<(std::ostream& os, const Class& cls) { - os << "class " << cls.name << "{\n"; + 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) + BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values) os << m << ";\n"; os << "};" << std::endl; return os; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 782c0ca80..35cc0fa53 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -69,7 +69,7 @@ void Constructor::proxy_fragment(FileWriter& file, /* ************************************************************************* */ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const { const string wrapFunctionName = matlabUniqueName + "_constructor_" @@ -100,9 +100,9 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, << 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) { file.oss << "\n"; - file.oss << " typedef boost::shared_ptr<" << cppBaseClassName + file.oss << " typedef boost::shared_ptr<" << *cppBaseClassName << "> SharedBase;\n"; file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index a026bf423..1e061d17c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -19,7 +19,7 @@ #pragma once #include "OverloadedFunction.h" - +#include #include #include @@ -68,7 +68,7 @@ struct Constructor: public OverloadedFunction { /// cpp wrapper std::string wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const; /// constructor function diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 5506dd778..5ec022ca4 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -27,6 +27,7 @@ namespace wrap { std::string name; bool isVirtual; ForwardDeclaration() : isVirtual(false) {} + ForwardDeclaration(const std::string& s) : name(s), isVirtual(false) {} }; -} \ No newline at end of file +} diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index ac22ec3a8..30e27764b 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -109,8 +109,8 @@ 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) { + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false) { bool first = initializeOrCheck(name, instName, verbose); SignatureOverloads::push_back(args, retVal); return first; diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 6faa70fb9..7e4ded040 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -29,12 +29,11 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -bool Function::initializeOrCheck(const std::string& name, - const Qualified& instName, bool verbose) { +bool Function::initializeOrCheck(const string& name, + boost::optional instName, bool verbose) { if (name.empty()) - throw std::runtime_error( - "Function::initializeOrCheck called with empty name"); + throw runtime_error("Function::initializeOrCheck called with empty name"); // Check if this overload is give to the correct method if (name_.empty()) { @@ -43,15 +42,38 @@ bool Function::initializeOrCheck(const std::string& name, 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(":")); + if (name_ != name || verbose_ != verbose + || ((bool) templateArgValue_ != (bool) instName) + || ((bool) templateArgValue_ && (bool) instName + && !(*templateArgValue_ == *instName))) + throw runtime_error( + "Function::initializeOrCheck called with different arguments"); return false; } } /* ************************************************************************* */ +void Function::emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const string& wrapperName, int id) const { + returnVal.emit_matlab(proxyFile); + proxyFile.oss << wrapperName << "(" << id; + if (!isStatic()) + proxyFile.oss << ", this"; + proxyFile.oss << ", varargin{:});\n"; +} + +/* ************************************************************************* */ +void Function::emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const string& wrapperName, int id) const { + + // Check all arguments + args.proxy_check(proxyFile); + + // output call to C++ wrapper + proxyFile.oss << " "; + emit_call(proxyFile, returnVal, wrapperName, id); +} + +/* ************************************************************************* */ diff --git a/wrap/Function.h b/wrap/Function.h index 49a26bd8d..ad57a28c8 100644 --- a/wrap/Function.h +++ b/wrap/Function.h @@ -19,6 +19,7 @@ #pragma once #include "Argument.h" +#include namespace wrap { @@ -28,7 +29,7 @@ class Function { protected: std::string name_; ///< name of method - Qualified templateArgValue_; ///< value of template argument if applicable + boost::optional templateArgValue_; ///< value of template argument if applicable bool verbose_; public: @@ -37,19 +38,35 @@ 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); + bool initializeOrCheck(const std::string& name, + boost::optional instName = boost::none, bool verbose = + false); std::string name() const { return name_; } - std::string matlabName() const { - if (templateArgValue_.empty()) - return name_; - else - return name_ + templateArgValue_.name; + /// Only Methods are non-static + virtual bool isStatic() const { + return true; } + + std::string matlabName() const { + if (templateArgValue_) + return name_ + templateArgValue_->name(); + else + return name_; + } + + /// Emit function call to MATLAB (no argument checking) + void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const std::string& wrapperName, int id) const; + + /// Emit checking arguments and function call to MATLAB + void emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const std::string& wrapperName, int id) const; + }; } // \namespace wrap diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index e843481a1..013ef6d28 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -18,9 +18,9 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const Qualified& instName, bool verbose) { - string name(overload.name); - FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose); + boost::optional instName, bool verbose) { + FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName, + verbose); overloads.push_back(overload); } @@ -37,7 +37,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath, 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); + string str_ns = qualifiedName("", overload.namespaces()); const ReturnValue& ret = returnValue(i); const ArgumentList& args = argumentList(i); grouped_functions[str_ns].addOverload(overload, args, ret); @@ -59,7 +59,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // create the folder for the namespace const Qualified& overload1 = overloads.front(); - createNamespaceStructure(overload1.namespaces, toolboxPath); + createNamespaceStructure(overload1.namespaces(), toolboxPath); // open destination mfunctionFileName string mfunctionFileName = overload1.matlabName(toolboxPath); @@ -80,7 +80,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // Output proxy matlab code mfunctionFile.oss << " " << (i == 0 ? "" : "else"); - args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this" + emit_conditional_call(mfunctionFile, returnVal, args, wrapperName, id); // Output C++ wrapper code @@ -104,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const 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.name != "void") + if (returnVal.type1.name() != "void") returnVal.wrap_result(cppName + "(" + args.names() + ")", file, typeAttributes); else diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 6a49fe5ce..4805231fb 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -19,8 +19,8 @@ struct GlobalFunction: public FullyOverloadedFunction { // 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); + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false); void verifyArguments(const std::vector& validArgs) const { SignatureOverloads::verifyArguments(validArgs, name_); diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 49d90378d..2ad6e8259 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -30,9 +30,9 @@ using namespace wrap; /* ************************************************************************* */ 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); + const ReturnValue& retVal, bool is_const, + boost::optional instName, bool verbose) { + bool first = MethodBase::addOverload(name, args, retVal, instName, verbose); if (first) is_const_ = is_const; else if (is_const && !is_const_) @@ -52,14 +52,12 @@ void Method::proxy_header(FileWriter& proxyFile) const { /* ************************************************************************* */ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1," - << args.size() << ");\n"; + wrapperFile.oss << " checkArguments(\"" << matlabName() + << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); @@ -72,8 +70,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(obj->return_field(t)); string expanded = "obj->" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } diff --git a/wrap/Method.h b/wrap/Method.h index db9e6bb9f..33ff7072e 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,12 +18,12 @@ #pragma once -#include "StaticMethod.h" +#include "MethodBase.h" namespace wrap { /// Method class -class Method: public StaticMethod { +class Method: public MethodBase { bool is_const_; @@ -32,8 +32,9 @@ public: 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); + const ReturnValue& retVal, bool is_const, + boost::optional instName = boost::none, bool verbose = + false); virtual bool isStatic() const { return false; @@ -55,9 +56,7 @@ private: 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; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp new file mode 100644 index 000000000..9b5f7135f --- /dev/null +++ b/wrap/MethodBase.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 MethodBase.ccp + * @author Frank Dellaert + * @author Andrew Melim + * @author Richard Roberts + **/ + +#include "Method.h" +#include "utilities.h" + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, + Str matlabUniqueName, Str wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { + + // emit header, e.g., function varargout = templatedMethod(this, varargin) + proxy_header(proxyFile); + + // Emit comments for documentation + string up_name = boost::to_upper_copy(matlabName()); + proxyFile.oss << " % " << up_name << " usage: "; + usage_fragment(proxyFile, matlabName()); + + // 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; + + // Handle special case of single overload with all numeric arguments + if (nrOverloads() == 1 && argumentList(0).allScalar()) { + // Output proxy matlab code + // TODO: document why is it OK to not check arguments in this case + proxyFile.oss << " "; + const int id = (int) functionNames.size(); + emit_call(proxyFile, returnValue(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 i = 0; i < nrOverloads(); ++i) { + + // Output proxy matlab code + proxyFile.oss << " " << (i == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + emit_conditional_call(proxyFile, returnValue(i), argumentList(i), + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, i, id, typeAttributes); + + // 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 MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { + + // generate code + + const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" + + boost::lexical_cast(id); + + const ArgumentList& args = argumentList(overload); + const ReturnValue& returnVal = returnValue(overload); + + // call + wrapperFile.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + wrapperFile.oss << "{\n"; + + returnVal.wrapTypeUnwrap(wrapperFile); + + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; + + // get call + // for static methods: cppClassName::staticMethod + // for instance methods: obj->instanceMethod + string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, + args); + + expanded += ("(" + args.names() + ")"); + if (returnVal.type1.name() != "void") + returnVal.wrap_result(expanded, wrapperFile, typeAttributes); + else + wrapperFile.oss << " " + expanded + ";\n"; + + // finish + wrapperFile.oss << "}\n"; + + return wrapFunctionName; +} + +/* ************************************************************************* */ +void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const { + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" + << name_ << ");\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/MethodBase.h b/wrap/MethodBase.h new file mode 100644 index 000000000..903b89569 --- /dev/null +++ b/wrap/MethodBase.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 MethodBase.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + * @author Richard Roberts + **/ + +#pragma once + +#include "FullyOverloadedFunction.h" + +namespace wrap { + +/// MethodBase class +struct MethodBase: public FullyOverloadedFunction { + + typedef const std::string& Str; + + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile) const { + SignatureOverloads::comment_fragment(proxyFile, matlabName()); + } + + 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, + Str cppClassName, Str matlabQualName, Str matlabUniqueName, + Str wrapperName, const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; + + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile, Str className) const; + +protected: + + virtual void proxy_header(FileWriter& proxyFile) const = 0; + + std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + + virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args) const = 0; +}; + +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index ac0e0a579..917130f5c 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -23,11 +23,8 @@ #include "utilities.h" //#define BOOST_SPIRIT_DEBUG -#include "spirit_actors.h" +#include "spirit.h" -#include -#include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -51,8 +48,6 @@ using namespace BOOST_SPIRIT_CLASSIC_NS; namespace bl = boost::lambda; namespace fs = boost::filesystem; -typedef rule Rule; - /* ************************************************************************* */ // We parse an interface file into a Module object. // The grammar is defined using the boost/spirit combinatorial parser. @@ -102,7 +97,6 @@ Module::Module(const string& interfacePath, void Module::parseMarkup(const std::string& data) { // The parse imperatively :-( updates variables gradually during parse // The one with postfix 0 are used to reset the variables after parse. - vector namespaces; // current namespace tag //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -113,208 +107,129 @@ void Module::parseMarkup(const std::string& data) { // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html // ---------------------------------------------------------------------------- - Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); - - Rule basisType_p = - (str_p("string") | "bool" | "size_t" | "int" | "double" | "char" | "unsigned char"); - - Rule keywords_p = - (str_p("const") | "static" | "namespace" | "void" | basisType_p); - - Rule eigenType_p = - (str_p("Vector") | "Matrix"); - - //Rule for STL Containers (class names are lowercase) - Rule stlType_p = (str_p("vector") | "list"); - - Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p) | stlType_p; - - Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_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.name)]; - - Rule eigenRef_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - 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.name)] >> - !ch_p('&')[assign_a(arg.is_ref,true)]; - - Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; + // Define Rule and instantiate basic rules + typedef rule Rule; + BasicRules basic; // 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.namespaces)] >> str_p("::")) >> - className_p[assign_a(cls.qualifiedParent.name)]; - - // 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)]; - - // template - string templateArgName; - Rule templateArgValues_p = - (str_p("template") >> - '<' >> 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(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(singleInstantiation.typeList, templateArgValue)] - [clear_a(templateArgValue)]; + TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; + TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); // typedef gtsam::RangeFactor RangeFactorPosePoint2; - TemplateInstantiationTypedef singleInstantiation0; + vector namespaces; // current namespace tag + TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = - (str_p("typedef") >> - *(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)] >> + (str_p("typedef") >> instantiationClass_g >> + typelist_g >> + basic.className_p[assign_a(singleInstantiation.name_)] >> ';') - [assign_a(singleInstantiation.namespaces, namespaces)] + [assign_a(singleInstantiation.namespaces_, namespaces)] [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)]) >> + '<' >> basic.name_p[push_back_a(cls.templateArgs)] >> *(',' >> basic.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.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); + ArgumentListGrammar argumentList_g(args); // parse class constructor Constructor constructor0(verbose), constructor(verbose); Rule constructor_p = - (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) + (basic.className_p >> argumentList_g >> ';' >> !basic.comments_p) [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("::"); + Rule namespace_ret_p = basic.namespace_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 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; - - 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)]); - 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)]; + ReturnValueGrammar returnValue_g(retVal); - 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.name)][assign_a(retVal.type1.category, RETURN_VOID)]; - - Rule returnValue_p = void_p | pair_p | returnType1_p; - Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + // template + Template methodTemplate; + TemplateGrammar methodTemplate_g(methodTemplate); + // gtsam::Values retract(const gtsam::VectorValues& delta) const; string methodName; bool isConst, isConst0 = false; Rule method_p = - !templateArgValues_p >> - (returnValue_p >> methodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) + !methodTemplate_g >> + (returnValue_g >> methodName_p[assign_a(methodName)] >> + argumentList_g >> + !str_p("const")[assign_a(isConst,true)] >> ';' >> *basic.comments_p) [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))] + bl::var(methodTemplate))] [assign_a(retVal,retVal0)] [clear_a(args)] - [clear_a(templateArgValues)] + [clear_a(methodTemplate)] [assign_a(isConst,isConst0)]; Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; Rule static_method_p = - (str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) + (str_p("static") >> returnValue_g >> staticMethodName_p[assign_a(methodName)] >> + argumentList_g >> ';' >> *basic.comments_p) [bl::bind(&StaticMethod::addOverload, bl::var(cls.static_methods)[bl::var(methodName)], - bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)] + bl::var(methodName), bl::var(args), bl::var(retVal), boost::none,verbose)] [assign_a(retVal,retVal0)] [clear_a(args)]; Rule functions_p = constructor_p | method_p | static_method_p; + // template + Template classTemplate; + TemplateGrammar classTemplate_g(classTemplate); + + // Parent class + Qualified possibleParent; + TypeGrammar classParent_p(possibleParent); + // parse a full class - vector templateInstantiations; Rule class_p = eps_p[assign_a(cls,cls0)] - >> (!(templateArgValues_p - [push_back_a(cls.templateArgs, templateArgName)] - [assign_a(templateInstantiations,templateArgValues)] - [clear_a(templateArgValues)] + >> (!(classTemplate_g + [push_back_a(cls.templateArgs, classTemplate.argName())] | 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) + >> basic.className_p[assign_a(cls.name_)] + >> ((':' >> classParent_p >> '{') + [bl::bind(&Class::assignParent, bl::var(cls), bl::var(possibleParent))] + [clear_a(possibleParent)] | '{') + >> *(functions_p | basic.comments_p) >> str_p("};")) [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), - bl::var(cls.name), Qualified(), verbose)] + bl::var(cls.name_), boost::none, verbose)] [assign_a(cls.constructor, constructor)] - [assign_a(cls.namespaces, namespaces)] - [assign_a(cls.deconstructor.name,cls.name)] + [assign_a(cls.namespaces_, namespaces)] + [assign_a(cls.deconstructor.name,cls.name_)] [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), - bl::var(templateInstantiations))] - [clear_a(templateInstantiations)] + bl::var(classTemplate.argValues()))] + [clear_a(classTemplate)] [assign_a(constructor, constructor0)] [assign_a(cls,cls0)]; // parse a global function Qualified globalFunction; Rule global_function_p = - (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [assign_a(globalFunction.namespaces,namespaces)] + (returnValue_g >> staticMethodName_p[assign_a(globalFunction.name_)] >> + argumentList_g >> ';' >> *basic.comments_p) + [assign_a(globalFunction.namespaces_,namespaces)] [bl::bind(&GlobalFunction::addOverload, - bl::var(global_functions)[bl::var(globalFunction.name)], - bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)] + bl::var(global_functions)[bl::var(globalFunction.name_)], + bl::var(globalFunction), bl::var(args), bl::var(retVal), boost::none,verbose)] [assign_a(retVal,retVal0)] [clear_a(globalFunction)] [clear_a(args)]; @@ -328,9 +243,9 @@ void Module::parseMarkup(const std::string& data) { Rule namespace_def_p = (str_p("namespace") - >> namespace_name_p[push_back_a(namespaces)] + >> basic.namespace_p[push_back_a(namespaces)] >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | basic.comments_p) >> ch_p('}')) [pop_a(namespaces)]; @@ -343,12 +258,14 @@ void Module::parseMarkup(const std::string& data) { Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") - >> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)] + >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') [push_back_a(forward_declarations, fwDec)] [assign_a(fwDec, fwDec0)]; - Rule module_content_p = comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p | global_function_p | namespace_def_p; + Rule module_content_p = basic.comments_p | include_p | class_p + | templateSingleInstantiation_p | forward_declaration_p + | global_function_p | namespace_def_p; Rule module_p = *module_content_p >> !end_p; @@ -361,13 +278,13 @@ void Module::parseMarkup(const std::string& data) { BOOST_SPIRIT_DEBUG_NODE(basisType_p); BOOST_SPIRIT_DEBUG_NODE(name_p); BOOST_SPIRIT_DEBUG_NODE(argument_p); - BOOST_SPIRIT_DEBUG_NODE(argumentList_p); + BOOST_SPIRIT_DEBUG_NODE(argumentList_g); BOOST_SPIRIT_DEBUG_NODE(constructor_p); BOOST_SPIRIT_DEBUG_NODE(returnType1_p); BOOST_SPIRIT_DEBUG_NODE(returnType2_p); BOOST_SPIRIT_DEBUG_NODE(pair_p); BOOST_SPIRIT_DEBUG_NODE(void_p); - BOOST_SPIRIT_DEBUG_NODE(returnValue_p); + BOOST_SPIRIT_DEBUG_NODE(returnValue_g); BOOST_SPIRIT_DEBUG_NODE(methodName_p); BOOST_SPIRIT_DEBUG_NODE(method_p); BOOST_SPIRIT_DEBUG_NODE(class_p); @@ -381,9 +298,8 @@ void Module::parseMarkup(const std::string& data) { if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); cout << "Stopped near:\n" - "class '" << cls.name << "'\n" - "method '" << methodName << "'\n" - "argument '" << arg.name << "'" << endl; + "class '" << cls.name_ << "'\n" + "method '" << methodName << "'" << endl; throw ParseFailed((int)info.length); } @@ -416,6 +332,11 @@ void Module::parseMarkup(const std::string& data) { // Create type attributes table and check validity typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); + // add Eigen types as template arguments are also checked ? + vector eigen; + eigen.push_back(ForwardDeclaration("Vector")); + eigen.push_back(ForwardDeclaration("Matrix")); + typeAttributes.addForwardDeclarations(eigen); typeAttributes.checkValidity(expandedClasses); } @@ -546,7 +467,7 @@ vector Module::ExpandTypedefInstantiations(const vector& classes, vector Module::GenerateValidTypes(const vector& classes, const vector forwardDeclarations) { vector validTypes; BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDeclarations) { - validTypes.push_back(fwDec.name); + validTypes.push_back(fwDec.name); } validTypes.push_back("void"); validTypes.push_back("string"); diff --git a/wrap/Module.h b/wrap/Module.h index a4659dc3a..e99e77bc9 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,15 +18,15 @@ #pragma once -#include -#include -#include - #include "Class.h" #include "GlobalFunction.h" #include "TemplateInstantiationTypedef.h" #include "ForwardDeclaration.h" +#include +#include +#include + namespace wrap { /** @@ -35,7 +35,6 @@ namespace wrap { struct Module { typedef std::map GlobalFunctions; - typedef std::map Methods; // Filled during parsing: std::string name; ///< module name diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 47c418748..7718bc139 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -87,7 +87,8 @@ class OverloadedFunction: public Function, public ArgumentOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const Qualified& instName = Qualified(), bool verbose = false) { + boost::optional instName = boost::none, bool verbose = + false) { bool first = initializeOrCheck(name, instName, verbose); ArgumentOverloads::push_back(args); return first; diff --git a/wrap/Qualified.h b/wrap/Qualified.h index b23e9020d..000d749ec 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,43 +27,122 @@ namespace wrap { /** * Class to encapuslate a qualified name, i.e., with (nested) namespaces */ -struct Qualified { +class Qualified { - std::vector namespaces; ///< Stack of namespaces - std::string name; ///< type name +//protected: +public: - Qualified(const std::string& name_ = "") : - name(name_) { + std::vector namespaces_; ///< Stack of namespaces + std::string name_; ///< type name + + friend struct TypeGrammar; + friend class TemplateSubstitution; + +public: + + /// the different categories + typedef enum { + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 + } Category; + Category category; + + Qualified() : + category(VOID) { + } + + Qualified(const std::string& n, Category c = CLASS) : + name_(n), category(c) { + } + + Qualified(const std::string& ns1, const std::string& ns2, + const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + namespaces_.push_back(ns2); + } + + Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + } + + Qualified(std::vector ns, const std::string& name) : + namespaces_(ns), name_(name), category(CLASS) { + } + + std::string name() const { + return name_; + } + + std::vector namespaces() const { + return namespaces_; + } + + // Qualified is 'abused' as template argument name as well + // this function checks whether *this matches with templateArg + bool match(const std::string& templateArg) const { + return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS); + } + + void rename(const Qualified& q) { + namespaces_ = q.namespaces_; + name_ = q.name_; + category = q.category; + } + + void expand(const std::string& expansion) { + name_ += expansion; + } + + bool operator==(const Qualified& other) const { + return namespaces_ == other.namespaces_ && name_ == other.name_ + && category == other.category; } bool empty() const { - return namespaces.empty() && name.empty(); + return namespaces_.empty() && name_.empty(); } - void clear() { - namespaces.clear(); - name.clear(); + virtual void clear() { + namespaces_.clear(); + name_.clear(); + category = VOID; } - bool operator!=(const Qualified& other) const { - return other.name != name || other.namespaces != namespaces; +public: + + static Qualified MakeClass(std::vector namespaces, + const std::string& name) { + return Qualified(namespaces, name); + } + + static Qualified MakeEigen(const std::string& name) { + return Qualified(name, EIGEN); + } + + static Qualified MakeBasis(const std::string& name) { + return Qualified(name, BASIS); + } + + static Qualified MakeVoid() { + return Qualified("void", VOID); } /// 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; + 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"; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += ("/+" + namespaces_[i]); + result += "/" + name_ + ".m"; return result; } @@ -73,5 +153,108 @@ struct Qualified { }; +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TypeGrammar: classic::grammar { + + wrap::Qualified& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeGrammar(wrap::Qualified& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule void_p, basisType_p, eigenType_p, namespace_del_p, class_p, type_p; + + definition(TypeGrammar const& self) { + + using namespace wrap; + using namespace classic; + typedef BasicRules Basic; + + // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish + static const Qualified::Category EIGEN = Qualified::EIGEN; + static const Qualified::Category BASIS = Qualified::BASIS; + static const Qualified::Category CLASS = Qualified::CLASS; + static const Qualified::Category VOID = Qualified::VOID; + + void_p = str_p("void") // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, VOID)]; + + basisType_p = Basic::basisType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, BASIS)]; + + eigenType_p = Basic::eigenType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, EIGEN)]; + + namespace_del_p = Basic::namespace_p // + [push_back_a(self.result_.namespaces_)] >> str_p("::"); + + class_p = *namespace_del_p >> Basic::className_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, CLASS)]; + + type_p = void_p | basisType_p | class_p | eigenType_p; + } + + Rule const& start() const { + return type_p; + } + + }; +}; +// type_grammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +template +struct TypeListGrammar: public classic::grammar > { + + typedef std::vector TypeList; + TypeList& result_; ///< successful parse will be placed in here + + mutable wrap::Qualified type; // temporary type for use during parsing + TypeGrammar type_g; + + /// Construct type grammar and specify where result is placed + TypeListGrammar(TypeList& result) : + result_(result), type_g(type) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule type_p, typeList_p; + + definition(TypeListGrammar const& self) { + using namespace classic; + type_p = self.type_g // + [push_back_a(self.result_, self.type)] // + [clear_a(self.type)]; + typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE; + } + + classic::rule const& start() const { + return typeList_p; + } + + }; +}; +// TypeListGrammar + +/* ************************************************************************* */ +// Needed for other parsers in Argument.h and ReturnType.h +static const bool T = true; + } // \namespace wrap diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index a317a5420..41fd51680 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -14,7 +14,7 @@ using namespace wrap; /* ************************************************************************* */ string ReturnType::str(bool add_ptr) const { - return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name); + return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name()); } /* ************************************************************************* */ @@ -24,37 +24,46 @@ void ReturnType::wrap_result(const string& out, const string& result, string cppType = qualifiedName("::"), matlabType = qualifiedName("."); if (category == CLASS) { + + // Handle Classes string objCopy, ptrType; - ptrType = "Shared" + name; - const bool isVirtual = typeAttributes.at(cppType).isVirtual; - if (isVirtual) { - if (isPtr) - objCopy = result; - else + const bool isVirtual = typeAttributes.attributes(cppType).isVirtual; + if (isPtr) + objCopy = result; // a shared pointer can always be passed as is + else { + // but if we want an actual new object, things get more complex + if (isVirtual) + // A virtual class needs to be cloned, so the whole hierarchy is returned objCopy = result + ".clone()"; - } else { - if (isPtr) - objCopy = result; else - objCopy = ptrType + "(new " + cppType + "(" + result + "))"; + // ...but a non-virtual class can just be copied + objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))"; } + // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); 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; + + // Handle shared pointer case for BASIS/EIGEN/VOID + wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" << name() + << "(" << result << ");" << endl; wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType - << "\");\n"; + << "\");\n }\n"; + } else if (matlabType != "void") + + // Handle normal case case for BASIS/EIGEN 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; + << "> Shared" << name() << ";" << endl; } /* ************************************************************************* */ diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index 1829fbc81..1c67a1d9a 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -9,6 +9,7 @@ #include "FileWriter.h" #include "TypeAttributesTable.h" #include "utilities.h" +#include #pragma once @@ -17,28 +18,25 @@ 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; +struct ReturnType: public Qualified { bool isPtr; - return_category category; + friend struct ReturnValueGrammar; + + /// Makes a void type ReturnType() : - isPtr(false), category(CLASS) { + isPtr(false) { } - ReturnType(const std::string& name) : - isPtr(false), category(CLASS) { - Qualified::name = name; + /// Constructor, no namespaces + ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) : + Qualified(name, c), isPtr(ptr) { } - void rename(const Qualified& q) { - name = q.name; - namespaces = q.namespaces; + virtual void clear() { + Qualified::clear(); + isPtr = false; } /// Check if this type is in a set of valid types @@ -64,4 +62,36 @@ private: }; +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnTypeGrammar: public classic::grammar { + + wrap::ReturnType& result_; ///< successful parse will be placed in here + + TypeGrammar type_g; + + /// Construct ReturnType grammar and specify where result is placed + ReturnTypeGrammar(wrap::ReturnType& result) : + result_(result), type_g(result_) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule type_p; + + definition(ReturnTypeGrammar const& self) { + using namespace classic; + type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)]; + } + + classic::rule const& start() const { + return type_p; + } + + }; +}; +// ReturnTypeGrammar + } // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 54e585cad..596acb2c3 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -17,9 +17,9 @@ using namespace wrap; /* ************************************************************************* */ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { ReturnValue instRetVal = *this; - instRetVal.type1 = ts(type1); + instRetVal.type1 = ts.tryToSubstitite(type1); if (isPair) - instRetVal.type2 = ts(type2); + instRetVal.type2 = ts.tryToSubstitite(type2); return instRetVal; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index abfcec2b0..629684a34 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -25,6 +25,8 @@ struct ReturnValue { bool isPair; ReturnType type1, type2; + friend struct ReturnValueGrammar; + /// Constructor ReturnValue() : isPair(false) { @@ -35,6 +37,22 @@ struct ReturnValue { isPair(false), type1(type) { } + /// Constructor + ReturnValue(const ReturnType& t1, const ReturnType& t2) : + isPair(true), type1(t1), type2(t2) { + } + + virtual void clear() { + type1.clear(); + type2.clear(); + isPair = false; + } + + bool operator==(const ReturnValue& other) const { + return isPair == other.isPair && type1 == other.type1 + && type2 == other.type2; + } + /// Substitute template argument ReturnValue expandTemplate(const TemplateSubstitution& ts) const; @@ -59,4 +77,39 @@ struct ReturnValue { }; -} // \namespace wrap +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnValueGrammar: public classic::grammar { + + wrap::ReturnValue& result_; ///< successful parse will be placed in here + ReturnTypeGrammar returnType1_g, returnType2_g; ///< Type parsers + + /// Construct type grammar and specify where result is placed + ReturnValueGrammar(wrap::ReturnValue& result) : + result_(result), returnType1_g(result.type1), returnType2_g(result.type2) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule pair_p, returnValue_p; + + definition(ReturnValueGrammar const& self) { + using namespace classic; + + pair_p = (str_p("pair") >> '<' >> self.returnType1_g >> ',' + >> self.returnType2_g >> '>')[assign_a(self.result_.isPair, T)]; + + returnValue_p = pair_p | self.returnType1_g; + } + + classic::rule const& start() const { + return returnValue_p; + } + + }; +}; +// ReturnValueGrammar + +}// \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 5f91a15b4..e4e7c89ae 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,7 +16,7 @@ * @author Richard Roberts **/ -#include "Method.h" +#include "StaticMethod.h" #include "utilities.h" #include @@ -36,117 +36,9 @@ void StaticMethod::proxy_header(FileWriter& proxyFile) const { proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; } -/* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, - Str matlabUniqueName, Str wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { - - // emit header, e.g., function varargout = templatedMethod(this, varargin) - proxy_header(proxyFile); - - // Emit comments for documentation - string up_name = boost::to_upper_copy(matlabName()); - proxyFile.oss << " % " << up_name << " usage: "; - usage_fragment(proxyFile, matlabName()); - - // 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; - - // Handle special case of single overload with all numeric arguments - if (nrOverloads() == 1 && argumentList(0).allScalar()) { - // Output proxy matlab code - // TODO: document why is it OK to not check arguments in this case - proxyFile.oss << " "; - const int id = (int) functionNames.size(); - argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id, - isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, 0, id, typeAttributes, templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } else { - // Check arguments for all overloads - for (size_t i = 0; i < nrOverloads(); ++i) { - - // 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& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { - - // generate code - - const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" - + boost::lexical_cast(id); - - const ArgumentList& args = argumentList(overload); - const ReturnValue& returnVal = returnValue(overload); - - // call - wrapperFile.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - // start - wrapperFile.oss << "{\n"; - - returnVal.wrapTypeUnwrap(wrapperFile); - - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; - - // get call - // for static methods: cppClassName::staticMethod - // for instance methods: obj->instanceMethod - string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, - args, returnVal, typeAttributes, instName); - - expanded += ("(" + args.names() + ")"); - if (returnVal.type1.name != "void") - returnVal.wrap_result(expanded, wrapperFile, typeAttributes); - else - wrapperFile.oss << " " + expanded + ";\n"; - - // finish - 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 { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // NOTE: for static functions, there is no object passed wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_ @@ -158,17 +50,10 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(staticMethod(t)); string expanded = cppClassName + "::" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->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 de8e4a94e..a01eeff62 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,42 +19,15 @@ #pragma once -#include "FullyOverloadedFunction.h" +#include "MethodBase.h" namespace wrap { /// StaticMethod class -struct StaticMethod: public FullyOverloadedFunction { +struct StaticMethod: public MethodBase { typedef const std::string& Str; - virtual bool isStatic() const { - return true; - } - - // emit a list of comments, one for each overload - void comment_fragment(FileWriter& proxyFile) const { - SignatureOverloads::comment_fragment(proxyFile, matlabName()); - } - - 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, - Str cppClassName, Str matlabQualName, Str matlabUniqueName, - Str wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; - - // 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]; @@ -65,15 +38,8 @@ 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; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/Template.h b/wrap/Template.h new file mode 100644 index 000000000..5a64412ed --- /dev/null +++ b/wrap/Template.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 Template.h + * @brief Template name + * @author Frank Dellaert + * @date Nov 11, 2014 + **/ + +#pragma once + +#include + +namespace wrap { + +/// The template specification that goes before a method or a class +class Template { + std::string argName_; + std::vector argValues_; + friend struct TemplateGrammar; +public: + /// The only way to get values into a Template is via our friendly Grammar + Template() { + } + void clear() { + argName_.clear(); + argValues_.clear(); + } + const std::string& argName() const { + return argName_; + } + const std::vector& argValues() const { + return argValues_; + } + size_t nrValues() const { + return argValues_.size(); + } + const Qualified& operator[](size_t i) const { + return argValues_[i]; + } + bool valid() const { + return !argName_.empty() && argValues_.size() > 0; + } + +}; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TemplateGrammar: public classic::grammar { + + Template& result_; ///< successful parse will be placed in here + TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser + + /// Construct type grammar and specify where result is placed + TemplateGrammar(Template& result) : + result_(result), argValues_g(result.argValues_) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + classic::rule templateArgValues_p; + + definition(TemplateGrammar const& self) { + using classic::str_p; + using classic::assign_a; + templateArgValues_p = (str_p("template") >> '<' + >> (BasicRules::name_p)[assign_a(self.result_.argName_)] + >> '=' >> self.argValues_g >> '>'); + } + + classic::rule const& start() const { + return templateArgValues_p; + } + + }; +}; +// TemplateGrammar + +/// Cool initializer for tests +static inline boost::optional