From 83ccc6211e5efb3e0d6cddf083250b266bb97cd2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 12 Nov 2011 21:18:44 +0000 Subject: [PATCH 01/72] Fixed Data subdirectory with wrong path for make dist --- examples/Makefile.am | 5 +++++ examples/vSLAMexample/Makefile.am | 4 ---- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/Makefile.am b/examples/Makefile.am index ee8f0db18..14b6fb935 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -21,6 +21,11 @@ noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM examp noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same noinst_PROGRAMS += CameraResectioning + +EXTRA_DIST = Data +dist-hook: + rm -rf $(distdir)/Data/.svn + SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am index f4056bd99..6a8616cc9 100644 --- a/examples/vSLAMexample/Makefile.am +++ b/examples/vSLAMexample/Makefile.am @@ -21,10 +21,6 @@ headers += Feature2D.h vSLAMutils.h noinst_HEADERS = $(headers) -EXTRA_DIST = Data -dist-hook: - rm -rf $(distdir)/Data/.svn - #---------------------------------------------------------------------------------------------------- # rules to build local programs #---------------------------------------------------------------------------------------------------- From 8cf2b84f5aedb1ec16d8387b87159268b53fc1e6 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 14 Nov 2011 17:57:12 +0000 Subject: [PATCH 02/72] Lie/Manifold documentation --- gtsam/base/Group.h | 5 +- gtsam/base/Lie.h | 202 +++++++++++++++++++++--------------------- gtsam/base/Manifold.h | 106 ++++++++++++---------- 3 files changed, 165 insertions(+), 148 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 3c56a541a..533c042c0 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -2,8 +2,6 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * A Group concept extends a Manifold - * * @date Nov 5, 2011 * @author Alex Cunningham */ @@ -13,7 +11,8 @@ namespace gtsam { /** - * Concept check for general Group structure + * This concept check enforces a Group structure on a variable type, + * in which we require the existence of basic algebraic operations. */ template class GroupConcept { diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 756790f16..932e069ef 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -14,28 +14,6 @@ * @brief Base class and basic functions for Lie types * @author Richard Roberts * @author Alex Cunningham - * - * This concept check provides a specialization on the Manifold type, - * in which the Manifolds represented require an algebra and group structure. - * All Lie types must also be a Manifold. - * - * The necessary functions to implement for Lie are defined - * below with additional details as to the interface. The - * concept checking function in class Lie will check whether or not - * the function exists and throw compile-time errors. - * - * Expmap around identity - * static T Expmap(const Vector& v); - * - * - * Logmap around identity - * static Vector Logmap(const T& p); - * - * Compute l0 s.t. l2=l1*l0, where (*this) is l1 - * A default implementation of between(*this, lp) is available: - * between_default() - * T between(const T& l2) const; - * */ @@ -46,89 +24,115 @@ namespace gtsam { - /** - * These core global functions can be specialized by new Lie types - * for better performance. - */ +/** + * These core global functions can be specialized by new Lie types + * for better performance. + */ - /** Compute l0 s.t. l2=l1*l0 */ - template - inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} +/** Compute l0 s.t. l2=l1*l0 */ +template +inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} - /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ - template - inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} +/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ +template +inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} - /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ - template - inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} +/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ +template +inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} - /** - * Concept check class for Lie group type - * - * T is the Lie type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class LieConcept { - private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { +/** + * Concept check class for Lie group type + * + * This concept check provides a specialization on the Manifold type, + * in which the Manifolds represented require an algebra and group structure. + * All Lie types must also be a Manifold. + * + * The necessary functions to implement for Lie are defined + * below with additional details as to the interface. The + * concept checking function in class Lie will check whether or not + * the function exists and throw compile-time errors. + * + * The exponential map is a specific retraction for Lie groups, + * which maps the tangent space in canonical coordinates back to + * the underlying manifold. Because we can enforce a group structure, + * a retraction of delta v, with tangent space centered at x1 can be performed + * as x2 = x1.compose(Expmap(v)). + * + * Expmap around identity + * static T Expmap(const Vector& v); + * + * Logmap around identity + * static Vector Logmap(const T& p); + * + * Compute l0 s.t. l2=l1*l0, where (*this) is l1 + * A default implementation of between(*this, lp) is available: + * between_default() + * T between(const T& l2) const; + * + * By convention, we use capital letters to designate a static function + * + * @tparam T is a Lie type, like Point2, Pose3, etc. + */ +template +class LieConcept { +private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); + /** expmap around identity */ + T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); + /** Logmap around identity */ + Vector logmap_identity_ret = T::Logmap(t); - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = t.between(t2); - } - - }; - - /** - * Three term approximation of the Baker�Campbell�Hausdorff formula - * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) - * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH - * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 - * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula - */ - /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? - template - T BCH(const T& X, const T& Y) { - static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; - T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = t.between(t2); } - /** - * Declaration of wedge (see Murray94book) used to convert - * from n exponential coordinates to n*n element of the Lie algebra - */ - template Matrix wedge(const Vector& x); +}; - /** - * Exponential map given exponential coordinates - * class T needs a wedge<> function and a constructor from Matrix - * @param x exponential coordinates, vector of size n - * @ return a T - */ - template - T expm(const Vector& x, int K=7) { - Matrix xhat = wedge(x); - return expm(xhat,K); - } +/** + * Three term approximation of the Baker�Campbell�Hausdorff formula + * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) + * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH + * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 + * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula + */ +/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? +template +T BCH(const T& X, const T& Y) { + static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; + T X_Y = bracket(X, Y); + return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, + bracket(X, X_Y)); +} + +/** + * Declaration of wedge (see Murray94book) used to convert + * from n exponential coordinates to n*n element of the Lie algebra + */ +template Matrix wedge(const Vector& x); + +/** + * Exponential map given exponential coordinates + * class T needs a wedge<> function and a constructor from Matrix + * @param x exponential coordinates, vector of size n + * @ return a T + */ +template +T expm(const Vector& x, int K=7) { + Matrix xhat = wedge(x); + return expm(xhat,K); +} } // namespace gtsam @@ -141,11 +145,11 @@ namespace gtsam { * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; #define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 87506f31f..430be4f7d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -12,23 +12,7 @@ /** * @file Manifold.h * @brief Base class and basic functions for Manifold types - * @author Richard Roberts * @author Alex Cunningham - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * - * Returns dimensionality of the tangent space - * inline size_t dim() const; - * - * Returns Retraction update of T - * T retract(const Vector& v) const; - * - * Returns inverse retraction operation - * Vector localCoordinates(const T& lp) const; - * */ #pragma once @@ -38,40 +22,70 @@ namespace gtsam { - /** - * Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * T is the Manifold type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class ManifoldConcept { - private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { +/** + * Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. + * + * A manifold defines a space in which there is a notion of a linear tangent space + * that can be centered around a given point on the manifold. These nonlinear + * spaces may have such properties as wrapping around (as is the case with rotations), + * which might make linear operations on parameters not return a viable element of + * the manifold. + * + * We perform optimization by computing a linear delta in the tangent space of the + * current estimate, and then apply this change using a retraction operation, which + * maps the change in tangent space back to the manifold itself. + * + * There may be multiple possible retractions for a given manifold, which can be chosen + * between depending on the computational complexity. The important criteria for + * the creation for the retract and localCoordinates functions is that they be + * inverse operations. + * + * Returns dimensionality of the tangent space, which may be smaller than the number + * of nonlinear parameters. + * size_t dim() const; + * + * Returns a new T that is a result of updating *this with the delta v after pulling + * the updated value back to the manifold T. + * T retract(const Vector& v) const; + * + * Returns the linear coordinates of lp in the tangent space centered around *this. + * Vector localCoordinates(const T& lp) const; + * + * By convention, we use capital letters to designate a static function + * @tparam T is a Lie type, like Point2, Pose3, etc. + */ +template +class ManifoldConcept { +private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); - } - }; + /** + * Returns local coordinates of another object + */ + Vector localCoords_ret = t.localCoordinates(t2); + } +}; } // namespace gtsam From 7cd6eaa8e41da021bc4332fb9b69ea23bc2d6ed3 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 14 Nov 2011 20:22:20 +0000 Subject: [PATCH 03/72] get measurement --- gtsam/slam/ProjectionFactor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 2d25d70a9..ee5649f78 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -87,6 +87,11 @@ namespace gtsam { return reprojectionError.vector(); } + /** return the measurement */ + inline const Point2 measured() const { + return z_; + } + private: /// Serialization function From 2c5135e5f97bad5e652f8a31dae9752a7f555e3a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 14 Nov 2011 21:39:42 +0000 Subject: [PATCH 04/72] Fixed bug in relinarized variable count --- gtsam/nonlinear/ISAM2-inl.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 383951e6d..8a724cbcf 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -490,14 +490,14 @@ ISAM2Result ISAM2::update( Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); toc(6,"expmap"); - result.variablesRelinearized = markedRelinMask.size(); + result.variablesRelinearized = markedKeys.size(); #ifndef NDEBUG lastRelinVariables_ = markedRelinMask; #endif } else { -#ifndef NDEBUG result.variablesRelinearized = 0; +#ifndef NDEBUG lastRelinVariables_ = vector(ordering_.nVars(), false); #endif } From 2f13d0b961594a8309761eb9107f7d55f57e81ba Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 15 Nov 2011 17:06:34 +0000 Subject: [PATCH 05/72] Added version of calculateEstimate(const KEY&) to calculate only a single variable --- gtsam/nonlinear/ISAM2-inl.h | 9 +++++++++ gtsam/nonlinear/ISAM2.h | 12 +++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 8a724cbcf..b5dfea527 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -564,6 +564,15 @@ Values ISAM2::calculateEstimate() const { return ret; } +/* ************************************************************************* */ +template +template +typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return getLinearizationPoint()[key].retract(delta); +} + /* ************************************************************************* */ template Values ISAM2::calculateBestEstimate() const { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8159ebfb5..67335648e 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -205,10 +205,20 @@ public: const VALUES& getLinearizationPoint() const {return theta_;} /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. + * This delta is incomplete because it was not updated below wildfire_threshold. If only + * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ VALUES calculateEstimate() const; + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. + * @param key + * @return + */ + template + typename KEY::Value calculateEstimate(const KEY& key) const; + /// @name Public members for non-typical usage //@{ From 68cdbfa9f267fa67eebb5ac8ec225abef854e944 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Wed, 16 Nov 2011 03:54:07 +0000 Subject: [PATCH 06/72] fix typo --- gtsam/slam/PlanarSLAMOdometry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h index c15f2a701..941eda285 100644 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ b/gtsam/slam/PlanarSLAMOdometry.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { From f0b5e5ea3b7a374b931e5e0bfc5c16cff86bb00b Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 16 Nov 2011 19:05:51 +0000 Subject: [PATCH 07/72] Added Base and This typedefs to TupleValuesN --- gtsam/nonlinear/TupleValues.h | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index a809c360f..c16c2303f 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -428,9 +428,12 @@ namespace gtsam { typedef C2 Values2; typedef C3 Values3; + typedef TupleValues > > Base; + typedef TupleValues3 This; + TupleValues3() {} - TupleValues3(const TupleValues > >& values); - TupleValues3(const TupleValues3& values); + TupleValues3(const Base& values); + TupleValues3(const This& values); TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3); // access functions @@ -466,16 +469,18 @@ namespace gtsam { template class TupleValues5 : public TupleValues > > > > { public: - // typedefs typedef C1 Values1; typedef C2 Values2; typedef C3 Values3; typedef C4 Values4; typedef C5 Values5; + typedef TupleValues > > > > Base; + typedef TupleValues5 This; + TupleValues5() {} - TupleValues5(const TupleValues5& values); - TupleValues5(const TupleValues > > > >& values); + TupleValues5(const This& values); + TupleValues5(const Base& values); TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5); @@ -490,7 +495,6 @@ namespace gtsam { template class TupleValues6 : public TupleValues > > > > > { public: - // typedefs typedef C1 Values1; typedef C2 Values2; typedef C3 Values3; @@ -498,9 +502,12 @@ namespace gtsam { typedef C5 Values5; typedef C6 Values6; + typedef TupleValues > > > > > Base; + typedef TupleValues6 This; + TupleValues6() {} - TupleValues6(const TupleValues6& values); - TupleValues6(const TupleValues > > > > >& values); + TupleValues6(const This& values); + TupleValues6(const Base& values); TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5, const Values6& cfg6); // access functions From 6d961844c482778b4861bf68b58d94a55cd7a2c1 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 17 Nov 2011 14:09:04 +0000 Subject: [PATCH 08/72] Added optional template parameter for NonlinearISAM to specify a specialized graph --- gtsam/nonlinear/NonlinearISAM-inl.h | 20 ++++++++++---------- gtsam/nonlinear/NonlinearISAM.h | 5 +++-- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 761f2e032..478a351a3 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -30,8 +30,8 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, +template +void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -63,8 +63,8 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +template +void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; @@ -90,8 +90,8 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -Values NonlinearISAM::estimate() const { +template +VALUES NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else @@ -99,14 +99,14 @@ Values NonlinearISAM::estimate() const { } /* ************************************************************************* */ -template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { +template +Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { return isam_.marginalCovariance(ordering_[key]); } /* ************************************************************************* */ -template -void NonlinearISAM::print(const std::string& s) const { +template +void NonlinearISAM::print(const std::string& s) const { cout << "ISAM - " << s << ":" << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; isam_.print("GaussianISAM"); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 55bff857f..1eb6e66e2 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -25,11 +25,12 @@ namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ -template +template > class NonlinearISAM { public: - typedef gtsam::NonlinearFactorGraph Factors; + typedef VALUES Values; + typedef GRAPH Factors; protected: From b5e46e9b7c1d6cc054fedae8d74835983dd34592 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 17 Nov 2011 18:15:41 +0000 Subject: [PATCH 09/72] Added an optional template parameter to ISAM2 for a customized nonlinear factor graph --- gtsam/nonlinear/GaussianISAM2.h | 9 ++--- gtsam/nonlinear/ISAM2-impl-inl.h | 34 +++++++++--------- gtsam/nonlinear/ISAM2-inl.h | 62 ++++++++++++++++---------------- gtsam/nonlinear/ISAM2.h | 11 +++--- 4 files changed, 60 insertions(+), 56 deletions(-) diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 943e53aa1..0c6d4c289 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -26,15 +26,16 @@ namespace gtsam { * * @tparam VALUES The Values or TupleValues\Emph{N} that contains the * variables. + * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph */ -template -class GaussianISAM2 : public ISAM2 { +template > +class GaussianISAM2 : public ISAM2 { public: /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} + GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} + GaussianISAM2() : ISAM2() {} }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index d32085eeb..07cb0a036 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -23,10 +23,10 @@ namespace gtsam { -template -struct ISAM2::Impl { +template +struct ISAM2::Impl { - typedef ISAM2 ISAM2Type; + typedef ISAM2 ISAM2Type; struct PartialSolveResult { typename ISAM2Type::sharedClique bayesTree; @@ -58,7 +58,7 @@ struct ISAM2::Impl { * @param factors The factors from which to extract the variables * @return The set of variables indices from the factors */ - static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); + static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -138,8 +138,8 @@ struct _VariableAdder { }; /* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( +template +void ISAM2::Impl::AddVariables( const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -166,8 +166,8 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { +template +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const Symbol& key, factor->keys()) { @@ -178,8 +178,8 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Orderin } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { +template +FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { FastSet relinKeys; for(Index var=0; var(); @@ -191,8 +191,8 @@ FastSet ISAM2::Impl::CheckRelinearization(Permuted -void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask) { +template +void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -245,8 +245,8 @@ struct _SelectiveExpmapAndClear { }; /* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, +template +void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -260,9 +260,9 @@ void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permut } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +template +typename ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index b5dfea527..914ac5449 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -50,24 +50,24 @@ static const bool latestLast = true; static const bool structuralLast = false; /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): +template +ISAM2::ISAM2(const ISAM2Params& params): delta_(Permutation(), deltaUnpermuted_), params_(params) {} /* ************************************************************************* */ -template -ISAM2::ISAM2(): +template +ISAM2::ISAM2(): delta_(Permutation(), deltaUnpermuted_) {} /* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +template +FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph > allAffected; + FactorGraph > allAffected; FastList indices; BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); @@ -89,15 +89,15 @@ FastList ISAM2::getAffectedFactors(const FastList +template FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); toc(1,"getAffectedFactors"); - NonlinearFactorGraph nonlinearAffectedFactors; + GRAPH nonlinearAffectedFactors; tic(2,"affectedKeysSet"); // for fast lookup below @@ -128,9 +128,9 @@ ISAM2::relinearizeAffectedFactors(const FastList& af /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -template -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +template +FactorGraph::CacheFactor> +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -140,9 +140,9 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { // find the last variable that was eliminated Index key = (*orphan)->frontals().back(); #ifndef NDEBUG -// typename BayesNet::const_iterator it = orphan->end(); -// const Conditional& lastConditional = **(--it); -// typename Conditional::const_iterator keyit = lastConditional.endParents(); +// typename BayesNet::const_iterator it = orphan->end(); +// const CONDITIONAL& lastCONDITIONAL = **(--it); +// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); // const Index lastKey = *(--keyit); // assert(key == lastKey); #endif @@ -154,8 +154,8 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -template -boost::shared_ptr > ISAM2::recalculate( +template +boost::shared_ptr > ISAM2::recalculate( const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -174,8 +174,8 @@ boost::shared_ptr > ISAM2::recalculate( if (counter>0) { // cannot call on empty tree GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxConditionalSize; - avgClique = cstats.avgConditionalSize; + maxClique = cstats.maxCONDITIONALSize; + avgClique = cstats.avgCONDITIONALSize; numCliques = cdata.conditionalSizes.size(); nnzR = calculate_nnz(this->root()); } @@ -292,7 +292,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(5,"eliminate"); tic(6,"insert"); - BayesTree::clear(); + BayesTree::clear(); this->insert(newRoot); toc(6,"insert"); @@ -418,9 +418,9 @@ boost::shared_ptr > ISAM2::recalculate( } /* ************************************************************************* */ -template -ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, bool force_relinearize) { +template +ISAM2Result ISAM2::update( + const GRAPH& newFactors, const Values& newTheta, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); static const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -554,28 +554,28 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -template -Values ISAM2::calculateEstimate() const { +template +VALUES ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted - Values ret(theta_); + VALUES ret(theta_); vector mask(ordering_.nVars(), true); Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; } /* ************************************************************************* */ -template +template template -typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { +typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { const Index index = getOrdering()[key]; const SubVector delta = getDelta()[index]; return getLinearizationPoint()[key].retract(delta); } /* ************************************************************************* */ -template -Values ISAM2::calculateBestEstimate() const { +template +VALUES ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); return theta_.retract(delta, ordering_); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 67335648e..b567d94a6 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -123,8 +123,9 @@ struct ISAM2Result { * to the constructor, then add measurements and variables as they arrive using the update() * method. At any time, calculateEstimate() may be called to obtain the current * estimate of all variables. + * */ -template +template > class ISAM2: public BayesTree { protected: @@ -148,7 +149,7 @@ protected: Permuted delta_; /** All original nonlinear factors are stored here to use during relinearization */ - NonlinearFactorGraph nonlinearFactors_; + GRAPH nonlinearFactors_; /** @brief The current elimination ordering Symbols to Index (integer) keys. * @@ -170,6 +171,8 @@ public: typedef BayesTree Base; ///< The BayesTree base class typedef ISAM2 This; ///< This class + typedef VALUES Values; + typedef GRAPH Graph; /** Create an empty ISAM2 instance */ ISAM2(const ISAM2Params& params); @@ -198,7 +201,7 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, + ISAM2Result update(const GRAPH& newFactors, const VALUES& newTheta, bool force_relinearize = false); /** Access the current linearization point */ @@ -233,7 +236,7 @@ public: const Permuted& getDelta() const { return delta_; } /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the current ordering */ const Ordering& getOrdering() const { return ordering_; } From ba22799b67597a86376c227a5733a35541a5d260 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 17 Nov 2011 18:45:41 +0000 Subject: [PATCH 10/72] Removed unnecessary includes --- gtsam/nonlinear/GaussianISAM2.cpp | 2 +- gtsam/nonlinear/GaussianISAM2.h | 5 +---- gtsam/nonlinear/ISAM2-impl-inl.h | 16 ++++++---------- gtsam/nonlinear/ISAM2-inl.h | 11 +---------- gtsam/nonlinear/ISAM2.h | 13 ------------- tests/testGaussianISAM2.cpp | 1 + 6 files changed, 10 insertions(+), 38 deletions(-) diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index e4476b260..6c75f96a3 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -5,7 +5,7 @@ */ #include -#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 0c6d4c289..d4d177815 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -9,10 +9,7 @@ #pragma once #include -#include -#include -#include -#include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 07cb0a036..5dc201c18 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -15,14 +15,10 @@ * @author Michael Kaess, Richard Roberts */ -#include - -#include - -#include - namespace gtsam { +using namespace std; + template struct ISAM2::Impl { @@ -85,7 +81,7 @@ struct ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask); + static void FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const std::vector& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -100,7 +96,7 @@ struct ISAM2::Impl { * recalculate its delta. */ static void ExpmapMasked(VALUES& values, const Permuted& delta, - const Ordering& ordering, const vector& mask, + const Ordering& ordering, const std::vector& mask, boost::optional&> invalidateIfDebug = boost::optional&>()); /** @@ -146,7 +142,7 @@ void ISAM2::Impl::AddVariables( theta.insert(newTheta); if(debug) newTheta.print("The new variables are: "); // Add the new keys onto the ordering, add zeros to the delta for the new variables - vector dims(newTheta.dims(*newTheta.orderingArbitrary())); + std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; const size_t newDim = accumulate(dims.begin(), dims.end(), 0); const size_t originalDim = delta->dim(); @@ -192,7 +188,7 @@ FastSet ISAM2::Impl::CheckRelinearization(Permu /* ************************************************************************* */ template -void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const std::vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 914ac5449..3f68d2db4 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -21,20 +21,11 @@ #include // for operator += using namespace boost::assign; -#include -#include -#include - #include #include -#include -#include -#include #include - #include - -#include +#include #include #include diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index b567d94a6..a2ee6f8e4 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -19,21 +19,8 @@ #pragma once -#include -#include -#include -#include - -#include -#include -#include -#include #include -#include -#include #include -#include -#include namespace gtsam { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index ad9855fa5..6c96c7134 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -18,6 +18,7 @@ using namespace boost::assign; #include #include #include +#include #include #include From f050fc614f4dc07d9adeef1fd492d0c0aa18f616 Mon Sep 17 00:00:00 2001 From: Nick Barrash Date: Thu, 17 Nov 2011 19:12:05 +0000 Subject: [PATCH 11/72] Updated the documentation for Pose3 --- gtsam/geometry/Pose3.h | 87 ++++++++++++++++++++++++------------------ 1 file changed, 50 insertions(+), 37 deletions(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2fd5d7091..b8951c3ee 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -65,66 +65,79 @@ namespace gtsam { /** convert to 4*4 matrix */ Matrix matrix() const; - /** print with optional string */ + /// @name Testable + /// @{ + + /// print with optional string void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ + /// assert equality up to a tolerance bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Compose two poses */ + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Pose3 identity() { return Pose3(); } + + /// inverse transformation with derivatives + Pose3 inverse(boost::optional H1=boost::none) const; + + ///compose this transformation onto another (first *this and then p2) + Pose3 compose(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); } - Pose3 transform_to(const Pose3& pose) const; + /// @} + /// @name Manifold + /// @{ - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } + /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes + static size_t Dim() { return dimension; } - /** Lie requirements */ - - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space = 6 DOF size_t dim() const { return dimension; } - /** identity */ - static Pose3 identity() { - return Pose3(); - } - /** - * Derivative of inverse - */ - Pose3 inverse(boost::optional H1=boost::none) const; + /** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose + Pose3 retract(const Vector& d) const; - /** - * composes two poses (first (*this) then p2) - * with optional derivatives - */ - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose + Vector localCoordinates(const Pose3& T2) const; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map from Lie algebra se(3) to SE(3) + static Pose3 Expmap(const Vector& xi); + + /// Exponential map from SE(3) to Lie algebra se(3) + static Vector Logmap(const Pose3& p); + + /// @} + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } + + Pose3 transform_to(const Pose3& pose) const; + + /** Lie requirements */ /** receives the point in Pose coordinates and transforms it to world coordinates */ Point3 transform_from(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** syntactic sugar for transform */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** receives the point in world coordinates and transforms it to Pose coordinates */ Point3 transform_to(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Exponential map around another pose */ - Pose3 retract(const Vector& d) const; - - /** Logarithm map around another pose T1 */ - Vector localCoordinates(const Pose3& T2) const; - - /** non-approximated versions of Expmap/Logmap */ - static Pose3 Expmap(const Vector& xi); - static Vector Logmap(const Pose3& p); - /** * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives From 9d8c2088d2f4223496d4dd7dd3ebc5c9d5c84b1a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 17 Nov 2011 21:07:08 +0000 Subject: [PATCH 12/72] Fixed error in NegativeMatrixException print statement --- gtsam/base/cholesky.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 155b36418..0c4689113 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -33,7 +33,7 @@ struct NegativeMatrixException : public std::exception { Matrix A; ///< The original matrix attempted to factor Matrix U; ///< The produced upper-triangular factor Matrix D; ///< The produced diagonal factor - Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_A), D(_D) {} + Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_U), D(_D) {} void print(const std::string& str = "") const { std::cout << str << "\n"; gtsam::print(A, " A: "); From 96f77466c6aa573caf4ac70afa4eb822d0fae66f Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 17 Nov 2011 21:20:04 +0000 Subject: [PATCH 13/72] Removed need to include -inl.h for isam variants --- examples/vSLAMexample/vISAMexample.cpp | 4 +--- gtsam/nonlinear/GaussianISAM2.cpp | 1 - gtsam/nonlinear/ISAM2-inl.h | 1 - gtsam/nonlinear/ISAM2.h | 2 ++ gtsam/nonlinear/NonlinearISAM-inl.h | 1 - gtsam/nonlinear/NonlinearISAM.h | 3 ++- tests/testGaussianISAM2.cpp | 1 - tests/testNonlinearISAM.cpp | 2 +- 8 files changed, 6 insertions(+), 9 deletions(-) diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 300124c4e..ed3a68ba8 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -27,9 +27,7 @@ using namespace boost; #include #include #include -#include -#include -#include +#include #include "vSLAMutils.h" #include "Feature2D.h" diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index 6c75f96a3..7df21bf2c 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -5,7 +5,6 @@ */ #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 3f68d2db4..0519af50c 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -27,7 +27,6 @@ using namespace boost::assign; #include #include -#include #include diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a2ee6f8e4..1fd077c3c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -250,3 +250,5 @@ private: }; // ISAM2 } /// namespace gtsam + +#include diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 478a351a3..16ec47247 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -24,7 +24,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 1eb6e66e2..8c6d2499d 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include @@ -102,3 +101,5 @@ public: }; } // \namespace gtsam + +#include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 6c96c7134..ad9855fa5 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -18,7 +18,6 @@ using namespace boost::assign; #include #include #include -#include #include #include diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index a7f3bb285..b90e98625 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include using namespace gtsam; From 8bc83d4219a3805e00840c06afba7fb479669f1c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 17 Nov 2011 22:44:46 +0000 Subject: [PATCH 14/72] Changed -inl.h use with Values and TupleValues, removed instantiation macros for Values and TupleValues --- examples/PlanarSLAMSelfContained_advanced.cpp | 2 +- examples/SimpleRotation.cpp | 2 +- examples/easyPoint2KalmanFilter.cpp | 2 +- examples/elaboratePoint2KalmanFilter.cpp | 2 +- gtsam/nonlinear/TupleValues-inl.h | 23 ------------------- gtsam/nonlinear/TupleValues.h | 2 ++ gtsam/nonlinear/Values-inl.h | 4 ---- gtsam/nonlinear/Values.h | 4 +++- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam/slam/Simulated3D.cpp | 8 ------- gtsam/slam/planarSLAM.cpp | 3 --- gtsam/slam/pose2SLAM.cpp | 2 -- gtsam/slam/pose3SLAM.cpp | 2 -- gtsam/slam/simulated2D.cpp | 8 ------- gtsam/slam/simulated2DOriented.cpp | 4 ---- gtsam/slam/smallExample.cpp | 1 - gtsam/slam/tests/testGeneralSFMFactor.cpp | 2 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- gtsam/slam/visualSLAM.cpp | 3 +-- tests/testExtendedKalmanFilter.cpp | 2 +- tests/testGraph.cpp | 1 - tests/testNonlinearEquality.cpp | 2 -- tests/testNonlinearFactor.cpp | 1 - tests/testRot3QOptimization.cpp | 2 +- tests/testTupleValues.cpp | 2 +- 25 files changed, 16 insertions(+), 72 deletions(-) diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 549acabae..c8ee21997 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -34,7 +34,7 @@ #include // implementations for structures - needed if self-contained, and these should be included last -#include +#include #include #include #include diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index c31e91b5f..438459e64 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 3b8ee2980..f22d65f95 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include using namespace std; diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 73efc653c..8c8cbe147 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h index 09e510093..a9d6300c1 100644 --- a/gtsam/nonlinear/TupleValues-inl.h +++ b/gtsam/nonlinear/TupleValues-inl.h @@ -18,29 +18,6 @@ #pragma once -#include -#include - -// TupleValues instantiations for N = 1-6 -#define INSTANTIATE_TUPLE_VALUES1(Values1) \ - template class TupleValues1; - -#define INSTANTIATE_TUPLE_VALUES2(Values1, Values2) \ - template class TupleValues2; - -#define INSTANTIATE_TUPLE_VALUES3(Values1, Values2, Values3) \ - template class TupleValues3; - -#define INSTANTIATE_TUPLE_VALUES4(Values1, Values2, Values3, Values4) \ - template class TupleValues4; - -#define INSTANTIATE_TUPLE_VALUES5(Values1, Values2, Values3, Values4, Values5) \ - template class TupleValues5; - -#define INSTANTIATE_TUPLE_VALUES6(Values1, Values2, Values3, Values4, Values5, Values6) \ - template class TupleValues6; - - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index c16c2303f..9f1cbebae 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -520,3 +520,5 @@ namespace gtsam { }; } + +#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4a4e1898f..e53c14944 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -28,10 +28,6 @@ #include #include -#include - -#define INSTANTIATE_VALUES(J) template class Values; - using namespace std; namespace gtsam { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bb45435ce..95f2409be 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -229,5 +229,7 @@ namespace gtsam { } }; -} +} // \namespace gtsam + +#include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 0cabc5f6d..60c64b164 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -22,7 +22,7 @@ using namespace boost::assign; #include #include -#include +#include using namespace gtsam; using namespace std; diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp index 3c73b5ba9..76e65a8cf 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/Simulated3D.cpp @@ -16,17 +16,9 @@ **/ #include -#include -#include namespace gtsam { -INSTANTIATE_VALUES(simulated3D::PointKey) -INSTANTIATE_VALUES(simulated3D::PoseKey) - -using namespace simulated3D; -INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - namespace simulated3D { Point3 prior (const Point3& x, boost::optional H) { diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 955d190c2..ae47a6e7d 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -19,13 +19,10 @@ #include #include #include -#include // Use planarSLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(planarSLAM::PointKey) - INSTANTIATE_TUPLE_VALUES2(planarSLAM::PoseValues, planarSLAM::PointValues) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(planarSLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(planarSLAM::Graph, planarSLAM::Values) diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 9219c242c..0eaaeaca1 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -16,14 +16,12 @@ **/ #include -#include #include #include // Use pose2SLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(pose2SLAM::Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose2SLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose2SLAM::Graph, pose2SLAM::Values) template class NonlinearOptimizer; diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 7c7bb0d2e..4f4618896 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -16,14 +16,12 @@ **/ #include -#include #include #include // Use pose3SLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(pose3SLAM::Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose3SLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose3SLAM::Graph, pose3SLAM::Values) diff --git a/gtsam/slam/simulated2D.cpp b/gtsam/slam/simulated2D.cpp index 17a11dc06..ae30c6800 100644 --- a/gtsam/slam/simulated2D.cpp +++ b/gtsam/slam/simulated2D.cpp @@ -16,17 +16,9 @@ */ #include -#include -#include namespace gtsam { - INSTANTIATE_VALUES(simulated2D::PoseKey) - - using namespace simulated2D; - - INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - namespace simulated2D { static Matrix I = gtsam::eye(2); diff --git a/gtsam/slam/simulated2DOriented.cpp b/gtsam/slam/simulated2DOriented.cpp index 5dc1b2b22..0b68f0e46 100644 --- a/gtsam/slam/simulated2DOriented.cpp +++ b/gtsam/slam/simulated2DOriented.cpp @@ -16,13 +16,9 @@ */ #include -#include namespace gtsam { - using namespace simulated2DOriented; - - namespace simulated2DOriented { static Matrix I = gtsam::eye(3); diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 78ffeb6f8..8f09c60e7 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -34,7 +34,6 @@ using namespace std; // template definitions #include #include -#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 053f7a971..4316aadc6 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,7 +20,7 @@ using namespace boost; #include #include #include -#include +#include #include #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 36b80bd8e..f42cb6e9c 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -20,7 +20,7 @@ using namespace boost; #include #include #include -#include +#include #include #include diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 23244f5f9..5a8200dd0 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -16,12 +16,11 @@ */ #include -#include #include #include namespace gtsam { - INSTANTIATE_TUPLE_VALUES2(visualSLAM::PoseValues, visualSLAM::PointValues) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Values) diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 3027ab02f..3a946858a 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include using namespace gtsam; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 1bd46f12f..b4e04603f 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -27,7 +27,6 @@ using namespace boost::assign; #define GTSAM_MAGIC_GAUSSIAN 3 #include -#include #include #include diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 31e870f00..501bb9db8 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -24,8 +24,6 @@ #include #include -#include - using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 01d2920ad..9917273b9 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -35,7 +35,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/tests/testRot3QOptimization.cpp b/tests/testRot3QOptimization.cpp index b94cad3a5..d46c30ea4 100644 --- a/tests/testRot3QOptimization.cpp +++ b/tests/testRot3QOptimization.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index b75e98102..4ba2e3a5f 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include using namespace gtsam; using namespace std; From 163e60d43d68eba120511c149d91c2269d5629ff Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 18 Nov 2011 17:08:24 +0000 Subject: [PATCH 15/72] Fixed bug in add() - now finds the correct function --- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index c7f10475a..2cea72e7e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -58,7 +58,7 @@ namespace gtsam { template void add(const F& factor) { - push_back(boost::shared_ptr(new F(factor))); + this->push_back(boost::shared_ptr(new F(factor))); } /** From 5d2790aa5e964c970c74e92c6a9a97f4a5fc5d97 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 18 Nov 2011 19:38:55 +0000 Subject: [PATCH 16/72] Added fixme comments in LDL --- gtsam/base/cholesky.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index a437c9e13..16f07f5ab 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -179,14 +179,14 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) throw NegativeMatrixException(); } - Vector sqrtD = ldlt.vectorD().cwiseSqrt(); + Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL if (debug) cout << "Dsqrt: " << sqrtD << endl; // U = sqrtD * L^ Matrix U = ldlt.matrixU(); // we store the permuted upper triangular matrix - ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; + ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; // toc(1, "ldl"); From 3eae2dd2b8ea0c1ba3c59ddea84c8314ba856572 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 22 Nov 2011 18:37:58 +0000 Subject: [PATCH 17/72] ARDrone configure script --- myconfigure.ardrone | 1 + 1 file changed, 1 insertion(+) create mode 100755 myconfigure.ardrone diff --git a/myconfigure.ardrone b/myconfigure.ardrone new file mode 100755 index 000000000..117bcdadf --- /dev/null +++ b/myconfigure.ardrone @@ -0,0 +1 @@ +../configure --build=i686-pc-linux-gnu --host=arm-none-linux-gnueabi -prefix=/usr CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static From 9a21cbc76e827cf04734f7b1d4c911f2c2b0e3fa Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 25 Nov 2011 19:27:00 +0000 Subject: [PATCH 18/72] Reworked PartialPriorFactor implementation to simpler --- gtsam/slam/PartialPriorFactor.h | 56 +++++++++--------------------- gtsam/slam/tests/testPose3SLAM.cpp | 5 ++- 2 files changed, 18 insertions(+), 43 deletions(-) diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 9735e61d7..eb1c4614c 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -49,8 +49,9 @@ namespace gtsam { typedef NonlinearFactor1 Base; typedef PartialPriorFactor This; - Vector prior_; /// measurement on logmap parameters, in compressed form - std::vector mask_; /// flags to mask all parameters not measured + Vector prior_; ///< measurement on logmap parameters, in compressed form + std::vector mask_; ///< indices of values to constrain in compressed prior vector + Matrix H_; ///< Constant jacobian - computed at creation /** default constructor - only use for serialization */ PartialPriorFactor() {} @@ -58,10 +59,9 @@ namespace gtsam { /** * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses - * Sets the size of the mask with all values off */ PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) - : Base(model, key), mask_(T::Dim(), false) {} + : Base(model, key) {} public: @@ -70,31 +70,20 @@ namespace gtsam { virtual ~PartialPriorFactor() {} - /** Full Constructor: requires mask and vector - not for typical use */ - PartialPriorFactor(const KEY& key, const std::vector& mask, - const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask) { - assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable - assert(nrTrue() == model->dim()); - assert(nrTrue() == prior_.size()); - } - /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) { + Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); - mask_[idx] = true; - assert(nrTrue() == 1); + this->fillH(); } /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(const KEY& key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(T::Dim(), false) { + Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); - setMask(mask); - assert(nrTrue() == this->dim()); + this->fillH(); } /** implement functions needed for Testable */ @@ -117,40 +106,26 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = zeros(this->dim(), p.dim()); + if (H) (*H) = H_; // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); Vector masked_logmap = zero(this->dim()); - size_t masked_idx=0; - for (size_t i=0;i& mask() const { return mask_; } + const Matrix& H() const { return H_; } protected: - /** counts true elements in the mask */ - size_t nrTrue() const { - size_t result=0; + /** Constructs the jacobian matrix in place */ + void fillH() { for (size_t i=0; i& mask) { - for (size_t i=0; i(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(mask_); + ar & BOOST_SERIALIZATION_NVP(H_); } }; // \class PartialPriorFactor diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 54f3efa38..1eb62ec93 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -107,8 +107,7 @@ TEST(Pose3Graph, partial_prior_height) { EXPECT(assert_equal(expA, actA)); pose3SLAM::Graph graph; -// graph.add(height); // FAIL - on compile, can't initialize a reference? - graph.push_back(boost::shared_ptr(new Partial(height))); + graph.add(height); pose3SLAM::Values values; values.insert(key, init); @@ -163,7 +162,7 @@ TEST(Pose3Graph, partial_prior_xy) { EXPECT(assert_equal(expA, actA)); pose3SLAM::Graph graph; - graph.push_back(Partial::shared_ptr(new Partial(priorXY))); + graph.add(priorXY); pose3SLAM::Values values; values.insert(key, init); From 57cfb0f717b1b7578604cca5805a930b24cfaa7d Mon Sep 17 00:00:00 2001 From: Nick Barrash Date: Sat, 26 Nov 2011 04:46:22 +0000 Subject: [PATCH 19/72] Updated comment groupings for a few of the geometry classes. --- gtsam/geometry/Point2.h | 94 ++++++++++++++++----------- gtsam/geometry/Point3.h | 93 ++++++++++++++++----------- gtsam/geometry/Pose2.h | 6 +- gtsam/geometry/Pose3.h | 2 - gtsam/geometry/Rot2.h | 99 ++++++++++++++++------------ gtsam/geometry/Rot3M.h | 118 ++++++++++++++++++---------------- gtsam/geometry/Rot3Q.h | 118 ++++++++++++++++++---------------- gtsam/geometry/StereoPoint2.h | 84 +++++++++++++----------- 8 files changed, 342 insertions(+), 272 deletions(-) diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 5b335f564..12f299637 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -42,18 +42,28 @@ public: Point2(double x, double y): x_(x), y_(y) {} Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @name Testable + /// @{ - /** print with optional string */ + /// print with optional string void print(const std::string& s = "") const; - /** equals with an tolerance, prints out message if unequal*/ + /// equals with an tolerance, prints out message if unequal bool equals(const Point2& q, double tol = 1e-9) const; - // Group requirements + /// @} + /// @name Group + /// @{ - /** "Compose", just adds the coordinates of two points. With optional derivatives */ + /// identity + inline static Point2 identity() { + return Point2(); + } + + /// "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() + inline Point2 inverse() const { return Point2(-x_, -y_); } + + /// "Compose", just adds the coordinates of two points. With optional derivatives inline Point2 compose(const Point2& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -62,33 +72,60 @@ public: return *this + p2; } - /** identity */ - inline static Point2 identity() { - return Point2(); - } + /** operators */ + inline Point2 operator- () const {return Point2(-x_,-y_);} + inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} + inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} + inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ - inline Point2 inverse() const { return Point2(-x_, -y_); } + /// @} + /// @name Manifold + /// @{ - // Manifold requirements + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { return dimension; } - /** Size of the tangent space */ + /// Dimensionality of tangent space = 2 DOF inline size_t dim() const { return dimension; } - /** Updates a with tangent space delta */ + /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - /** Lie requirements */ + /// @} + /// @name Lie Group + /// @{ - /** Exponential map around identity - just create a Point2 from a vector */ + /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } - /** Log map around identity - just return the Point2 as a vector */ + /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + /// @} + /// @name Vector Operators + /// @{ + + /** norm of point */ + double norm() const; + + /** creates a unit vector */ + Point2 unit() const { return *this/norm(); } + + /** distance between two points */ + inline double dist(const Point2& p2) const { + return (p2 - *this).norm(); + } + + /** operators */ + inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} + inline void operator *= (double s) {x_*=s;y_*=s;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + + /// @} /** "Between", subtracts point coordinates */ inline Point2 between(const Point2& p2, @@ -106,27 +143,6 @@ public: /** return vectorized form (column-wise) */ Vector vector() const { return Vector_(2, x_, y_); } - /** operators */ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} - inline Point2 operator- () const {return Point2(-x_,-y_);} - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - - /** norm of point */ - double norm() const; - - /** creates a unit vector */ - Point2 unit() const { return *this/norm(); } - - /** distance between two points */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 2f8c9f8cc..a01f5009b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -46,29 +46,28 @@ namespace gtsam { Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {} + /// @name Testable + /// @{ + /** print with optional string */ void print(const std::string& s = "") const; /** equals with an tolerance */ bool equals(const Point3& p, double tol = 1e-9) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Group + /// @{ - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return dimension; } - - /** identity */ + /// identity for group operation inline static Point3 identity() { return Point3(); } - /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ + /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } - /** "Compose" - just adds coordinates of two points */ + /// "Compose" - just adds coordinates of two points inline Point3 compose(const Point3& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -77,20 +76,58 @@ namespace gtsam { return *this + p2; } + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + inline size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } + + /// @} + /// @name Lie Group + /// @{ + /** Exponential map at identity - just create a Point3 from x,y,z */ static inline Point3 Expmap(const Vector& v) { return Point3(v); } /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - // Manifold requirements + /// @} + /// @name Vector Operators + /// @{ - inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + Point3 operator - () const { return Point3(-x_,-y_,-z_);} + bool operator ==(const Point3& q) const; + Point3 operator + (const Point3& q) const; + Point3 operator - (const Point3& q) const; + Point3 operator * (double s) const; + Point3 operator / (double s) const; - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } + /** distance between two points */ + double dist(const Point3& p2) const { + return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); + } + + /** dot product */ + double norm() const; + + /** cross product @return this x q */ + Point3 cross(const Point3 &q) const; + + /** dot product @return this * q*/ + double dot(const Point3 &q) const; + + /// @} /** Between using the default implementation */ inline Point3 between(const Point3& p2, @@ -112,19 +149,6 @@ namespace gtsam { inline double y() const {return y_;} inline double z() const {return z_;} - /** operators */ - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - bool operator ==(const Point3& q) const; - Point3 operator + (const Point3& q) const; - Point3 operator - (const Point3& q) const; - Point3 operator * (double s) const; - Point3 operator / (double s) const; - - /** distance between two points */ - double dist(const Point3& p2) const { - return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); - } - /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; @@ -133,15 +157,6 @@ namespace gtsam { Point3 sub (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** cross product @return this x q */ - Point3 cross(const Point3 &q) const; - - /** dot product @return this * q*/ - double dot(const Point3 &q) const; - - /** dot product */ - double norm() const; - private: /** Serialization function */ friend class boost::serialization::access; @@ -154,7 +169,7 @@ namespace gtsam { } }; - /** Syntactic sugar for multiplying coordinates by a scalar s*p */ + /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0e4bf4824..de311249a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -79,7 +79,6 @@ public: *this = Expmap(v); } - /// @name Testable /// @{ @@ -94,9 +93,7 @@ public: /// @{ /// identity for group operation - inline static Pose2 identity() { - return Pose2(); - } + inline static Pose2 identity() { return Pose2(); } /// inverse transformation with derivatives Pose2 inverse(boost::optional H1=boost::none) const; @@ -123,7 +120,6 @@ public: /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes inline static size_t Dim() { return dimension; } - /// Dimensionality of tangent space = 3 DOF inline size_t dim() const { return dimension; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b8951c3ee..f03dcde06 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -128,8 +128,6 @@ namespace gtsam { Pose3 transform_to(const Pose3& pose) const; - /** Lie requirements */ - /** receives the point in Pose coordinates and transforms it to world coordinates */ Point3 transform_from(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 3a8fb86f0..1a6d91b20 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -109,27 +109,25 @@ namespace gtsam { return s_; } + /// @name Testable + /// @{ + /** print */ void print(const std::string& s = "theta") const; /** equals with an tolerance */ bool equals(const Rot2& R, double tol = 1e-9) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { - return dimension; - } - - /** Lie requirements */ - - /** Dimensionality of the tangent space */ - inline size_t dim() const { - return dimension; - } + /// @} + /// @name Group + /// @{ /** identity */ - inline static Rot2 identity() { - return Rot2(); + inline static Rot2 identity() { return Rot2(); } + + /** The inverse rotation - negative angle */ + Rot2 inverse() const { + return Rot2(c_, -s_); } /** Compose - make a new rotation by adding angles */ @@ -140,7 +138,41 @@ namespace gtsam { return *this * R1; } - /** Expmap around identity - create a rotation from an angle */ + /** Compose - make a new rotation by adding angles */ + Rot2 operator*(const Rot2& R) const { + return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); + } + + /** syntactic sugar for rotate */ + inline Point2 operator*(const Point2& p) const { + return rotate(p); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { + return dimension; + } + + /// Dimensionality of the tangent space, DOF = 1 + inline size_t dim() const { + return dimension; + } + + /// Updates a with tangent space delta + inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + + /// Returns inverse retraction + inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /// Expmap around identity - create a rotation from an angle static Rot2 Expmap(const Vector& v) { if (zero(v)) return (Rot2()); @@ -148,19 +180,23 @@ namespace gtsam { return Rot2::fromAngle(v(0)); } - /** Logmap around identity - return the angle of the rotation */ + /// Logmap around identity - return the angle of the rotation static inline Vector Logmap(const Rot2& r) { return Vector_(1, r.theta()); } - // Manifold requirements + /// @} + /// @name Vector Operators + /// @{ - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + /** + * Creates a unit vector as a Point2 + */ + inline Point2 unit() const { + return Point2(c_, s_); + } - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + /// @} /** Between using the default implementation */ inline Rot2 between(const Rot2& p2, boost::optional H1 = @@ -176,16 +212,6 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - Rot2 operator*(const Rot2& R) const { - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } - /** * rotate point from rotated coordinate frame to * world = R*p @@ -193,11 +219,6 @@ namespace gtsam { Point2 rotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** syntactic sugar for rotate */ - inline Point2 operator*(const Point2& p) const { - return rotate(p); - } - /** * rotate point from world to rotated * frame = R'*p @@ -205,12 +226,6 @@ namespace gtsam { Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** - * Creates a unit vector as a Point2 - */ - inline Point2 unit() const { - return Point2(c_, s_); - } private: /** Serialization function */ diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 0c17060b3..e41e01ef3 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -130,12 +130,76 @@ namespace gtsam { static Rot3M rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3M& p, double tol = 1e-9) const; + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Rot3M identity() { + return Rot3M(); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3M compose(const Rot3M& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + inline Point3 operator*(const Point3& p) const { return rotate(p);} + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() + Rot3M inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3M( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3M Expmap(const Vector& v) { + if(zero(v)) return Rot3M(); + else return rodriguez(v); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3M& R); + + /// @} + /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -176,54 +240,6 @@ namespace gtsam { r1_.z(), r2_.z(), r3_.z()).finished()); } - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3M compose(const Rot3M& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3M Expmap(const Vector& v) { - if(zero(v)) return Rot3M(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3M identity() { - return Rot3M(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3M& R); - - // Manifold requirements - - Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() - Rot3M inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3M( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); - } - /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ @@ -236,12 +252,6 @@ namespace gtsam { return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { return rotate(p);} - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h index d21f7e2fb..5986a8103 100644 --- a/gtsam/geometry/Rot3Q.h +++ b/gtsam/geometry/Rot3Q.h @@ -127,12 +127,76 @@ namespace gtsam { static Rot3Q rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3Q& p, double tol = 1e-9) const; + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Rot3Q identity() { + return Rot3Q(); + } + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() + Rot3Q inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3Q(quaternion_.inverse()); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3Q compose(const Rot3Q& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + inline Point3 operator*(const Point3& p) const { + Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); + return Point3(r(0), r(1), r(2)); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose + Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3Q Expmap(const Vector& v) { + if(zero(v)) return Rot3Q(); + else return rodriguez(v); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3Q& R); + + /// @} + /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -167,51 +231,6 @@ namespace gtsam { */ Quaternion toQuaternion() const { return quaternion_; } - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3Q compose(const Rot3Q& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3Q Expmap(const Vector& v) { - if(zero(v)) return Rot3Q(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3Q identity() { - return Rot3Q(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3Q& R); - - // Manifold requirements - - Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() - Rot3Q inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3Q(quaternion_.inverse()); - } - /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ @@ -222,15 +241,6 @@ namespace gtsam { /** compose two rotations */ Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { - Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); - return Point3(r(0), r(1), r(2)); - } - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index e493b0047..b0f2d738c 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -44,6 +44,9 @@ namespace gtsam { uL_(uL), uR_(uR), v_(v) { } + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="") const; @@ -53,49 +56,53 @@ namespace gtsam { - q.v_) < tol); } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Group + /// @{ - /** Lie requirements */ - inline size_t dim() const { return dimension; } + /// identity + inline static StereoPoint2 identity() { return StereoPoint2(); } - /** convert to vector */ - Vector vector() const { - return Vector_(3, uL_, uR_, v_); + /// inverse + inline StereoPoint2 inverse() const { + return StereoPoint2()- (*this); } - /** add two stereo points */ - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } - - /** subtract two stereo points */ - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /* - * convenient function to get a Point2 from the left image - */ - inline Point2 point2(){ - return Point2(uL_, v_); - } - - /** "Compose", just adds the coordinates of two points. */ + /// "Compose", just adds the coordinates of two points. inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1; } - /** identity */ - inline static StereoPoint2 identity() { - return StereoPoint2(); + /// add two stereo points + StereoPoint2 operator+(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); } - /** inverse */ - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); + /// subtract two stereo points + StereoPoint2 operator-(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + inline size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + /** Exponential map around identity - just create a Point2 from a vector */ static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); @@ -106,14 +113,17 @@ namespace gtsam { return p.vector(); } - // Manifold requirements + /// @}} - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /** convert to vector */ + Vector vector() const { + return Vector_(3, uL_, uR_, v_); + } - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /** convenient function to get a Point2 from the left image */ + inline Point2 point2(){ + return Point2(uL_, v_); + } inline StereoPoint2 between(const StereoPoint2& p2) const { return gtsam::between_default(*this, p2); From cc858224aacba633c7d1ba2a6e17aa7675d3913c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 26 Nov 2011 22:28:45 +0000 Subject: [PATCH 20/72] Comments only - broken parts in Pose3 correct expmap --- gtsam/geometry/Pose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index a7e687af1..fa6f02828 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -188,7 +188,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { if (H1) { #ifdef CORRECT_POSE3_EXMAP - *H1 = AdjointMap(inverse(p2)); + *H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); @@ -216,7 +216,7 @@ namespace gtsam { Pose3 Pose3::inverse(boost::optional H1) const { if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } + { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? #else { Matrix Rt = R_.transpose(); From 7164a6d7906839a35542bc04a7d1cd81f63f342f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 28 Nov 2011 20:34:28 +0000 Subject: [PATCH 21/72] Stop from saying 'converged' and added nonlinear optimizer warnings (if verbose) if maximum iterations reached --- gtsam/nonlinear/DoglegOptimizerImpl.h | 5 +++-- gtsam/nonlinear/NonlinearOptimizer-inl.h | 6 +++++- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 ++++++-- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index eefa76df1..bfb1c1878 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -183,7 +183,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( const double dx_d_norm = result.dx_d.vector().norm(); const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta - if(mode == ONE_STEP_PER_ITERATION) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { @@ -217,8 +216,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( Delta *= 0.5; if(Delta > 1e-5) stay = true; - else + else { + if(verbose) cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << endl; stay = false; + } } } diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 15a7d341d..d0d5009eb 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -200,6 +200,8 @@ namespace gtsam { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + if(verbosity >= Parameters::ERROR) + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { lambda *= factor; @@ -212,6 +214,8 @@ namespace gtsam { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + if(verbosity >= Parameters::ERROR) + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { lambda *= factor; @@ -306,7 +310,7 @@ namespace gtsam { S solver(*graph_->linearize(*values_, *ordering_)); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), - *graph_, *values_, *ordering_, error_); + *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR); shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); cout << "newValues: " << newValues.get() << endl; return newValuesErrorLambda_(newValues, result.f_error, result.Delta); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 5e0a6003e..4d7d76bfc 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -69,8 +69,12 @@ bool check_convergence( } bool converged = (relativeErrorTreshold && (relativeDecrease < relativeErrorTreshold)) || (absoluteDecrease < absoluteErrorTreshold); - if (verbosity >= 1 && converged) - cout << "converged" << endl; + if (verbosity >= 1 && converged) { + if(absoluteDecrease >= 0.0) + cout << "converged" << endl; + else + cout << "Warning: stopping nonlinear iterations because error increased" << endl; + } return converged; } From 9fcf789e3da322d3ba68e28e5e3416922ccaec98 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 28 Nov 2011 21:31:34 +0000 Subject: [PATCH 22/72] Added GaussianSequentialSolver to Matlab wrapper --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 52422cf3c..331bdd09a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -178,3 +178,9 @@ class PlanarSLAMOdometry { void print(string s) const; GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; }; + +class GaussianSequentialSolver { + GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); + GaussianBayesNet* eliminate(); + VectorValues* optimize(); +}; From ca121c2872a93510552ab1cae75df45fce56bced Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 29 Nov 2011 19:02:36 +0000 Subject: [PATCH 23/72] added retract_ function for matlab wrapper of Pose2 --- gtsam.h | 7 +++++-- gtsam/geometry/Pose2.h | 5 +++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index 331bdd09a..834e1368b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -41,6 +41,7 @@ class Pose2 { Pose2* compose_(const Pose2& p2); Pose2* between_(const Pose2& p2); Vector localCoordinates(const Pose2& p); + Pose2* retract_(Vector v); }; class SharedGaussian { @@ -181,6 +182,8 @@ class PlanarSLAMOdometry { class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); - GaussianBayesNet* eliminate(); - VectorValues* optimize(); + GaussianBayesNet* eliminate() const; + VectorValues* optimize() const; + GaussianFactor* marginalFactor(int j) const; + Matrix marginalCovariance(int j) const; }; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index de311249a..cdbf141c3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -126,6 +126,11 @@ public: /// Retraction from R^3 to Pose2 manifold neighborhood around current pose Pose2 retract(const Vector& v) const; + /// MATLAB version returns shared pointer + boost::shared_ptr retract_(const Vector& v) { + return boost::shared_ptr(new Pose2(retract(v))); + } + /// Local 3D coordinates of Pose2 manifold neighborhood around current pose Vector localCoordinates(const Pose2& p2) const; From a7ea0f4e04d08b4ea6119b50d6fb0f8e74c9dc06 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 01:59:34 +0000 Subject: [PATCH 24/72] Formatting and comments, adding Rot3 and Pose3 to matlab interface --- gtsam.h | 25 ++- gtsam/geometry/Pose3.cpp | 27 +-- gtsam/geometry/Pose3.h | 14 +- gtsam/geometry/Rot3M.cpp | 404 ++++++++++++++++++++------------------- 4 files changed, 257 insertions(+), 213 deletions(-) diff --git a/gtsam.h b/gtsam.h index 834e1368b..f37bafc67 100644 --- a/gtsam.h +++ b/gtsam.h @@ -11,6 +11,7 @@ class Point3 { Point3(double x, double y, double z); Point3(Vector v); void print(string s) const; + bool equals(const Point3& p, double tol); Vector vector() const; double x(); double y(); @@ -21,11 +22,18 @@ class Rot2 { Rot2(); Rot2(double theta); void print(string s) const; - bool equals(const Rot2& pose, double tol) const; + bool equals(const Rot2& rot, double tol) const; double c() const; double s() const; }; +class Rot3 { + Rot3(); + Rot3(Matrix R); + void print(string s) const; + bool equals(const Rot3& rot, double tol) const; +}; + class Pose2 { Pose2(); Pose2(double x, double y, double theta); @@ -44,6 +52,21 @@ class Pose2 { Pose2* retract_(Vector v); }; +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t); + Pose3(Vector v); + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + int dim() const; + Pose3* compose_(const Pose3& p2); + Pose3* between_(const Pose3& p2); + Vector localCoordinates(const Pose3& p); +}; + class SharedGaussian { SharedGaussian(Matrix covariance); void print(string s) const; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index fa6f02828..c600669b6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -214,22 +214,23 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) + if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? + // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? + { *H1 = - AdjointMap(p); } #else - { - Matrix Rt = R_.transpose(); - Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; - Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; - Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt = collect(2, &Dt_R1, &Dt_t1); - *H1 = gtsam::stack(2, &DR, &Dt); - } + { + Matrix Rt = R_.transpose(); + Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; + Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; + Matrix DR = collect(2, &DR_R1, &DR_t1); + Matrix Dt = collect(2, &Dt_R1, &Dt_t1); + *H1 = gtsam::stack(2, &DR, &Dt); + } #endif - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt*(-t_)); - } + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt*(-t_)); + } /* ************************************************************************* */ // between = compose(p2,inverse(p1)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f03dcde06..ee08c8956 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -89,6 +89,11 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Pose3& p2) { + return boost::shared_ptr(new Pose3(compose(p2))); + } + /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); @@ -144,12 +149,17 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Pose3& p2) { + return boost::shared_ptr(new Pose3(between(p2))); + } + /** * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } + Matrix AdjointMap() const; /// FIXME Not tested - marked as incorrect + Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } /// FIXME Not tested - marked as incorrect /** * wedge for Pose3: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 928b9b450..a4816914e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -25,224 +25,234 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Rot3M); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Rot3M); - static const Matrix I3 = eye(3); +static const Matrix I3 = eye(3); - /* ************************************************************************* */ - // static member functions to construct rotations +/* ************************************************************************* */ +Rot3M Rot3M::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} - Rot3M Rot3M::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} - Rot3M Rot3M::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} - Rot3M Rot3M::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); - } +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3M Rot3M::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3M( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} - // Considerably faster than composing matrices above ! - Rot3M Rot3M::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3M( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); - } - - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; #ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); + double l_n = wwTxx + wwTyy + wwTzz; + if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); #endif - double c = cos(theta), s = sin(theta), c_1 = 1 - c; + double c = cos(theta), s = sin(theta), c_1 = 1 - c; - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; - return Rot3M( c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); - } + return Rot3M( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3M(); - return rodriguez(w/t, t); - } +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3M(); + return rodriguez(w/t, t); +} - /* ************************************************************************* */ - bool Rot3M::equals(const Rot3M & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } +/* ************************************************************************* */ +bool Rot3M::equals(const Rot3M & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} - /* ************************************************************************* */ - Matrix Rot3M::matrix() const { - double r[] = { r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z() }; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::matrix() const { + Matrix R(3,3); + R << + r1_.x(), r2_.x(), r3_.x(), + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z(); + return R; +} - /* ************************************************************************* */ - Matrix Rot3M::transpose() const { - double r[] = { r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()}; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::transpose() const { + Matrix Rt(3,3); + Rt << + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z(); + return Rt; +} - /* ************************************************************************* */ - Point3 Rot3M::column(int index) const{ - if(index == 3) - return r3_; - else if (index == 2) - return r2_; - else - return r1_; // default returns r1 - } +/* ************************************************************************* */ +Point3 Rot3M::column(int index) const{ + if(index == 3) + return r3_; + else if (index == 2) + return r2_; + else + return r1_; // default returns r1 +} - /* ************************************************************************* */ - Vector Rot3M::xyz() const { - Matrix I;Vector q; - boost::tie(I,q)=RQ(matrix()); - return q; - } +/* ************************************************************************* */ +Vector Rot3M::xyz() const { + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; +} - Vector Rot3M::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); - } +/* ************************************************************************* */ +Vector Rot3M::ypr() const { + Vector q = xyz(); + return Vector_(3,q(2),q(1),q(0)); +} - Vector Rot3M::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); - } +/* ************************************************************************* */ +Vector Rot3M::rpy() const { + Vector q = xyz(); + return Vector_(3,q(0),q(1),q(2)); +} - /* ************************************************************************* */ - // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3M::Logmap(const Rot3M& R) { - double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) - return zero(3); - } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) - double theta = acos((tr-1.0)/2.0); - // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) - return (0.5 + theta*theta/12)*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(fabs(R.r3().z() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * - Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(fabs(R.r2().y() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * - Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * - Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); - } else { - double theta = acos((tr-1.0)/2.0); - return (theta/2.0/sin(theta))*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } - } - - /* ************************************************************************* */ - Point3 Rot3M::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = matrix(); - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); - } - - /* ************************************************************************* */ - // see doc/math.lyx, SO(3) section - Point3 Rot3M::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; - } - - /* ************************************************************************* */ - Rot3M Rot3M::compose (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; - } - - /* ************************************************************************* */ - Rot3M Rot3M::between (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); - } - - /* ************************************************************************* */ - pair RQ(const Matrix& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3M Qx = Rot3M::Rx(-x); - Matrix B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3M Qy = Rot3M::Ry(-y); - Matrix C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3M Qz = Rot3M::Rz(-z); - Matrix R = C * Qz.matrix(); - - Vector xyz = Vector_(3, x, y, z); - return make_pair(R, xyz); +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector Rot3M::Logmap(const Rot3M& R) { + double tr = R.r1().x()+R.r2().y()+R.r3().z(); + // FIXME should tr in statement below be absolute value? + if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) + return zero(3); + } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) + double theta = acos((tr-1.0)/2.0); + // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) + return (0.5 + theta*theta/12)*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + // FIXME: in statement below, is this the right comparision? + } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + if(fabs(R.r3().z() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * + Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); + else if(fabs(R.r2().y() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * + Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); + else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * + Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); + } else { + double theta = acos((tr-1.0)/2.0); + return (theta/2.0/sin(theta))*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); } +} - /* ************************************************************************* */ +/* ************************************************************************* */ +Point3 Rot3M::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = matrix(); + return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3M::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +Rot3M Rot3M::compose (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3M Rot3M::between (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); +} + +/* ************************************************************************* */ +pair RQ(const Matrix& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3M Qx = Rot3M::Rx(-x); + Matrix B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3M Qy = Rot3M::Ry(-y); + Matrix C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3M Qz = Rot3M::Rz(-z); + Matrix R = C * Qz.matrix(); + + Vector xyz = Vector_(3, x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ } // namespace gtsam From 5ab3211ff3597380ad81b5c77e7fdb664e638a65 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 01:59:36 +0000 Subject: [PATCH 25/72] Added more functions for geometric objects to gtsam.h --- gtsam.h | 17 +++++++++++++++++ gtsam/geometry/Point2.h | 15 +++++++++++++++ gtsam/geometry/Point3.h | 15 +++++++++++++++ gtsam/geometry/Pose3.h | 9 +++++++-- gtsam/geometry/Rot2.h | 15 +++++++++++++++ gtsam/geometry/Rot3M.h | 15 +++++++++++++++ 6 files changed, 84 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index f37bafc67..73c9cc207 100644 --- a/gtsam.h +++ b/gtsam.h @@ -4,6 +4,10 @@ class Point2 { void print(string s) const; double x(); double y(); + Point2* compose_(const Point2& p2); + Point2* between_(const Point2& p2); + Vector localCoordinates(const Point2& p); + Point2* retract_(Vector v); }; class Point3 { @@ -16,6 +20,10 @@ class Point3 { double x(); double y(); double z(); + Point3* compose_(const Point3& p2); + Point3* between_(const Point3& p2); + Vector localCoordinates(const Point3& p); + Point3* retract_(Vector v); }; class Rot2 { @@ -25,6 +33,10 @@ class Rot2 { bool equals(const Rot2& rot, double tol) const; double c() const; double s() const; + Rot2* compose_(const Rot2& p2); + Rot2* between_(const Rot2& p2); + Vector localCoordinates(const Rot2& p); + Rot2* retract_(Vector v); }; class Rot3 { @@ -32,6 +44,10 @@ class Rot3 { Rot3(Matrix R); void print(string s) const; bool equals(const Rot3& rot, double tol) const; + Rot3* compose_(const Rot3& p2); + Rot3* between_(const Rot3& p2); + Vector localCoordinates(const Rot3& p); + Rot3* retract_(Vector v); }; class Pose2 { @@ -65,6 +81,7 @@ class Pose3 { Pose3* compose_(const Pose3& p2); Pose3* between_(const Pose3& p2); Vector localCoordinates(const Pose3& p); + Pose3* retract_(Vector v); }; class SharedGaussian { diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 12f299637..73831aa38 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -72,6 +72,11 @@ public: return *this + p2; } + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Point2& p2) { + return boost::shared_ptr(new Point2(compose(p2))); + } + /** operators */ inline Point2 operator- () const {return Point2(-x_,-y_);} inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} @@ -92,6 +97,11 @@ public: /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } + /// MATLAB version returns shared pointer + boost::shared_ptr retract_(const Vector& v) { + return boost::shared_ptr(new Point2(retract(v))); + } + /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } @@ -136,6 +146,11 @@ public: return p2 - (*this); } + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Point2& p2) { + return boost::shared_ptr(new Point2(between(p2))); + } + /** get functions for x, y */ double x() const {return x_;} double y() const {return y_;} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index a01f5009b..5a52ec67d 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -76,6 +76,11 @@ namespace gtsam { return *this + p2; } + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Point3& p2) { + return boost::shared_ptr(new Point3(compose(p2))); + } + /// @} /// @name Manifold /// @{ @@ -89,6 +94,11 @@ namespace gtsam { /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + /// MATLAB version returns shared pointer + boost::shared_ptr retract_(const Vector& v) { + return boost::shared_ptr(new Point3(retract(v))); + } + /// Returns inverse retraction inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } @@ -138,6 +148,11 @@ namespace gtsam { return p2 - *this; } + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Point3& p2) { + return boost::shared_ptr(new Point3(between(p2))); + } + /** return vectorized form (column-wise)*/ Vector vector() const { Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ee08c8956..51f6f80a9 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -110,10 +110,15 @@ namespace gtsam { size_t dim() const { return dimension; } - /** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose + /// Retraction from R^6 to Pose3 manifold neighborhood around current pose Pose3 retract(const Vector& d) const; - /// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose + /// MATLAB version returns shared pointer + boost::shared_ptr retract_(const Vector& v) { + return boost::shared_ptr(new Pose3(retract(v))); + } + + /// Local 6D coordinates of Pose3 manifold neighborhood around current pose Vector localCoordinates(const Pose3& T2) const; /// @} diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 1a6d91b20..f251a5843 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -138,6 +138,11 @@ namespace gtsam { return *this * R1; } + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Rot2& p2) { + return boost::shared_ptr(new Rot2(compose(p2))); + } + /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); @@ -165,6 +170,11 @@ namespace gtsam { /// Updates a with tangent space delta inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + /// MATLAB version returns shared pointer + boost::shared_ptr retract_(const Vector& v) { + return boost::shared_ptr(new Rot2(retract(v))); + } + /// Returns inverse retraction inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } @@ -206,6 +216,11 @@ namespace gtsam { return between_default(*this, p2); } + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Rot2& p2) { + return boost::shared_ptr(new Rot2(between(p2))); + } + /** return 2*2 rotation matrix */ Matrix matrix() const; diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index e41e01ef3..771b7da39 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -152,6 +152,11 @@ namespace gtsam { Rot3M compose(const Rot3M& R2, boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// MATLAB version returns shared pointer + boost::shared_ptr compose_(const Rot3M& p2) { + return boost::shared_ptr(new Rot3M(compose(p2))); + } + /// rotate point from rotated coordinate frame to world = R*p inline Point3 operator*(const Point3& p) const { return rotate(p);} @@ -177,6 +182,11 @@ namespace gtsam { /// Updates a with tangent space delta Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } + /// MATLAB version returns shared pointer + boost::shared_ptr retract_(const Vector& v) { + return boost::shared_ptr(new Rot3M(retract(v))); + } + /// Returns inverse retraction Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } @@ -247,6 +257,11 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// MATLAB version returns shared pointer + boost::shared_ptr between_(const Rot3M& p2) { + return boost::shared_ptr(new Rot3M(between(p2))); + } + /** compose two rotations */ Rot3M operator*(const Rot3M& R2) const { return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); From 8bd894275a2c6c92e538448b309d4f8c68a08bf2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 01:59:38 +0000 Subject: [PATCH 26/72] Added flag to install matlab tests into toolbox path --- configure.ac | 11 +++++++++++ wrap/Makefile.am | 10 +++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/configure.ac b/configure.ac index 6b057ba46..b1dc2fc06 100644 --- a/configure.ac +++ b/configure.ac @@ -126,6 +126,17 @@ AC_ARG_ENABLE([build_toolbox], AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue]) +# enable installation of matlab tests +AC_ARG_ENABLE([install_matlab_tests], + [ --enable-install-matlab-tests Enable installation of tests for the Matlab toolbox], + [case "${enableval}" in + yes) install_matlab_tests=true ;; + no) install_matlab_tests=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-tests]) ;; + esac],[install_matlab_tests=true]) + +AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue]) + # Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix AC_ARG_WITH([toolbox], [AS_HELP_STRING([--with-toolbox], diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 895830537..c32a9cc5f 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -57,9 +57,17 @@ all: ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} # install the headers and matlab toolbox +if ENABLE_INSTALL_MATLAB_TESTS install-exec-hook: all install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam + cp -rf ../toolbox/* ${toolbox}/gtsam && \ + mkdir -p ${toolbox}/gtsam/tests && \ + cp -rf ../../tests/matlab/*.m ${toolbox}/gtsam/tests +else +install-exec-hook: all + install -d ${toolbox}/gtsam && \ + cp -rf ../toolbox/* ${toolbox}/gtsam/tests +endif # clean local toolbox dir clean: From ec4cfdf0659e13c8e61f1d6fdc61bd62eb936cc8 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 01:59:39 +0000 Subject: [PATCH 27/72] Added a test runner script for matlab unit tests --- tests/matlab/test_gtsam.m | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 tests/matlab/test_gtsam.m diff --git a/tests/matlab/test_gtsam.m b/tests/matlab/test_gtsam.m new file mode 100644 index 000000000..aab3dac0f --- /dev/null +++ b/tests/matlab/test_gtsam.m @@ -0,0 +1,10 @@ +% Test runner script - runs each test + +display 'Starting: testJacobianFactor' +testJacobianFactor + +display 'Starting: testKalmanFilter' +testKalmanFilter + +% end of tests +display 'Tests complete!' From 4b4815e97f3f02ef3b7fbe4c763dd282e6e38fb6 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 18:57:32 +0000 Subject: [PATCH 28/72] Added more functions to wrap, started going though wrap tests --- gtsam-broken.h | 5 +++++ gtsam.h | 8 +++++++- gtsam/base/TestableAssertions.h | 12 ++++++++++++ gtsam/geometry/Pose3.cpp | 8 ++++---- gtsam/geometry/Pose3.h | 13 ++++++++++--- gtsam/geometry/tests/testPose3.cpp | 8 ++++---- wrap/Module.cpp | 4 ++-- wrap/tests/testSpirit.cpp | 2 +- wrap/tests/testWrap.cpp | 8 ++++++++ wrap/utilities.cpp | 15 +++++++++++++++ wrap/utilities.h | 12 +++++++++++- wrap/wrap.cpp | 2 +- 12 files changed, 80 insertions(+), 17 deletions(-) diff --git a/gtsam-broken.h b/gtsam-broken.h index 09113a695..6d7fbe4b0 100644 --- a/gtsam-broken.h +++ b/gtsam-broken.h @@ -1,6 +1,11 @@ // These are considered to be broken and will be added back as they start working // It's assumed that there have been interface changes that might break this +class Pose3 { + Point3* _translation() const; + Rot3* _rotation() const; +}; + class Ordering{ Ordering(string key); void print(string s) const; diff --git a/gtsam.h b/gtsam.h index 73c9cc207..9b4eb1437 100644 --- a/gtsam.h +++ b/gtsam.h @@ -42,6 +42,10 @@ class Rot2 { class Rot3 { Rot3(); Rot3(Matrix R); + Matrix matrix() const; + Matrix transpose() const; + Vector xyz() const; + Vector ypr() const; void print(string s) const; bool equals(const Rot3& rot, double tol) const; Rot3* compose_(const Rot3& p2); @@ -72,12 +76,14 @@ class Pose3 { Pose3(); Pose3(const Rot3& r, const Point3& t); Pose3(Vector v); + Pose3(Matrix t); void print(string s) const; bool equals(const Pose3& pose, double tol) const; double x() const; double y() const; double z() const; - int dim() const; + Matrix matrix() const; + Matrix adjointMap() const; Pose3* compose_(const Pose3& p2); Pose3* between_(const Pose3& p2); Vector localCoordinates(const Pose3& p); diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 69fa02a45..46f738f7d 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -277,6 +277,18 @@ bool assert_container_equality(const V& expected, const V& actual) { return true; } +/** + * Compare strings for unit tests + */ +bool assert_equal(const std::string& expected, const std::string& actual) { + if (expected == actual) + return true; + printf("Not equal:\n"); + std::cout << "expected: [" << expected << "]\n"; + std::cout << "actual: [" << actual << "]" << std::endl; + return false; +} + /** * Allow for testing inequality */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c600669b6..6c6860f0f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -38,7 +38,7 @@ namespace gtsam { // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Experimental - unit tests of derivatives based on it do not check out yet - Matrix Pose3::AdjointMap() const { + Matrix Pose3::adjointMap() const { const Matrix R = R_.matrix(); const Vector t = t_.vector(); Matrix A = skewSymmetric(t)*R; @@ -188,7 +188,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { if (H1) { #ifdef CORRECT_POSE3_EXMAP - *H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface + *H1 = adjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); @@ -216,8 +216,8 @@ namespace gtsam { Pose3 Pose3::inverse(boost::optional H1) const { if (H1) #ifdef CORRECT_POSE3_EXMAP - // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? - { *H1 = - AdjointMap(p); } + // FIXME: this function doesn't exist with this interface - should this be "*H1 = -adjointMap();" ? + { *H1 = - adjointMap(p); } #else { Matrix Rt = R_.transpose(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 51f6f80a9..f8e40efd1 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -56,7 +56,14 @@ namespace gtsam { T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} const Rot3& rotation() const { return R_; } + boost::shared_ptr _rotation() const { + return boost::shared_ptr(new Rot3(R_)); + } + const Point3& translation() const { return t_; } + boost::shared_ptr _translation() const { + return boost::shared_ptr(new Point3(t_)); + } double x() const { return t_.x(); } double y() const { return t_.y(); } @@ -104,7 +111,7 @@ namespace gtsam { /// @{ /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { return dimension; } + static size_t Dim() { return dimension; } /// Dimensionality of the tangent space = 6 DOF size_t dim() const { return dimension; } @@ -163,8 +170,8 @@ namespace gtsam { * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix AdjointMap() const; /// FIXME Not tested - marked as incorrect - Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } /// FIXME Not tested - marked as incorrect + Matrix adjointMap() const; /// FIXME Not tested - marked as incorrect + Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect /** * wedge for Pose3: diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index cb336bb7e..afc177256 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -195,15 +195,15 @@ TEST(Pose3, expmap_c_full) TEST(Pose3, Adjoint_full) { Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); - Vector xiprime = T.Adjoint(screw::xi); + Vector xiprime = T.adjoint(screw::xi); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); - Vector xiprime2 = T2.Adjoint(screw::xi); + Vector xiprime2 = T2.adjoint(screw::xi); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); - Vector xiprime3 = T3.Adjoint(screw::xi); + Vector xiprime3 = T3.adjoint(screw::xi); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } @@ -259,7 +259,7 @@ TEST(Pose3, Adjoint_compose_full) const Pose3& T1 = T; Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = T2.inverse().Adjoint(x); + Vector y = T2.inverse().adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); EXPECT(assert_equal(expected, actual, 1e-6)); } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 7e35b0d2b..7a01161e1 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -76,14 +76,14 @@ Module::Module(const string& interfacePath, Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double"); - Rule ublasType = + Rule eigenType = (str_p("Vector") | "Matrix")[assign_a(arg.type)] >> !ch_p('*')[assign_a(arg.is_ptr,true)]; Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; Rule argument_p = - ((basisType_p[assign_a(arg.type)] | ublasType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) + ((basisType_p[assign_a(arg.type)] | eigenType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) [push_back_a(args, arg)] [assign_a(arg,arg0)]; diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 3f68952f3..a1862f439 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -54,7 +54,7 @@ TEST( spirit, sequence ) { CHECK(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); CHECK(parse("const \t string", str_p("const") >> str_p("string"), space_p).full); - // not that (see spirit FAQ) the vanilla rule<> does not deal with whitespace + // note that (see spirit FAQ) the vanilla rule<> does not deal with whitespace rule<>vanilla_p = str_p("const") >> str_p("string"); CHECK(!parse("const \t string", vanilla_p, space_p).full); diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 0650538a5..e343aad16 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -112,6 +112,14 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/make_geometry.m" , "actual/make_geometry.m" )); } +///* ************************************************************************* */ +//TEST( wrap, strip_comments ) { +// string full_string = "This is the first line // comments\n// comments at beginning of line\n"; +// string act_string = strip_comments(full_string); +// string exp_string = "This is the first line \n\n"; +// EXPECT(assert_equal(exp_string, act_string)); +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 72833f79a..9ccdfc919 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -39,6 +39,16 @@ string file_contents(const string& filename, bool skipheader) { return ss.str(); } +/* ************************************************************************* */ +bool assert_equal(const std::string& expected, const std::string& actual) { + if (expected == actual) + return true; + printf("Not equal:\n"); + std::cout << "expected: [" << expected << "]\n"; + std::cout << "actual: [" << actual << "]" << std::endl; + return false; +} + /* ************************************************************************* */ bool files_equal(const string& expected, const string& actual, bool skipheader) { try { @@ -66,3 +76,8 @@ void emit_header_comment(ofstream& ofs, const string& delimiter) { } /* ************************************************************************* */ +std::string strip_comments(const std::string& full_string) { + return full_string; /// PLACEHOLDER +} + +/* ************************************************************************* */ diff --git a/wrap/utilities.h b/wrap/utilities.h index d3ab3e197..53f9c7414 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -38,7 +38,8 @@ class ParseFailed : public std::exception { ~ParseFailed() throw() {} virtual const char* what() const throw() { std::stringstream buf; - buf << "Parse failed at character " << (length_+1); + int len = length_+1; + buf << "Parse failed at character [" << len << "]"; return buf.str().c_str(); } }; @@ -54,7 +55,16 @@ std::string file_contents(const std::string& filename, bool skipheader=false); */ bool files_equal(const std::string& expected, const std::string& actual, bool skipheader=true); +/** + * Compare strings for unit tests + */ +bool assert_equal(const std::string& expected, const std::string& actual); /** * emit a header at the top of generated files */ void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); + +/** + * Removes comments denoted with '//' from a string + */ +std::string strip_comments(const std::string& full_string); diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index d18664cda..51ba636c3 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -46,7 +46,7 @@ void generate_matlab_toolbox(const string& interfacePath, /** * main parses arguments and calls generate_matlab_toolbox above - * Typyically called from "make all" using appropriate arguments + * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { if (argc<5 || argc>6) { From 7e221aa7a1c5d7e5fa0bd4c2730b114f67a37f90 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 22:06:03 +0000 Subject: [PATCH 29/72] Added comment support in wrap --- .cproject | 229 ++++++++++-------- gtsam-broken.h | 187 -------------- gtsam.h | 197 +++++++++++++++ gtsam/geometry/Pose3.h | 4 +- wrap/Module.cpp | 31 ++- wrap/geometry.h | 21 +- .../tests/expected/@Test/return_Point2Ptr.cpp | 11 + wrap/tests/expected/@Test/return_Point2Ptr.m | 4 + wrap/tests/expected/make_geometry.m | 1 + wrap/tests/testSpirit.cpp | 55 +++-- wrap/tests/testWrap.cpp | 9 +- 11 files changed, 422 insertions(+), 327 deletions(-) delete mode 100644 gtsam-broken.h create mode 100644 wrap/tests/expected/@Test/return_Point2Ptr.cpp create mode 100644 wrap/tests/expected/@Test/return_Point2Ptr.m diff --git a/.cproject b/.cproject index a35bd2fed..bdbc3c0dd 100644 --- a/.cproject +++ b/.cproject @@ -375,6 +375,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -401,7 +409,6 @@ make - tests/testBayesTree.run true false @@ -409,7 +416,6 @@ make - testBinaryBayesNet.run true false @@ -457,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -465,7 +470,6 @@ make - tests/testSymbolicFactor.run true false @@ -473,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -489,20 +492,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -529,6 +523,7 @@ make + testGraph.run true false @@ -600,6 +595,7 @@ make + testInference.run true false @@ -607,6 +603,7 @@ make + testGaussianFactor.run true false @@ -614,6 +611,7 @@ make + testJunctionTree.run true false @@ -621,6 +619,7 @@ make + testSymbolicBayesNet.run true false @@ -628,6 +627,7 @@ make + testSymbolicFactorGraph.run true false @@ -681,22 +681,6 @@ true true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -713,6 +697,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -737,15 +737,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -753,14 +745,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -801,7 +785,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -809,6 +801,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1131,7 +1131,6 @@ make - testErrors.run true false @@ -1539,6 +1538,7 @@ make + testSimulated2DOriented.run true false @@ -1578,6 +1578,7 @@ make + testSimulated2D.run true false @@ -1585,6 +1586,7 @@ make + testSimulated3D.run true false @@ -1832,6 +1834,7 @@ make + tests/testGaussianISAM2 true false @@ -1853,46 +1856,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -1989,6 +1952,78 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + make -j2 @@ -2029,22 +2064,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - diff --git a/gtsam-broken.h b/gtsam-broken.h deleted file mode 100644 index 6d7fbe4b0..000000000 --- a/gtsam-broken.h +++ /dev/null @@ -1,187 +0,0 @@ -// These are considered to be broken and will be added back as they start working -// It's assumed that there have been interface changes that might break this - -class Pose3 { - Point3* _translation() const; - Rot3* _rotation() const; -}; - -class Ordering{ - Ordering(string key); - void print(string s) const; - bool equals(const Ordering& ord, double tol) const; - Ordering subtract(const Ordering& keys) const; - void unique (); - void reverse (); - void push_back(string s); -}; - -class GaussianFactorSet { - GaussianFactorSet(); - void push_back(GaussianFactor* factor); -}; - -class Simulated2DValues { - Simulated2DValues(); - void print(string s) const; - void insertPose(int i, const Point2& p); - void insertPoint(int j, const Point2& p); - int nrPoses() const; - int nrPoints() const; - Point2* pose(int i); - Point2* point(int j); -}; - -class Simulated2DOrientedValues { - Simulated2DOrientedValues(); - void print(string s) const; - void insertPose(int i, const Pose2& p); - void insertPoint(int j, const Point2& p); - int nrPoses() const; - int nrPoints() const; - Pose2* pose(int i); - Point2* point(int j); -}; - -class Simulated2DPosePrior { - Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -class Simulated2DOrientedPosePrior { - Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); - void print(string s) const; - double error(const Simulated2DOrientedValues& c) const; -}; - -class Simulated2DPointPrior { - Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -class Simulated2DOdometry { - Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -class Simulated2DOrientedOdometry { - Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2); - void print(string s) const; - double error(const Simulated2DOrientedValues& c) const; -}; - -class Simulated2DMeasurement { - Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -// These are currently broken -// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class -// We also have to solve the shared pointer mess to avoid duplicate methods - -class GaussianFactor { - GaussianFactor(string key1, - Matrix A1, - Vector b_in, - const SharedDiagonal& model); - GaussianFactor(string key1, - Matrix A1, - string key2, - Matrix A2, - Vector b_in, - const SharedDiagonal& model); - GaussianFactor(string key1, - Matrix A1, - string key2, - Matrix A2, - string key3, - Matrix A3, - Vector b_in, - const SharedDiagonal& model); - bool involves(string key) const; - Matrix getA(string key) const; - pair matrix(const Ordering& ordering) const; - pair eliminate(string key) const; -}; - -class GaussianFactorGraph { - GaussianConditional* eliminateOne(string key); - GaussianBayesNet* eliminate_(const Ordering& ordering); - VectorValues* optimize_(const Ordering& ordering); - pair matrix(const Ordering& ordering) const; - VectorValues* steepestDescent_(const VectorValues& x0) const; - VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; -}; - - -class Pose2Values{ - Pose2Values(); - Pose2 get(string key) const; - void insert(string name, const Pose2& val); - void print(string s) const; - void clear(); - int size(); -}; - -class Pose2Factor { - Pose2Factor(string key1, string key2, - const Pose2& measured, Matrix measurement_covariance); - void print(string name) const; - double error(const Pose2Values& c) const; - size_t size() const; - GaussianFactor* linearize(const Pose2Values& config) const; -}; - -class Pose2Graph{ - Pose2Graph(); - void print(string s) const; - GaussianFactorGraph* linearize_(const Pose2Values& config) const; - void push_back(Pose2Factor* factor); -}; - -class SymbolicFactor{ - SymbolicFactor(const Ordering& keys); - void print(string s) const; -}; - -class Simulated2DPosePrior { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Simulated2DOrientedPosePrior { - GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; -}; - -class Simulated2DPointPrior { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Simulated2DOdometry { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Simulated2DOrientedOdometry { - GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; -}; - -class Simulated2DMeasurement { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Pose2SLAMOptimizer { - Pose2SLAMOptimizer(string dataset_name); - void print(string s) const; - void update(Vector x) const; - Vector optimize() const; - double error() const; - Matrix a1() const; - Matrix a2() const; - Vector b1() const; - Vector b2() const; -}; - - diff --git a/gtsam.h b/gtsam.h index 9b4eb1437..ed746c033 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1,3 +1,17 @@ +/** + * GTSAM Wrap Module definition + * + * These are the current classes available through the matlab toolbox interface, + * add more functions/classes as they are available. + * + * Requirements: + * Constructors must appear in a class before any methods + * Methods can only return Matrix, Vector, double, int, void, and a shared_ptr to any other object + * Comments can use either C++ or C style + * Static methods are not supported + * Methods must start with a lowercase letter + */ + class Point2 { Point2(); Point2(double x, double y); @@ -88,6 +102,8 @@ class Pose3 { Pose3* between_(const Pose3& p2); Vector localCoordinates(const Pose3& p); Pose3* retract_(Vector v); + Point3* translation_() const; + Rot3* rotation_() const; }; class SharedGaussian { @@ -233,3 +249,184 @@ class GaussianSequentialSolver { GaussianFactor* marginalFactor(int j) const; Matrix marginalCovariance(int j) const; }; + +//// These are considered to be broken and will be added back as they start working +//// It's assumed that there have been interface changes that might break this +// +//class Ordering{ +// Ordering(string key); +// void print(string s) const; +// bool equals(const Ordering& ord, double tol) const; +// Ordering subtract(const Ordering& keys) const; +// void unique (); +// void reverse (); +// void push_back(string s); +//}; +// +//class GaussianFactorSet { +// GaussianFactorSet(); +// void push_back(GaussianFactor* factor); +//}; +// +//class Simulated2DValues { +// Simulated2DValues(); +// void print(string s) const; +// void insertPose(int i, const Point2& p); +// void insertPoint(int j, const Point2& p); +// int nrPoses() const; +// int nrPoints() const; +// Point2* pose(int i); +// Point2* point(int j); +//}; +// +//class Simulated2DOrientedValues { +// Simulated2DOrientedValues(); +// void print(string s) const; +// void insertPose(int i, const Pose2& p); +// void insertPoint(int j, const Point2& p); +// int nrPoses() const; +// int nrPoints() const; +// Pose2* pose(int i); +// Point2* point(int j); +//}; +// +//class Simulated2DPosePrior { +// Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class Simulated2DOrientedPosePrior { +// Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); +// void print(string s) const; +// double error(const Simulated2DOrientedValues& c) const; +//}; +// +//class Simulated2DPointPrior { +// Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class Simulated2DOdometry { +// Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class Simulated2DOrientedOdometry { +// Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2); +// void print(string s) const; +// double error(const Simulated2DOrientedValues& c) const; +//}; +// +//class Simulated2DMeasurement { +// Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//// These are currently broken +//// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class +//// We also have to solve the shared pointer mess to avoid duplicate methods +// +//class GaussianFactor { +// GaussianFactor(string key1, +// Matrix A1, +// Vector b_in, +// const SharedDiagonal& model); +// GaussianFactor(string key1, +// Matrix A1, +// string key2, +// Matrix A2, +// Vector b_in, +// const SharedDiagonal& model); +// GaussianFactor(string key1, +// Matrix A1, +// string key2, +// Matrix A2, +// string key3, +// Matrix A3, +// Vector b_in, +// const SharedDiagonal& model); +// bool involves(string key) const; +// Matrix getA(string key) const; +// pair matrix(const Ordering& ordering) const; +// pair eliminate(string key) const; +//}; +// +//class GaussianFactorGraph { +// GaussianConditional* eliminateOne(string key); +// GaussianBayesNet* eliminate_(const Ordering& ordering); +// VectorValues* optimize_(const Ordering& ordering); +// pair matrix(const Ordering& ordering) const; +// VectorValues* steepestDescent_(const VectorValues& x0) const; +// VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; +//}; +// +// +//class Pose2Values{ +// Pose2Values(); +// Pose2 get(string key) const; +// void insert(string name, const Pose2& val); +// void print(string s) const; +// void clear(); +// int size(); +//}; +// +//class Pose2Factor { +// Pose2Factor(string key1, string key2, +// const Pose2& measured, Matrix measurement_covariance); +// void print(string name) const; +// double error(const Pose2Values& c) const; +// size_t size() const; +// GaussianFactor* linearize(const Pose2Values& config) const; +//}; +// +//class Pose2Graph{ +// Pose2Graph(); +// void print(string s) const; +// GaussianFactorGraph* linearize_(const Pose2Values& config) const; +// void push_back(Pose2Factor* factor); +//}; +// +//class SymbolicFactor{ +// SymbolicFactor(const Ordering& keys); +// void print(string s) const; +//}; +// +//class Simulated2DPosePrior { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Simulated2DOrientedPosePrior { +// GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; +//}; +// +//class Simulated2DPointPrior { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Simulated2DOdometry { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Simulated2DOrientedOdometry { +// GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; +//}; +// +//class Simulated2DMeasurement { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Pose2SLAMOptimizer { +// Pose2SLAMOptimizer(string dataset_name); +// void print(string s) const; +// void update(Vector x) const; +// Vector optimize() const; +// double error() const; +// Matrix a1() const; +// Matrix a2() const; +// Vector b1() const; +// Vector b2() const; +//}; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f8e40efd1..27147cdca 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -56,12 +56,12 @@ namespace gtsam { T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} const Rot3& rotation() const { return R_; } - boost::shared_ptr _rotation() const { + boost::shared_ptr rotation_() const { return boost::shared_ptr(new Rot3(R_)); } const Point3& translation() const { return t_; } - boost::shared_ptr _translation() const { + boost::shared_ptr translation_() const { return boost::shared_ptr(new Point3(t_)); } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 7a01161e1..922983de2 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -19,6 +19,7 @@ //#define BOOST_SPIRIT_DEBUG #include +#include #include #include @@ -59,6 +60,8 @@ Module::Module(const string& interfacePath, // - The types unsigned int and bool should be specified as int. // ---------------------------------------------------------------------------- + Rule comments_p = comment_p("/*", "*/") | comment_p("//"); + // lexeme_d turns off white space skipping // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html @@ -90,7 +93,7 @@ Module::Module(const string& interfacePath, Rule argumentList_p = !argument_p >> * (',' >> argument_p); Rule constructor_p = - (className_p >> '(' >> argumentList_p >> ')' >> ';') + (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) [assign_a(constructor.args,args)] [assign_a(args,args0)] [push_back_a(cls.constructors, constructor)] @@ -119,20 +122,25 @@ Module::Module(const string& interfacePath, Rule method_p = (returnType_p >> methodName_p[assign_a(method.name_)] >> '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(method.is_const_,true)] >> ';') + !str_p("const")[assign_a(method.is_const_,true)] >> ';' >> *comments_p) [assign_a(method.args_,args)] [assign_a(args,args0)] [push_back_a(cls.methods, method)] [assign_a(method,method0)]; - Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> - *constructor_p >> - *method_p >> + Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> + *comments_p >> + *constructor_p >> + *comments_p >> + *method_p >> + *comments_p >> '}' >> ";"; - Rule module_p = +class_p + Rule module_p = *comments_p >> +(class_p [push_back_a(classes,cls)] - [assign_a(cls,cls0)] + [assign_a(cls,cls0)] + >> *comments_p) + >> *comments_p >> !end_p; //---------------------------------------------------------------------------- @@ -162,9 +170,12 @@ Module::Module(const string& interfacePath, string interfaceFile = interfacePath + "/" + moduleName + ".h"; string contents = file_contents(interfaceFile); - // Comment parser : does not work for some reason - rule<> comment_p = str_p("/*") >> +anychar_p >> "*/"; - rule<> skip_p = space_p; // | comment_p; + // FIXME: Comment parser does not work for some reason - see confix parsers +// rule<> comment_p = str_p("/*") >> +anychar_p >> "*/"; +// rule<> skip_p = space_p | comment_p; +// rule<> skip_p = space_p | comment_p("//"); // FIXME: also doesn't work +// rule<> skip_p = space_p | comment_p("/*", "*/"); // FIXME: Doesn't compile + rule<> skip_p = space_p; // and parse contents parse_info info = parse(contents.c_str(), module_p, skip_p); diff --git a/wrap/geometry.h b/wrap/geometry.h index 9b1d80b00..70500a46d 100644 --- a/wrap/geometry.h +++ b/wrap/geometry.h @@ -1,3 +1,4 @@ + // comments! class Point2 { Point2(); @@ -12,14 +13,22 @@ class Point3 { double norm() const; }; +// another comment + class Test { + /* a comment! */ + // another comment Test(); - bool return_bool (bool value); + bool return_bool (bool value); // comment after a line! size_t return_size_t (size_t value); int return_int (int value); double return_double (double value); + // comments in the middle! + + // (more) comments in the middle! + string return_string (string value); Vector return_vector1(Vector value); Matrix return_matrix1(Matrix value); @@ -32,8 +41,18 @@ class Test { Test* return_TestPtr(Test* value); + Point2* return_Point2Ptr(bool value); + pair create_ptrs (); pair return_ptrs (Test* p1, Test* p2); void print(); + + // comments at the end! + + // even more comments at the end! }; + +// comments at the end! + +// even more comments at the end! diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp new file mode 100644 index 000000000..e30ffe722 --- /dev/null +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-01 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Point2Ptr",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + bool value = unwrap< bool >(in[1]); + shared_ptr result = self->return_Point2Ptr(value); + out[0] = wrap_shared_ptr(result,"Point2"); +} diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.m b/wrap/tests/expected/@Test/return_Point2Ptr.m new file mode 100644 index 000000000..2da8b3710 --- /dev/null +++ b/wrap/tests/expected/@Test/return_Point2Ptr.m @@ -0,0 +1,4 @@ +function result = return_Point2Ptr(obj,value) +% usage: obj.return_Point2Ptr(value) + error('need to compile return_Point2Ptr.cpp'); +end diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index cad4eab1d..c27fd466c 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -41,6 +41,7 @@ mex -O5 return_matrix2.cpp mex -O5 return_pair.cpp mex -O5 return_field.cpp mex -O5 return_TestPtr.cpp +mex -O5 return_Point2Ptr.cpp mex -O5 create_ptrs.cpp mex -O5 return_ptrs.cpp mex -O5 print.cpp diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index a1862f439..5310ecff6 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -17,7 +17,9 @@ #include #include +#include #include +#include using namespace std; using namespace BOOST_SPIRIT_CLASSIC_NS; @@ -36,31 +38,31 @@ Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double" | "Ve /* ************************************************************************* */ TEST( spirit, real ) { // check if we can parse 8.99 as a real - CHECK(parse("8.99", real_p, space_p).full); + EXPECT(parse("8.99", real_p, space_p).full); // make sure parsing fails on this one - CHECK(!parse("zztop", real_p, space_p).full); + EXPECT(!parse("zztop", real_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, string ) { // check if we can parse a string - CHECK(parse("double", str_p("double"), space_p).full); + EXPECT(parse("double", str_p("double"), space_p).full); } /* ************************************************************************* */ TEST( spirit, sequence ) { // check that we skip white space - CHECK(parse("int int", str_p("int") >> *str_p("int"), space_p).full); - CHECK(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); - CHECK(parse("const \t string", str_p("const") >> str_p("string"), space_p).full); + EXPECT(parse("int int", str_p("int") >> *str_p("int"), space_p).full); + EXPECT(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); + EXPECT(parse("const \t string", str_p("const") >> str_p("string"), space_p).full); // note that (see spirit FAQ) the vanilla rule<> does not deal with whitespace rule<>vanilla_p = str_p("const") >> str_p("string"); - CHECK(!parse("const \t string", vanilla_p, space_p).full); + EXPECT(!parse("const \t string", vanilla_p, space_p).full); // to fix it, we need to use rulephrase_level_p = str_p("const") >> str_p("string"); - CHECK(parse("const \t string", phrase_level_p, space_p).full); + EXPECT(parse("const \t string", phrase_level_p, space_p).full); } /* ************************************************************************* */ @@ -81,24 +83,49 @@ Rule constMethod_p = basisType_p >> methodName_p >> '(' >> ')' >> "const" >> ';' /* ************************************************************************* */ TEST( spirit, basisType_p ) { - CHECK(!parse("Point3", basisType_p, space_p).full); - CHECK(parse("string", basisType_p, space_p).full); + EXPECT(!parse("Point3", basisType_p, space_p).full); + EXPECT(parse("string", basisType_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, className_p ) { - CHECK(parse("Point3", className_p, space_p).full); + EXPECT(parse("Point3", className_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, classRef_p ) { - CHECK(parse("Point3 &", classRef_p, space_p).full); - CHECK(parse("Point3&", classRef_p, space_p).full); + EXPECT(parse("Point3 &", classRef_p, space_p).full); + EXPECT(parse("Point3&", classRef_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, constMethod_p ) { - CHECK(parse("double norm() const;", constMethod_p, space_p).full); + EXPECT(parse("double norm() const;", constMethod_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, comments ) { +// Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; +// +// Rule argument_p = +// ((basisType_p[assign_a(arg.type)] | eigenType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) +// [push_back_a(args, arg)] +// [assign_a(arg,arg0)]; +// +// Rule void_p = str_p("void")[assign_a(method.returns_)]; + + vector all_strings; + string actual; +// Rule slash_comment_p = str_p("//"); +// Rule comments_p = anychar_p[assign_a(actual)] +// Rule comments_p = lexeme_d[*(anychar_p)[assign_a(actual)] + +// Rule line_p = (*anychar_p - comment_p("//"))[assign_a(actual)] >> !(comment_p("//") >> *anychar_p); // FAIL: matches everything +// Rule line_p = *anychar_p[assign_a(actual)] >> !(comment_p("//") >> *anychar_p); // FAIL: matches last letter +// Rule line_p = (*anychar_p - comment_p("//"))[assign_a(actual)] >> !(comment_p("//") >> *anychar_p); +// +// EXPECT(parse("not commentq // comment", line_p, eol_p).full); +// EXPECT(assert_equal(string("not comments "), actual)); } /* ************************************************************************* */ diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index e343aad16..39621c10f 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -105,6 +105,7 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/@Test/return_pair.cpp" , "actual/@Test/return_pair.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/return_field.cpp" , "actual/@Test/return_field.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/return_TestPtr.cpp", "actual/@Test/return_TestPtr.cpp")); + EXPECT(files_equal(path + "/tests/expected/@Test/return_Point2Ptr.cpp", "actual/@Test/return_Point2Ptr.cpp")); EXPECT(files_equal(path + "/tests/expected/@Test/return_ptrs.cpp" , "actual/@Test/return_ptrs.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/print.m" , "actual/@Test/print.m" )); EXPECT(files_equal(path + "/tests/expected/@Test/print.cpp" , "actual/@Test/print.cpp" )); @@ -112,14 +113,6 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/make_geometry.m" , "actual/make_geometry.m" )); } -///* ************************************************************************* */ -//TEST( wrap, strip_comments ) { -// string full_string = "This is the first line // comments\n// comments at beginning of line\n"; -// string act_string = strip_comments(full_string); -// string exp_string = "This is the first line \n\n"; -// EXPECT(assert_equal(exp_string, act_string)); -//} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From eb109c4dc55eb68abecf4c201d1385cb0b4fbdf2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 1 Dec 2011 22:06:05 +0000 Subject: [PATCH 30/72] Cleaned up debug code --- gtsam.h | 1 + wrap/Module.cpp | 9 +-------- wrap/tests/testSpirit.cpp | 26 -------------------------- wrap/utilities.cpp | 5 ----- wrap/utilities.h | 5 ----- 5 files changed, 2 insertions(+), 44 deletions(-) diff --git a/gtsam.h b/gtsam.h index ed746c033..7e3a7eaa1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -10,6 +10,7 @@ * Comments can use either C++ or C style * Static methods are not supported * Methods must start with a lowercase letter + * Classes must start with an uppercase letter */ class Point2 { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 922983de2..523e90793 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -170,15 +170,8 @@ Module::Module(const string& interfacePath, string interfaceFile = interfacePath + "/" + moduleName + ".h"; string contents = file_contents(interfaceFile); - // FIXME: Comment parser does not work for some reason - see confix parsers -// rule<> comment_p = str_p("/*") >> +anychar_p >> "*/"; -// rule<> skip_p = space_p | comment_p; -// rule<> skip_p = space_p | comment_p("//"); // FIXME: also doesn't work -// rule<> skip_p = space_p | comment_p("/*", "*/"); // FIXME: Doesn't compile - rule<> skip_p = space_p; - // and parse contents - parse_info info = parse(contents.c_str(), module_p, skip_p); + parse_info info = parse(contents.c_str(), module_p, space_p); if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); throw ParseFailed(info.length); diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 5310ecff6..9683a3372 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -103,31 +102,6 @@ TEST( spirit, constMethod_p ) { EXPECT(parse("double norm() const;", constMethod_p, space_p).full); } -/* ************************************************************************* */ -TEST( spirit, comments ) { -// Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; -// -// Rule argument_p = -// ((basisType_p[assign_a(arg.type)] | eigenType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) -// [push_back_a(args, arg)] -// [assign_a(arg,arg0)]; -// -// Rule void_p = str_p("void")[assign_a(method.returns_)]; - - vector all_strings; - string actual; -// Rule slash_comment_p = str_p("//"); -// Rule comments_p = anychar_p[assign_a(actual)] -// Rule comments_p = lexeme_d[*(anychar_p)[assign_a(actual)] - -// Rule line_p = (*anychar_p - comment_p("//"))[assign_a(actual)] >> !(comment_p("//") >> *anychar_p); // FAIL: matches everything -// Rule line_p = *anychar_p[assign_a(actual)] >> !(comment_p("//") >> *anychar_p); // FAIL: matches last letter -// Rule line_p = (*anychar_p - comment_p("//"))[assign_a(actual)] >> !(comment_p("//") >> *anychar_p); -// -// EXPECT(parse("not commentq // comment", line_p, eol_p).full); -// EXPECT(assert_equal(string("not comments "), actual)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 9ccdfc919..fb2f0fdc6 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -76,8 +76,3 @@ void emit_header_comment(ofstream& ofs, const string& delimiter) { } /* ************************************************************************* */ -std::string strip_comments(const std::string& full_string) { - return full_string; /// PLACEHOLDER -} - -/* ************************************************************************* */ diff --git a/wrap/utilities.h b/wrap/utilities.h index 53f9c7414..b5de303b8 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -63,8 +63,3 @@ bool assert_equal(const std::string& expected, const std::string& actual); * emit a header at the top of generated files */ void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); - -/** - * Removes comments denoted with '//' from a string - */ -std::string strip_comments(const std::string& full_string); From 221a6ad87763c771e16b7b0ed1833187d5550bf6 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 2 Dec 2011 02:32:18 +0000 Subject: [PATCH 31/72] Added static function parsing to wrap, included Expmap/Logmap in geometric objects. Static functions appear to still crash matlab, however. --- gtsam.h | 15 ++- gtsam/geometry/Point2.h | 3 + gtsam/geometry/Point3.h | 3 + gtsam/geometry/Pose2.h | 3 + gtsam/geometry/Pose3.h | 3 + gtsam/geometry/Rot2.h | 3 + gtsam/geometry/Rot3M.h | 3 + wrap/Class.cpp | 10 ++ wrap/Class.h | 12 ++- wrap/Makefile.am | 2 +- wrap/Method.cpp | 44 +------- wrap/Method.h | 18 ++-- wrap/Module.cpp | 37 +++++-- wrap/README | 6 +- wrap/ReturnValue.cpp | 44 ++++++++ wrap/ReturnValue.h | 34 ++++++ wrap/StaticMethod.cpp | 100 ++++++++++++++++++ wrap/StaticMethod.h | 49 +++++++++ wrap/geometry.h | 37 ++++--- wrap/matlab.h | 3 - wrap/tests/expected/@Point2/dim.cpp | 4 +- wrap/tests/expected/@Point2/dim.m | 1 - wrap/tests/expected/@Point2/x.cpp | 4 +- wrap/tests/expected/@Point2/x.m | 1 - wrap/tests/expected/@Point2/y.cpp | 4 +- wrap/tests/expected/@Point2/y.m | 1 - wrap/tests/expected/@Point3/norm.cpp | 4 +- wrap/tests/expected/@Test/create_ptrs.cpp | 4 +- wrap/tests/expected/@Test/create_ptrs.m | 1 - wrap/tests/expected/@Test/print.cpp | 4 +- .../tests/expected/@Test/return_Point2Ptr.cpp | 2 +- wrap/tests/expected/@Test/return_TestPtr.cpp | 4 +- wrap/tests/expected/@Test/return_TestPtr.m | 1 - wrap/tests/expected/@Test/return_bool.cpp | 4 +- wrap/tests/expected/@Test/return_bool.m | 1 - wrap/tests/expected/@Test/return_double.cpp | 4 +- wrap/tests/expected/@Test/return_double.m | 1 - wrap/tests/expected/@Test/return_field.cpp | 4 +- wrap/tests/expected/@Test/return_field.m | 1 - wrap/tests/expected/@Test/return_int.cpp | 4 +- wrap/tests/expected/@Test/return_int.m | 1 - wrap/tests/expected/@Test/return_matrix1.cpp | 4 +- wrap/tests/expected/@Test/return_matrix1.m | 1 - wrap/tests/expected/@Test/return_matrix2.cpp | 4 +- wrap/tests/expected/@Test/return_matrix2.m | 1 - wrap/tests/expected/@Test/return_pair.cpp | 4 +- wrap/tests/expected/@Test/return_pair.m | 1 - wrap/tests/expected/@Test/return_ptrs.cpp | 4 +- wrap/tests/expected/@Test/return_ptrs.m | 1 - wrap/tests/expected/@Test/return_size_t.cpp | 4 +- wrap/tests/expected/@Test/return_size_t.m | 1 - wrap/tests/expected/@Test/return_string.cpp | 4 +- wrap/tests/expected/@Test/return_string.m | 1 - wrap/tests/expected/@Test/return_vector1.cpp | 4 +- wrap/tests/expected/@Test/return_vector1.m | 1 - wrap/tests/expected/@Test/return_vector2.cpp | 4 +- wrap/tests/expected/@Test/return_vector2.m | 1 - wrap/tests/expected/Point3_StaticFunction.cpp | 9 ++ wrap/tests/expected/Point3_StaticFunction.m | 4 + wrap/tests/expected/make_geometry.m | 3 +- wrap/tests/expected/new_Point2_.cpp | 2 +- wrap/tests/expected/new_Point2_.m | 2 +- wrap/tests/expected/new_Point2_dd.cpp | 2 +- wrap/tests/expected/new_Point2_dd.m | 2 +- wrap/tests/expected/new_Point3_ddd.cpp | 2 +- wrap/tests/expected/new_Point3_ddd.m | 2 +- wrap/tests/expected/new_Test_.cpp | 2 +- wrap/tests/expected/new_Test_.m | 2 +- wrap/tests/expected/new_Test_b.cpp | 10 ++ wrap/tests/expected/new_Test_b.m | 4 + wrap/tests/testWrap.cpp | 5 +- wrap/utilities.cpp | 8 ++ wrap/utilities.h | 3 + 73 files changed, 432 insertions(+), 154 deletions(-) create mode 100644 wrap/ReturnValue.cpp create mode 100644 wrap/ReturnValue.h create mode 100644 wrap/StaticMethod.cpp create mode 100644 wrap/StaticMethod.h create mode 100644 wrap/tests/expected/Point3_StaticFunction.cpp create mode 100644 wrap/tests/expected/Point3_StaticFunction.m create mode 100644 wrap/tests/expected/new_Test_b.cpp create mode 100644 wrap/tests/expected/new_Test_b.m diff --git a/gtsam.h b/gtsam.h index 7e3a7eaa1..6aa6546f5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -8,14 +8,17 @@ * Constructors must appear in a class before any methods * Methods can only return Matrix, Vector, double, int, void, and a shared_ptr to any other object * Comments can use either C++ or C style - * Static methods are not supported + * Static methods are not supported - FIXED * Methods must start with a lowercase letter + * Static methods must start with an uppercase letter * Classes must start with an uppercase letter */ class Point2 { Point2(); Point2(double x, double y); + static Point2* Expmap_(Vector v); + static Vector Logmap(const Point2& p); void print(string s) const; double x(); double y(); @@ -29,6 +32,8 @@ class Point3 { Point3(); Point3(double x, double y, double z); Point3(Vector v); + static Point3* Expmap_(Vector v); + static Vector Logmap(const Point3& p); void print(string s) const; bool equals(const Point3& p, double tol); Vector vector() const; @@ -44,6 +49,8 @@ class Point3 { class Rot2 { Rot2(); Rot2(double theta); + static Rot2* Expmap_(Vector v); + static Vector Logmap(const Rot2& p); void print(string s) const; bool equals(const Rot2& rot, double tol) const; double c() const; @@ -57,6 +64,8 @@ class Rot2 { class Rot3 { Rot3(); Rot3(Matrix R); + static Rot3* Expmap_(Vector v); + static Vector Logmap(const Rot3& p); Matrix matrix() const; Matrix transpose() const; Vector xyz() const; @@ -75,6 +84,8 @@ class Pose2 { Pose2(double theta, const Point2& t); Pose2(const Rot2& r, const Point2& t); Pose2(Vector v); + static Pose2* Expmap_(Vector v); + static Vector Logmap(const Pose2& p); void print(string s) const; bool equals(const Pose2& pose, double tol) const; double x() const; @@ -92,6 +103,8 @@ class Pose3 { Pose3(const Rot3& r, const Point3& t); Pose3(Vector v); Pose3(Matrix t); + static Pose3* Expmap_(Vector v); + static Vector Logmap(const Pose3& p); void print(string s) const; bool equals(const Pose3& pose, double tol) const; double x() const; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 73831aa38..18a390d7b 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -111,6 +111,9 @@ public: /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } + static inline boost::shared_ptr Expmap_(const Vector& v) { + return boost::shared_ptr(new Point2(Expmap(v))); + } /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5a52ec67d..7b1520a16 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -108,6 +108,9 @@ namespace gtsam { /** Exponential map at identity - just create a Point3 from x,y,z */ static inline Point3 Expmap(const Vector& v) { return Point3(v); } + static inline boost::shared_ptr Expmap_(const Vector& v) { + return boost::shared_ptr(new Point3(Expmap(v))); + } /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index cdbf141c3..cee792ea6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -140,6 +140,9 @@ public: /// Exponential map from Lie algebra se(2) to SE(2) static Pose2 Expmap(const Vector& xi); + static inline boost::shared_ptr Expmap_(const Vector& v) { + return boost::shared_ptr(new Pose2(Expmap(v))); + } /// Exponential map from SE(2) to Lie algebra se(2) static Vector Logmap(const Pose2& p); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 27147cdca..be9966c60 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -134,6 +134,9 @@ namespace gtsam { /// Exponential map from Lie algebra se(3) to SE(3) static Pose3 Expmap(const Vector& xi); + static inline boost::shared_ptr Expmap_(const Vector& v) { + return boost::shared_ptr(new Pose3(Expmap(v))); + } /// Exponential map from SE(3) to Lie algebra se(3) static Vector Logmap(const Pose3& p); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index f251a5843..c52776cb9 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -189,6 +189,9 @@ namespace gtsam { else return Rot2::fromAngle(v(0)); } + static inline boost::shared_ptr Expmap_(const Vector& v) { + return boost::shared_ptr(new Rot2(Expmap(v))); + } /// Logmap around identity - return the angle of the rotation static inline Vector Logmap(const Rot2& r) { diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 771b7da39..5f71b0668 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -202,6 +202,9 @@ namespace gtsam { if(zero(v)) return Rot3M(); else return rodriguez(v); } + static inline boost::shared_ptr Expmap_(const Vector& v) { + return boost::shared_ptr(new Rot3M(Expmap(v))); + } /** * Log map at identity - return the canonical coordinates of this rotation diff --git a/wrap/Class.cpp b/wrap/Class.cpp index d701731d8..628e0ea83 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -67,6 +67,14 @@ void Class::matlab_methods(const string& classPath, const string& nameSpace) { } } +/* ************************************************************************* */ +void Class::matlab_static_methods(const string& toolboxPath, const string& nameSpace) { + BOOST_FOREACH(StaticMethod& m, static_methods) { + m.matlab_mfile (toolboxPath, name); + m.matlab_wrapper(toolboxPath, name, nameSpace); + } +} + /* ************************************************************************* */ void Class::matlab_make_fragment(ofstream& ofs, const string& toolboxPath, @@ -75,6 +83,8 @@ void Class::matlab_make_fragment(ofstream& ofs, string mex = "mex " + mexFlags + " "; BOOST_FOREACH(Constructor c, constructors) ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl; + BOOST_FOREACH(StaticMethod sm, static_methods) + ofs << mex << name + "_" + sm.name_ << ".cpp" << endl; ofs << endl << "cd @" << name << endl; BOOST_FOREACH(Method m, methods) ofs << mex << m.name_ << ".cpp" << endl; diff --git a/wrap/Class.h b/wrap/Class.h index ca85b06b1..68ff4e1ca 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -22,6 +22,7 @@ #include "Constructor.h" #include "Method.h" +#include "StaticMethod.h" /// Class has name, constructors, methods struct Class { @@ -29,10 +30,11 @@ struct Class { Class(bool verbose=true) : verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - std::string name; ///< Class name - std::list constructors; ///< Class constructors - std::list methods; ///< Class methods - bool verbose_; ///< verbose flag + std::string name; ///< Class name + std::list constructors; ///< Class constructors + std::list methods; ///< Class methods + std::list static_methods; ///< Static methods + bool verbose_; ///< verbose flag // And finally MATLAB code is emitted, methods below called by Module::matlab_code void matlab_proxy(const std::string& classFile); ///< emit proxy class @@ -40,6 +42,8 @@ struct Class { const std::string& nameSpace); ///< emit constructor wrappers void matlab_methods(const std::string& classPath, const std::string& nameSpace); ///< emit method wrappers + void matlab_static_methods(const std::string& classPath, + const std::string& nameSpace); ///< emit static method wrappers void matlab_make_fragment(std::ofstream& ofs, const std::string& toolboxPath, const std::string& mexFlags); ///< emit make fragment for global make script diff --git a/wrap/Makefile.am b/wrap/Makefile.am index c32a9cc5f..43573033c 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -15,7 +15,7 @@ check_PROGRAMS = if ENABLE_BUILD_TOOLBOX # Build a library from the core sources -sources += utilities.cpp Argument.cpp Constructor.cpp Method.cpp Class.cpp Module.cpp +sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp Method.cpp StaticMethod.cpp Class.cpp Module.cpp check_PROGRAMS += tests/testSpirit tests/testWrap noinst_PROGRAMS = wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 1e0cd134e..0b3833437 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -24,27 +24,6 @@ using namespace std; -/* ************************************************************************* */ -// auxiliary function to wrap an argument into a shared_ptr template -/* ************************************************************************* */ -string maybe_shared_ptr(bool add, const string& type) { - string str = add? "shared_ptr<" : ""; - str += type; - if (add) str += ">"; - return str; -} - -/* ************************************************************************* */ -string Method::return_type(bool add_ptr, pairing p) { - if (p==pair && returns_pair_) { - string str = "pair< " + - maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + - maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >"; - return str; - } else - return maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); -} - /* ************************************************************************* */ void Method::matlab_mfile(const string& classPath) { @@ -55,7 +34,7 @@ void Method::matlab_mfile(const string& classPath) { if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - string returnType = returns_pair_? "[first,second]" : "result"; + string returnType = returnVal_.matlab_returnType(); ofs << "function " << returnType << " = " << name_ << "(obj"; if (args_.size()) ofs << "," << args_.names(); ofs << ")" << endl; @@ -98,7 +77,7 @@ void Method::matlab_wrapper(const string& classPath, // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className + ofs << " shared_ptr<" << ((is_const_) ? "const " : "") << className << "> self = unwrap_shared_ptr< " << className << " >(in[0],\"" << className << "\");" << endl; // unwrap arguments, see Argument.cpp @@ -107,26 +86,13 @@ void Method::matlab_wrapper(const string& classPath, // call method // example: bool result = self->return_field(t); ofs << " "; - if (returns_!="void") - ofs << return_type(true,pair) << " result = "; + if (returnVal_.returns_!="void") + ofs << returnVal_.return_type(true,ReturnValue::pair) << " result = "; ofs << "self->" << name_ << "(" << args_.names() << ");\n"; // wrap result // example: out[0]=wrap(result); - if (returns_pair_) { - if (returns_ptr_) - ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n"; - else - ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; - if (returns_ptr2_) - ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n"; - else - ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; - } - else if (returns_ptr_) - ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n"; - else if (returns_!="void") - ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; + returnVal_.wrap_result(ofs); // finish ofs << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index 56679e67b..edf98d717 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -21,27 +21,23 @@ #include #include "Argument.h" +#include "ReturnValue.h" /// Method class struct Method { /// Constructor creates empty object Method(bool verbose = true) : - returns_ptr_(false), returns_ptr2_(false), returns_pair_(false), verbose_( - verbose) { - } + verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - bool is_const_; - ArgumentList args_; - std::string returns_, returns2_, name_; - bool returns_ptr_, returns_ptr2_, returns_pair_; bool verbose_; + bool is_const_; + std::string name_; + ArgumentList args_; + ReturnValue returnVal_; - enum pairing { - arg1, arg2, pair - }; - std::string return_type(bool add_ptr, pairing p); +// std::string return_type(bool add_ptr, pairing p); // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 523e90793..47a1b0b50 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -43,10 +43,12 @@ Module::Module(const string& interfacePath, { // these variables will be imperatively updated to gradually build [cls] // The one with postfix 0 are used to reset the variables after parse. + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; Constructor constructor0(verbose), constructor(verbose); Method method0(verbose), method(verbose); + StaticMethod static_method0(verbose), static_method(verbose); Class cls0(verbose),cls(verbose); //---------------------------------------------------------------------------- @@ -100,20 +102,20 @@ Module::Module(const string& interfacePath, [assign_a(constructor,constructor0)]; Rule returnType1_p = - basisType_p[assign_a(method.returns_)] | - ((className_p | "Vector" | "Matrix")[assign_a(method.returns_)] >> - !ch_p('*') [assign_a(method.returns_ptr_,true)]); + basisType_p[assign_a(retVal.returns_)] | + ((className_p | "Vector" | "Matrix")[assign_a(retVal.returns_)] >> + !ch_p('*') [assign_a(retVal.returns_ptr_,true)]); Rule returnType2_p = - basisType_p[assign_a(method.returns2_)] | - ((className_p | "Vector" | "Matrix")[assign_a(method.returns2_)] >> - !ch_p('*') [assign_a(method.returns_ptr2_,true)]); + basisType_p[assign_a(retVal.returns2_)] | + ((className_p | "Vector" | "Matrix")[assign_a(retVal.returns2_)] >> + !ch_p('*') [assign_a(retVal.returns_ptr2_,true)]); Rule pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(method.returns_pair_,true)]; + [assign_a(retVal.returns_pair_,true)]; - Rule void_p = str_p("void")[assign_a(method.returns_)]; + Rule void_p = str_p("void")[assign_a(retVal.returns_)]; Rule returnType_p = void_p | returnType1_p | pair_p; @@ -125,14 +127,30 @@ Module::Module(const string& interfacePath, !str_p("const")[assign_a(method.is_const_,true)] >> ';' >> *comments_p) [assign_a(method.args_,args)] [assign_a(args,args0)] + [assign_a(method.returnVal_,retVal)] + [assign_a(retVal,retVal0)] [push_back_a(cls.methods, method)] [assign_a(method,method0)]; + Rule staticMethodName_p = lexeme_d[upper_p >> *(alnum_p | '_')]; + + Rule static_method_p = + (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name_)] >> + '(' >> argumentList_p >> ')' >> ';' >> *comments_p) + [assign_a(static_method.args_,args)] + [assign_a(args,args0)] + [assign_a(static_method.returnVal_,retVal)] + [assign_a(retVal,retVal0)] + [push_back_a(cls.static_methods, static_method)] + [assign_a(static_method,static_method0)]; + + Rule methods_p = method_p | static_method_p; + Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> *comments_p >> *constructor_p >> *comments_p >> - *method_p >> + *methods_p >> *comments_p >> '}' >> ";"; @@ -214,6 +232,7 @@ void Module::matlab_code(const string& toolboxPath, // create constructor and method wrappers cls.matlab_constructors(toolboxPath,nameSpace); + cls.matlab_static_methods(toolboxPath,nameSpace); cls.matlab_methods(classPath,nameSpace); // add lines to make m-file diff --git a/wrap/README b/wrap/README index 6996e3649..c47b34ddd 100644 --- a/wrap/README +++ b/wrap/README @@ -17,10 +17,10 @@ OBJECT CREATION new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there METHOD (AND CONSTRUCTOR) ARGUMENTS -- simple argument types of methods, such as "double", will be converted in the +- Simple argument types of methods, such as "double", will be converted in the mex warppers by calling unwrap, defined in matlab.h -- Vector and Matric arguments are normally passed by reference in GTSAM, but - in gtsam.h you need to pretedn they are passed by value, to trigger the +- Vector and Matrix arguments are normally passed by reference in GTSAM, but + in gtsam.h you need to pretend they are passed by value, to trigger the generation of the correct conversion routines unwrap and unwrap - passing classes as arguments works, provided they are passed by reference. This triggers a call to unwrap_shared_ptr diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp new file mode 100644 index 000000000..f6307954e --- /dev/null +++ b/wrap/ReturnValue.cpp @@ -0,0 +1,44 @@ +/** + * @file ReturnValue.cpp + * + * @date Dec 1, 2011 + * @author Alex Cunningham + */ + +#include "ReturnValue.h" +#include "utilities.h" + +using namespace std; + +/* ************************************************************************* */ +string ReturnValue::return_type(bool add_ptr, pairing p) { + if (p==pair && returns_pair_) { + string str = "pair< " + + maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + + maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >"; + return str; + } else + return maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); +} + +/* ************************************************************************* */ +void ReturnValue::wrap_result(std::ostream& ofs) { + if (returns_pair_) { + if (returns_ptr_) + ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n"; + else + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; + if (returns_ptr2_) + ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n"; + else + ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; + } + else if (returns_ptr_) + ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n"; + else if (returns_!="void") + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; +} + +/* ************************************************************************* */ + + diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h new file mode 100644 index 000000000..336a415ac --- /dev/null +++ b/wrap/ReturnValue.h @@ -0,0 +1,34 @@ +/** + * @file ReturnValue.h + * + * @brief Encapsulates a return value from a method + * + * @date Dec 1, 2011 + * @author Alex Cunningham + */ + +#include + +#pragma once + +struct ReturnValue { + + ReturnValue(bool verbose = true) + : verbose_(verbose), returns_ptr_(false), returns_ptr2_(false), returns_pair_(false) + {} + + bool verbose_; + std::string returns_, returns2_; + bool returns_ptr_, returns_ptr2_, returns_pair_; + + typedef enum { + arg1, arg2, pair + } pairing; + + std::string return_type(bool add_ptr, pairing p); + + std::string matlab_returnType() const { return returns_pair_? "[first,second]" : "result"; } + + void wrap_result(std::ostream& ofs); + +}; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp new file mode 100644 index 000000000..0a712219e --- /dev/null +++ b/wrap/StaticMethod.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Method.ccp + * @author Frank Dellaert + **/ + +#include +#include + +#include + +#include "StaticMethod.h" +#include "utilities.h" + +using namespace std; + +/* ************************************************************************* */ +void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) { + + // open destination m-file + string full_name = className + "_" + name_; + string wrapperFile = toolboxPath + "/" + full_name + ".m"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + if(verbose_) cerr << "generating " << wrapperFile << endl; + + // generate code + string returnType = returnVal_.matlab_returnType(); + ofs << "function " << returnType << " = " << full_name << "("; + if (args_.size()) ofs << "," << args_.names(); + ofs << ")" << endl; + ofs << "% usage: obj." << full_name << "(" << args_.names() << ")" << endl; + ofs << " error('need to compile " << full_name << ".cpp');" << endl; + ofs << "end" << endl; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ +void StaticMethod::matlab_wrapper(const string& toolboxPath, + const string& className, const string& nameSpace) +{ + // open destination wrapperFile + string full_name = className + "_" + name_; + string wrapperFile = toolboxPath + "/" + full_name + ".cpp"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + if(verbose_) cerr << "generating " << wrapperFile << endl; + + // generate code + + // header + emit_header_comment(ofs, "//"); + ofs << "#include \n"; + ofs << "#include <" << className << ".h>\n"; + if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + + // call + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + ofs << "{\n"; + + // check arguments + // extra argument obj -> nargin-1 is passed ! + // example: checkArguments("equals",nargout,nargin-1,2); + ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args_.size() << ");\n"; + + // unwrap arguments, see Argument.cpp + args_.matlab_unwrap(ofs,1); + + // call method + // example: bool result = Point2::return_field(t); + ofs << " "; + if (returnVal_.returns_!="void") + ofs << returnVal_.return_type(true,ReturnValue::pair) << " result = "; + ofs << className << "::" << name_ << "(" << args_.names() << ");\n"; + + // wrap result + // example: out[0]=wrap(result); + returnVal_.wrap_result(ofs); + + // finish + ofs << "}\n"; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h new file mode 100644 index 000000000..11caa2eb0 --- /dev/null +++ b/wrap/StaticMethod.h @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file StaticMethod.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + **/ + +#pragma once + +#include +#include + +#include "Argument.h" +#include "ReturnValue.h" + +/// StaticMethod class +struct StaticMethod { + + /// Constructor creates empty object + StaticMethod(bool verbose = true) : + verbose_(verbose) {} + + // Then the instance variables are set directly by the Module constructor + bool verbose_; + std::string name_; + ArgumentList args_; + ReturnValue returnVal_; + + // MATLAB code generation + // toolboxPath is the core toolbox directory, e.g., ../matlab + // NOTE: static functions are not inside the class, and + // are created with [ClassName]_[FunctionName]() format + + void matlab_mfile(const std::string& toolboxPath, const std::string& className); ///< m-file + void matlab_wrapper(const std::string& toolboxPath, + const std::string& className, const std::string& nameSpace); ///< wrapper +}; + diff --git a/wrap/geometry.h b/wrap/geometry.h index 70500a46d..e19f9f3cf 100644 --- a/wrap/geometry.h +++ b/wrap/geometry.h @@ -3,14 +3,17 @@ class Point2 { Point2(); Point2(double x, double y); - double x(); - double y(); + double x() const; + double y() const; int dim() const; }; class Point3 { Point3(double x, double y, double z); double norm() const; + + // Testing: static functions - use static keyword and uppercase + static double StaticFunction(); }; // another comment @@ -20,33 +23,33 @@ class Test { // another comment Test(); - bool return_bool (bool value); // comment after a line! - size_t return_size_t (size_t value); - int return_int (int value); - double return_double (double value); + bool return_bool (bool value) const; // comment after a line! + size_t return_size_t (size_t value) const; + int return_int (int value) const; + double return_double (double value) const; // comments in the middle! // (more) comments in the middle! - string return_string (string value); - Vector return_vector1(Vector value); - Matrix return_matrix1(Matrix value); - Vector return_vector2(Vector value); - Matrix return_matrix2(Matrix value); + string return_string (string value) const; + Vector return_vector1(Vector value) const; + Matrix return_matrix1(Matrix value) const; + Vector return_vector2(Vector value) const; + Matrix return_matrix2(Matrix value) const; - pair return_pair (Vector v, Matrix A); + pair return_pair (Vector v, Matrix A) const; bool return_field(const Test& t) const; - Test* return_TestPtr(Test* value); + Test* return_TestPtr(Test* value) const; - Point2* return_Point2Ptr(bool value); + Point2* return_Point2Ptr(bool value) const; - pair create_ptrs (); - pair return_ptrs (Test* p1, Test* p2); + pair create_ptrs () const; + pair return_ptrs (Test* p1, Test* p2) const; - void print(); + void print() const; // comments at the end! diff --git a/wrap/matlab.h b/wrap/matlab.h index 410798718..31ee4dc80 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -36,9 +36,6 @@ using namespace std; using namespace boost; // not usual, but for conciseness of generated code // start GTSAM Specifics ///////////////////////////////////////////////// -//typedef gtsam::Vector Vector; // NOTE: outside of gtsam namespace -//typedef gtsam::Matrix Matrix; - // to make keys be constructed from strings: #define GTSAM_MAGIC_KEY // to enable Matrix and Vector constructor for SharedGaussian: diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index f81c416fe..c2002446b 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); int result = self->dim(); out[0] = wrap< int >(result); } diff --git a/wrap/tests/expected/@Point2/dim.m b/wrap/tests/expected/@Point2/dim.m index afd9ee5f7..40715292e 100644 --- a/wrap/tests/expected/@Point2/dim.m +++ b/wrap/tests/expected/@Point2/dim.m @@ -1,5 +1,4 @@ function result = dim(obj) % usage: obj.dim() -% automatically generated by wrap on 2011-Oct-31 error('need to compile dim.cpp'); end diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp index da5ed8a0c..8777b3a56 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); double result = self->x(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point2/x.m b/wrap/tests/expected/@Point2/x.m index 3309b286c..f91039ab7 100644 --- a/wrap/tests/expected/@Point2/x.m +++ b/wrap/tests/expected/@Point2/x.m @@ -1,5 +1,4 @@ function result = x(obj) % usage: obj.x() -% automatically generated by wrap on 2011-Oct-31 error('need to compile x.cpp'); end diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index 3d61a4048..f2e39d92e 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); double result = self->y(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point2/y.m b/wrap/tests/expected/@Point2/y.m index 015a11144..f9f36ae90 100644 --- a/wrap/tests/expected/@Point2/y.m +++ b/wrap/tests/expected/@Point2/y.m @@ -1,5 +1,4 @@ function result = y(obj) % usage: obj.y() -% automatically generated by wrap on 2011-Oct-31 error('need to compile y.cpp'); end diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 6daaadfd1..732f3f4c8 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("norm",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); + shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); double result = self->norm(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index 690466298..76e5ce40c 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); pair< shared_ptr, shared_ptr > result = self->create_ptrs(); out[0] = wrap_shared_ptr(result.first,"Test"); out[1] = wrap_shared_ptr(result.second,"Test"); diff --git a/wrap/tests/expected/@Test/create_ptrs.m b/wrap/tests/expected/@Test/create_ptrs.m index 07325487f..11af0ac5b 100644 --- a/wrap/tests/expected/@Test/create_ptrs.m +++ b/wrap/tests/expected/@Test/create_ptrs.m @@ -1,5 +1,4 @@ function [first,second] = create_ptrs(obj) % usage: obj.create_ptrs() -% automatically generated by wrap on 2011-Oct-31 error('need to compile create_ptrs.cpp'); end diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index 9c516c6b7..24c3a2258 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,9 +1,9 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); self->print(); } diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp index e30ffe722..0d6c35fce 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.cpp +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); bool value = unwrap< bool >(in[1]); shared_ptr result = self->return_Point2Ptr(value); out[0] = wrap_shared_ptr(result,"Point2"); diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index b4ebecfd8..a9d4ef983 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); shared_ptr result = self->return_TestPtr(value); out[0] = wrap_shared_ptr(result,"Test"); diff --git a/wrap/tests/expected/@Test/return_TestPtr.m b/wrap/tests/expected/@Test/return_TestPtr.m index 5da63f9c1..e1d0c90f5 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.m +++ b/wrap/tests/expected/@Test/return_TestPtr.m @@ -1,5 +1,4 @@ function result = return_TestPtr(obj,value) % usage: obj.return_TestPtr(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_TestPtr.cpp'); end diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp index 5a797b963..3f0aaa8e4 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); bool value = unwrap< bool >(in[1]); bool result = self->return_bool(value); out[0] = wrap< bool >(result); diff --git a/wrap/tests/expected/@Test/return_bool.m b/wrap/tests/expected/@Test/return_bool.m index bf41045d3..185ab992d 100644 --- a/wrap/tests/expected/@Test/return_bool.m +++ b/wrap/tests/expected/@Test/return_bool.m @@ -1,5 +1,4 @@ function result = return_bool(obj,value) % usage: obj.return_bool(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_bool.cpp'); end diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp index c1dc96f9d..9fd1e0edb 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); double value = unwrap< double >(in[1]); double result = self->return_double(value); out[0] = wrap< double >(result); diff --git a/wrap/tests/expected/@Test/return_double.m b/wrap/tests/expected/@Test/return_double.m index e8c154440..a6ba733cf 100644 --- a/wrap/tests/expected/@Test/return_double.m +++ b/wrap/tests/expected/@Test/return_double.m @@ -1,5 +1,4 @@ function result = return_double(obj,value) % usage: obj.return_double(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_double.cpp'); end diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp index adcb76bfd..5a211da3a 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Test& t = *unwrap_shared_ptr< Test >(in[1], "Test"); bool result = self->return_field(t); out[0] = wrap< bool >(result); diff --git a/wrap/tests/expected/@Test/return_field.m b/wrap/tests/expected/@Test/return_field.m index d99196354..278ffa411 100644 --- a/wrap/tests/expected/@Test/return_field.m +++ b/wrap/tests/expected/@Test/return_field.m @@ -1,5 +1,4 @@ function result = return_field(obj,t) % usage: obj.return_field(t) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_field.cpp'); end diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp index fc24edc3c..c349622d7 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); int value = unwrap< int >(in[1]); int result = self->return_int(value); out[0] = wrap< int >(result); diff --git a/wrap/tests/expected/@Test/return_int.m b/wrap/tests/expected/@Test/return_int.m index a36990b35..8b613285c 100644 --- a/wrap/tests/expected/@Test/return_int.m +++ b/wrap/tests/expected/@Test/return_int.m @@ -1,5 +1,4 @@ function result = return_int(obj,value) % usage: obj.return_int(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_int.cpp'); end diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp index 07c59cebc..1dfcb53f0 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix value = unwrap< Matrix >(in[1]); Matrix result = self->return_matrix1(value); out[0] = wrap< Matrix >(result); diff --git a/wrap/tests/expected/@Test/return_matrix1.m b/wrap/tests/expected/@Test/return_matrix1.m index 87200044f..29743158c 100644 --- a/wrap/tests/expected/@Test/return_matrix1.m +++ b/wrap/tests/expected/@Test/return_matrix1.m @@ -1,5 +1,4 @@ function result = return_matrix1(obj,value) % usage: obj.return_matrix1(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_matrix1.cpp'); end diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp index 6a942b54d..422642917 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix value = unwrap< Matrix >(in[1]); Matrix result = self->return_matrix2(value); out[0] = wrap< Matrix >(result); diff --git a/wrap/tests/expected/@Test/return_matrix2.m b/wrap/tests/expected/@Test/return_matrix2.m index 05b9c9c9c..e9ec91678 100644 --- a/wrap/tests/expected/@Test/return_matrix2.m +++ b/wrap/tests/expected/@Test/return_matrix2.m @@ -1,5 +1,4 @@ function result = return_matrix2(obj,value) % usage: obj.return_matrix2(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_matrix2.cpp'); end diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp index 637f0b365..0a9a8378a 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector v = unwrap< Vector >(in[1]); Matrix A = unwrap< Matrix >(in[2]); pair< Vector, Matrix > result = self->return_pair(v,A); diff --git a/wrap/tests/expected/@Test/return_pair.m b/wrap/tests/expected/@Test/return_pair.m index 61a0138f8..a97f7c46e 100644 --- a/wrap/tests/expected/@Test/return_pair.m +++ b/wrap/tests/expected/@Test/return_pair.m @@ -1,5 +1,4 @@ function [first,second] = return_pair(obj,v,A) % usage: obj.return_pair(v,A) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_pair.cpp'); end diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp index ae1ceae84..556b5607c 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "Test"); shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "Test"); pair< shared_ptr, shared_ptr > result = self->return_ptrs(p1,p2); diff --git a/wrap/tests/expected/@Test/return_ptrs.m b/wrap/tests/expected/@Test/return_ptrs.m index 8eb7d0ce2..ef7f8e5fc 100644 --- a/wrap/tests/expected/@Test/return_ptrs.m +++ b/wrap/tests/expected/@Test/return_ptrs.m @@ -1,5 +1,4 @@ function [first,second] = return_ptrs(obj,p1,p2) % usage: obj.return_ptrs(p1,p2) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_ptrs.cpp'); end diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp index 1ffaf6a4f..e8358e1d0 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); size_t value = unwrap< size_t >(in[1]); size_t result = self->return_size_t(value); out[0] = wrap< size_t >(result); diff --git a/wrap/tests/expected/@Test/return_size_t.m b/wrap/tests/expected/@Test/return_size_t.m index 9124824b7..bc2734410 100644 --- a/wrap/tests/expected/@Test/return_size_t.m +++ b/wrap/tests/expected/@Test/return_size_t.m @@ -1,5 +1,4 @@ function result = return_size_t(obj,value) % usage: obj.return_size_t(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_size_t.cpp'); end diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp index 3055a8422..bd5eb9c22 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); string value = unwrap< string >(in[1]); string result = self->return_string(value); out[0] = wrap< string >(result); diff --git a/wrap/tests/expected/@Test/return_string.m b/wrap/tests/expected/@Test/return_string.m index ad22de567..3f2304a65 100644 --- a/wrap/tests/expected/@Test/return_string.m +++ b/wrap/tests/expected/@Test/return_string.m @@ -1,5 +1,4 @@ function result = return_string(obj,value) % usage: obj.return_string(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_string.cpp'); end diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp index e1a384957..0977c7de9 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector value = unwrap< Vector >(in[1]); Vector result = self->return_vector1(value); out[0] = wrap< Vector >(result); diff --git a/wrap/tests/expected/@Test/return_vector1.m b/wrap/tests/expected/@Test/return_vector1.m index a0ee6e8ba..f67382978 100644 --- a/wrap/tests/expected/@Test/return_vector1.m +++ b/wrap/tests/expected/@Test/return_vector1.m @@ -1,5 +1,4 @@ function result = return_vector1(obj,value) % usage: obj.return_vector1(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_vector1.cpp'); end diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp index 1380643c1..af65b8434 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector value = unwrap< Vector >(in[1]); Vector result = self->return_vector2(value); out[0] = wrap< Vector >(result); diff --git a/wrap/tests/expected/@Test/return_vector2.m b/wrap/tests/expected/@Test/return_vector2.m index 3768e9e0e..95b6bcfd6 100644 --- a/wrap/tests/expected/@Test/return_vector2.m +++ b/wrap/tests/expected/@Test/return_vector2.m @@ -1,5 +1,4 @@ function result = return_vector2(obj,value) % usage: obj.return_vector2(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_vector2.cpp'); end diff --git a/wrap/tests/expected/Point3_StaticFunction.cpp b/wrap/tests/expected/Point3_StaticFunction.cpp new file mode 100644 index 000000000..7662b583b --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunction.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-01 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("Point3_StaticFunction",nargout,nargin,0); + double result = Point3::StaticFunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected/Point3_StaticFunction.m b/wrap/tests/expected/Point3_StaticFunction.m new file mode 100644 index 000000000..cdb0d483d --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunction.m @@ -0,0 +1,4 @@ +function result = Point3_StaticFunction(obj) +% usage: obj.Point3_StaticFunction() + error('need to compile Point3_StaticFunction.cpp'); +end diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index c27fd466c..864dc60e5 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Nov-04 +% automatically generated by wrap on 2011-Dec-01 echo on toolboxpath = mfilename('fullpath'); @@ -20,6 +20,7 @@ mex -O5 dim.cpp %% Point3 cd(toolboxpath) mex -O5 new_Point3_ddd.cpp +mex -O5 Point3_StaticFunction.cpp cd @Point3 mex -O5 norm.cpp diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index 4745ae6a1..da07147da 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Point2_.m b/wrap/tests/expected/new_Point2_.m index 5b3756ef7..f6e31bbf2 100644 --- a/wrap/tests/expected/new_Point2_.m +++ b/wrap/tests/expected/new_Point2_.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-01 function result = new_Point2_(obj) error('need to compile new_Point2_.cpp'); end diff --git a/wrap/tests/expected/new_Point2_dd.cpp b/wrap/tests/expected/new_Point2_dd.cpp index 30db28cd9..e447c234e 100644 --- a/wrap/tests/expected/new_Point2_dd.cpp +++ b/wrap/tests/expected/new_Point2_dd.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Point2_dd.m b/wrap/tests/expected/new_Point2_dd.m index f83962584..19e3b3ebd 100644 --- a/wrap/tests/expected/new_Point2_dd.m +++ b/wrap/tests/expected/new_Point2_dd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-01 function result = new_Point2_dd(obj,x,y) error('need to compile new_Point2_dd.cpp'); end diff --git a/wrap/tests/expected/new_Point3_ddd.cpp b/wrap/tests/expected/new_Point3_ddd.cpp index c83518386..3a572c26c 100644 --- a/wrap/tests/expected/new_Point3_ddd.cpp +++ b/wrap/tests/expected/new_Point3_ddd.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Point3_ddd.m b/wrap/tests/expected/new_Point3_ddd.m index 3b15dcfdb..890a93bf5 100644 --- a/wrap/tests/expected/new_Point3_ddd.m +++ b/wrap/tests/expected/new_Point3_ddd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-01 function result = new_Point3_ddd(obj,x,y,z) error('need to compile new_Point3_ddd.cpp'); end diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp index 9afe37223..77b74027f 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-01 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Test_.m b/wrap/tests/expected/new_Test_.m index 03f3ca25f..023f8a451 100644 --- a/wrap/tests/expected/new_Test_.m +++ b/wrap/tests/expected/new_Test_.m @@ -1,4 +1,4 @@ +% automatically generated by wrap on 2011-Dec-01 function result = new_Test_(obj) -% automatically generated by wrap on 2011-Oct-31 error('need to compile new_Test_.cpp'); end diff --git a/wrap/tests/expected/new_Test_b.cpp b/wrap/tests/expected/new_Test_b.cpp new file mode 100644 index 000000000..921c692af --- /dev/null +++ b/wrap/tests/expected/new_Test_b.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-01 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_Test_b",nargout,nargin,1); + bool value = unwrap< bool >(in[0]); + Test* self = new Test(value); + out[0] = wrap_constructed(self,"Test"); +} diff --git a/wrap/tests/expected/new_Test_b.m b/wrap/tests/expected/new_Test_b.m new file mode 100644 index 000000000..a07945d9f --- /dev/null +++ b/wrap/tests/expected/new_Test_b.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-01 +function result = new_Test_b(obj,value) + error('need to compile new_Test_b.cpp'); +end diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 39621c10f..6ff90a8c1 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -61,6 +61,7 @@ TEST( wrap, parse ) { EXPECT(cls.name=="Point3"); EXPECT(cls.constructors.size()==1); EXPECT(cls.methods.size()==1); + EXPECT(cls.static_methods.size()==1); // first constructor takes 3 doubles Constructor c1 = cls.constructors.front(); @@ -75,7 +76,7 @@ TEST( wrap, parse ) { // check method Method m1 = cls.methods.front(); - EXPECT(m1.returns_=="double"); + EXPECT(m1.returnVal_.returns_=="double"); EXPECT(m1.name_=="norm"); EXPECT(m1.args_.size()==0); EXPECT(m1.is_const_); @@ -96,6 +97,8 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/@Point3/Point3.m" , "actual/@Point3/Point3.m" )); EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.m" , "actual/new_Point3_ddd.m" )); EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.cpp", "actual/new_Point3_ddd.cpp")); + EXPECT(files_equal(path + "/tests/expected/Point3_StaticFunction.m" , "actual/Point3_StaticFunction.m" )); + EXPECT(files_equal(path + "/tests/expected/Point3_StaticFunction.cpp", "actual/Point3_StaticFunction.cpp")); EXPECT(files_equal(path + "/tests/expected/@Point3/norm.m" , "actual/@Point3/norm.m" )); EXPECT(files_equal(path + "/tests/expected/@Point3/norm.cpp" , "actual/@Point3/norm.cpp" )); diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index fb2f0fdc6..e00956536 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -76,3 +76,11 @@ void emit_header_comment(ofstream& ofs, const string& delimiter) { } /* ************************************************************************* */ +std::string maybe_shared_ptr(bool add, const std::string& type) { + string str = add? "shared_ptr<" : ""; + str += type; + if (add) str += ">"; + return str; +} + +/* ************************************************************************* */ diff --git a/wrap/utilities.h b/wrap/utilities.h index b5de303b8..eba80c57b 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -63,3 +63,6 @@ bool assert_equal(const std::string& expected, const std::string& actual); * emit a header at the top of generated files */ void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); + +// auxiliary function to wrap an argument into a shared_ptr template +std::string maybe_shared_ptr(bool add, const std::string& type); From 3050dc2dde47473153a0760e5cf05fd05ae47eaf Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 2 Dec 2011 16:43:15 +0000 Subject: [PATCH 32/72] Added wrap components to "wrap" namespace, added options for installing wrap program --- configure.ac | 19 +++++++++++++++++ wrap/Argument.cpp | 1 + wrap/Argument.h | 4 ++++ wrap/Class.cpp | 1 + wrap/Class.h | 4 ++++ wrap/Constructor.cpp | 5 +++-- wrap/Constructor.h | 4 ++++ wrap/Makefile.am | 46 +++++++++++++++++++++++++++++++---------- wrap/Method.cpp | 3 ++- wrap/Method.h | 4 ++++ wrap/Module.cpp | 5 +++-- wrap/Module.h | 3 +++ wrap/ReturnValue.cpp | 7 ++++--- wrap/ReturnValue.h | 4 ++++ wrap/StaticMethod.cpp | 3 ++- wrap/StaticMethod.h | 4 ++++ wrap/tests/testWrap.cpp | 1 + wrap/utilities.cpp | 8 +++++-- wrap/utilities.h | 4 ++++ wrap/wrap.cpp | 4 ++-- 20 files changed, 110 insertions(+), 24 deletions(-) diff --git a/configure.ac b/configure.ac index b1dc2fc06..fecc7f5f9 100644 --- a/configure.ac +++ b/configure.ac @@ -145,6 +145,25 @@ AC_ARG_WITH([toolbox], [toolbox=$prefix]) AC_SUBST([toolbox]) +# enable installation of the wrap utility +AC_ARG_ENABLE([install_wrap], + [ --enable-install-wrap Enable installation of the wrap tool for generating matlab interfaces], + [case "${enableval}" in + yes) install_wrap=true ;; + no) install_wrap=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-wrap]) ;; + esac],[install_wrap=false]) + +AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue]) + +# wrap install path: optional flag to change location of wrap, defaults to install prefix/bin +AC_ARG_WITH([wrap], + [AS_HELP_STRING([--with-wrap], + [specify the wrap directory for installation])], + [wrap=$withval], + [wrap=$prefix/bin]) +AC_SUBST([wrap]) + AC_CONFIG_FILES([CppUnitLite/Makefile \ wrap/Makefile \ gtsam/3rdparty/Makefile \ diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index e0d01ab63..c196212ef 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -22,6 +22,7 @@ #include "Argument.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ void Argument::matlab_unwrap(ofstream& ofs, diff --git a/wrap/Argument.h b/wrap/Argument.h index 2248e1790..3ebfa505e 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -20,6 +20,8 @@ #include #include +namespace wrap { + /// Argument class struct Argument { bool is_const, is_ref, is_ptr; @@ -50,3 +52,5 @@ struct ArgumentList: public std::list { void matlab_unwrap(std::ofstream& ofs, int start = 0); // MATLAB to C++ }; +} // \namespace wrap + diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 628e0ea83..580d898c4 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -23,6 +23,7 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ void Class::matlab_proxy(const string& classFile) { diff --git a/wrap/Class.h b/wrap/Class.h index 68ff4e1ca..226872748 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -24,6 +24,8 @@ #include "Method.h" #include "StaticMethod.h" +namespace wrap { + /// Class has name, constructors, methods struct Class { /// Constructor creates an empty class @@ -49,3 +51,5 @@ struct Class { const std::string& mexFlags); ///< emit make fragment for global make script }; +} // \namespace wrap + diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 9caf4793f..346fc45da 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -23,6 +23,7 @@ #include "Constructor.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ string Constructor::matlab_wrapper_name(const string& className) { @@ -55,7 +56,7 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& classNam if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "%"); + wrap::emit_header_comment(ofs, "%"); ofs << "function result = " << name << "(obj"; if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; @@ -81,7 +82,7 @@ void Constructor::matlab_wrapper(const string& toolboxPath, if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "//"); + wrap::emit_header_comment(ofs, "//"); ofs << "#include " << endl; ofs << "#include <" << className << ".h>" << endl; if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 340ac0fd2..93f9b145b 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -22,6 +22,8 @@ #include "Argument.h" +namespace wrap { + // Constructor class struct Constructor { @@ -53,3 +55,5 @@ struct Constructor { const std::string& className, const std::string& nameSpace); }; +} // \namespace wrap + diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 43573033c..5783ec433 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -9,15 +9,22 @@ AM_DEFAULT_SOURCE_EXT = .cpp headers = sources = -check_PROGRAMS = +check_PROGRAMS = +noinst_PROGRAMS = +wrap_PROGRAMS = +wrapdir = $(pkgincludedir)/wrap # disable all of matlab toolbox build by default if ENABLE_BUILD_TOOLBOX # Build a library from the core sources sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp Method.cpp StaticMethod.cpp Class.cpp Module.cpp -check_PROGRAMS += tests/testSpirit tests/testWrap -noinst_PROGRAMS = wrap +check_PROGRAMS += tests/testSpirit tests/testWrap +if ENABLE_INSTALL_WRAP +wrap_PROGRAMS += wrap +else +noinst_PROGRAMS += wrap +endif #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed @@ -25,7 +32,6 @@ noinst_PROGRAMS = wrap #---------------------------------------------------------------------------------------------------- # Only install the header necessary for wrap interfaces to build with mex headers += matlab.h -wrapdir = $(pkgincludedir)/wrap wrap_HEADERS = $(headers) noinst_LTLIBRARIES = libwrap.la libwrap_la_SOURCES = $(sources) @@ -55,19 +61,37 @@ nameSpace = "gtsam" mexFlags = "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" all: ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} - -# install the headers and matlab toolbox -if ENABLE_INSTALL_MATLAB_TESTS -install-exec-hook: all - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam && \ - mkdir -p ${toolbox}/gtsam/tests && \ + +wrap-install-bin: all + install -d ${wrap} && \ + install ./wrap ${wrap} + +wrap-install-matlab-tests: all + install -d ${toolbox}/gtsam/tests && \ cp -rf ../../tests/matlab/*.m ${toolbox}/gtsam/tests + +# install the headers and matlab toolbox +if ENABLE_INSTALL_WRAP +if ENABLE_INSTALL_MATLAB_TESTS +install-exec-hook: wrap-install-bin wrap-install-matlab-tests + install -d ${toolbox}/gtsam && \ + cp -rf ../toolbox/* ${toolbox}/gtsam +else +install-exec-hook: wrap-install-bin + install -d ${toolbox}/gtsam && \ + cp -rf ../toolbox/* ${toolbox}/gtsam/tests +endif +else +if ENABLE_INSTALL_MATLAB_TESTS +install-exec-hook: wrap-install-matlab-tests + install -d ${toolbox}/gtsam && \ + cp -rf ../toolbox/* ${toolbox}/gtsam else install-exec-hook: all install -d ${toolbox}/gtsam && \ cp -rf ../toolbox/* ${toolbox}/gtsam/tests endif +endif # clean local toolbox dir clean: diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 0b3833437..9319f1036 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -23,6 +23,7 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ void Method::matlab_mfile(const string& classPath) { @@ -60,7 +61,7 @@ void Method::matlab_wrapper(const string& classPath, // generate code // header - emit_header_comment(ofs, "//"); + wrap::emit_header_comment(ofs, "//"); ofs << "#include \n"; ofs << "#include <" << className << ".h>\n"; if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; diff --git a/wrap/Method.h b/wrap/Method.h index edf98d717..9d3860ff1 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -23,6 +23,8 @@ #include "Argument.h" #include "ReturnValue.h" +namespace wrap { + /// Method class struct Method { @@ -47,3 +49,5 @@ struct Method { const std::string& className, const std::string& nameSpace); ///< wrapper }; +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 47a1b0b50..230453979 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -26,6 +26,7 @@ #include using namespace std; +using namespace wrap; using namespace BOOST_SPIRIT_CLASSIC_NS; typedef rule Rule; @@ -186,7 +187,7 @@ Module::Module(const string& interfacePath, // read interface file string interfaceFile = interfacePath + "/" + moduleName + ".h"; - string contents = file_contents(interfaceFile); + string contents = wrap::file_contents(interfaceFile); // and parse contents parse_info info = parse(contents.c_str(), module_p, space_p); @@ -211,7 +212,7 @@ void Module::matlab_code(const string& toolboxPath, if(!ofs) throw CantOpenFile(makeFile); if (verbose_) cerr << "generating " << makeFile << endl; - emit_header_comment(ofs,"%"); + wrap::emit_header_comment(ofs,"%"); ofs << "echo on" << endl << endl; ofs << "toolboxpath = mfilename('fullpath');" << endl; ofs << "delims = find(toolboxpath == '/');" << endl; diff --git a/wrap/Module.h b/wrap/Module.h index 0227e86d9..8175bf719 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -22,6 +22,8 @@ #include "Class.h" +namespace wrap { + /** * A module just has a name and a list of classes */ @@ -41,3 +43,4 @@ struct Module { const std::string& mexFlags); }; +} // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index f6307954e..7ffd3b372 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -9,16 +9,17 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ string ReturnValue::return_type(bool add_ptr, pairing p) { if (p==pair && returns_pair_) { string str = "pair< " + - maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + - maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >"; + wrap::maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + + wrap::maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >"; return str; } else - return maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); + return wrap::maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); } /* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 336a415ac..02382aebb 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -11,6 +11,8 @@ #pragma once +namespace wrap { + struct ReturnValue { ReturnValue(bool verbose = true) @@ -32,3 +34,5 @@ struct ReturnValue { void wrap_result(std::ostream& ofs); }; + +} // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 0a712219e..3d380c1da 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -23,6 +23,7 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) { @@ -61,7 +62,7 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, // generate code // header - emit_header_comment(ofs, "//"); + wrap::emit_header_comment(ofs, "//"); ofs << "#include \n"; ofs << "#include <" << className << ".h>\n"; if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 11caa2eb0..0366b64ef 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -24,6 +24,8 @@ #include "Argument.h" #include "ReturnValue.h" +namespace wrap { + /// StaticMethod class struct StaticMethod { @@ -47,3 +49,5 @@ struct StaticMethod { const std::string& className, const std::string& nameSpace); ///< wrapper }; +} // \namespace wrap + diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 6ff90a8c1..98af93425 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -24,6 +24,7 @@ #include using namespace std; +using namespace wrap; static bool verbose = false; #ifdef TOPSRCDIR static string topdir = TOPSRCDIR; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index e00956536..8a30ed3bc 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -21,6 +21,8 @@ #include "utilities.h" +namespace wrap { + using namespace std; using namespace boost::gregorian; @@ -52,8 +54,8 @@ bool assert_equal(const std::string& expected, const std::string& actual) { /* ************************************************************************* */ bool files_equal(const string& expected, const string& actual, bool skipheader) { try { - string expected_contents = file_contents(expected, skipheader); - string actual_contents = file_contents(actual, skipheader); + string expected_contents = wrap::file_contents(expected, skipheader); + string actual_contents = wrap::file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { stringstream command; @@ -84,3 +86,5 @@ std::string maybe_shared_ptr(bool add, const std::string& type) { } /* ************************************************************************* */ + +} // \namespace wrap diff --git a/wrap/utilities.h b/wrap/utilities.h index eba80c57b..ee636b58c 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -19,6 +19,8 @@ #include #include +namespace wrap { + class CantOpenFile : public std::exception { private: std::string filename_; @@ -66,3 +68,5 @@ void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); // auxiliary function to wrap an argument into a shared_ptr template std::string maybe_shared_ptr(bool add, const std::string& type); + +} // \namespace wrap diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 51ba636c3..27dbd9237 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file wrap.ccp + * @file wrap.cpp * @brief wraps functions * @author Frank Dellaert **/ @@ -38,7 +38,7 @@ void generate_matlab_toolbox(const string& interfacePath, { // Parse interface file into class object // This recursively creates Class objects, Method objects, etc... - Module module(interfacePath, moduleName, true); + wrap::Module module(interfacePath, moduleName, true); // Then emit MATLAB code module.matlab_code(toolboxPath,nameSpace,mexFlags); From fa4947b99c64db83fe47f18f73cc3aad326c40e4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 5 Dec 2011 16:36:48 +0000 Subject: [PATCH 33/72] Fixed segfault issue with static functions --- wrap/StaticMethod.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 3d380c1da..196ba639f 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -73,12 +73,11 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, ofs << "{\n"; // check arguments - // extra argument obj -> nargin-1 is passed ! - // example: checkArguments("equals",nargout,nargin-1,2); + // NOTE: for static functions, there is no object passed ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args_.size() << ");\n"; // unwrap arguments, see Argument.cpp - args_.matlab_unwrap(ofs,1); + args_.matlab_unwrap(ofs,0); // We start at 0 because there is no self object // call method // example: bool result = Point2::return_field(t); From a38b7245be1fb19b1cc4274e07b7b50c5a5fbfe2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 5 Dec 2011 20:54:37 +0000 Subject: [PATCH 34/72] added an (actual) makefile to parts generated by wrap --- wrap/Class.cpp | 44 ++++++++++++++++++++ wrap/Class.h | 1 + wrap/Module.cpp | 39 +++++++++++++++--- wrap/tests/expected/Makefile | 78 ++++++++++++++++++++++++++++++++++++ wrap/tests/testWrap.cpp | 1 + 5 files changed, 158 insertions(+), 5 deletions(-) create mode 100644 wrap/tests/expected/Makefile diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 580d898c4..489a56e18 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -14,6 +14,7 @@ * @author Frank Dellaert **/ +#include #include #include @@ -93,3 +94,46 @@ void Class::matlab_make_fragment(ofstream& ofs, } /* ************************************************************************* */ +void Class::makefile_fragment(ofstream& ofs) { +// new_Point2_.$(MEXENDING): new_Point2_.cpp +// $(MEX) $(mex_flags) new_Point2_.cpp +// new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp +// $(MEX) $(mex_flags) new_Point2_dd.cpp +// @Point2/x.$(MEXENDING): @Point2/x.cpp +// $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x +// @Point2/y.$(MEXENDING): @Point2/y.cpp +// $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y +// @Point2/dim.$(MEXENDING): @Point2/dim.cpp +// $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim +// +// Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) + + // collect names + vector file_names; + BOOST_FOREACH(Constructor c, constructors) { + string file_base = c.matlab_wrapper_name(name); + file_names.push_back(file_base); + } + BOOST_FOREACH(StaticMethod c, static_methods) { + string file_base = name + "_" + c.name_; + file_names.push_back(file_base); + } + BOOST_FOREACH(Method c, methods) { + string file_base = "@" + name + "/" + c.name_; + file_names.push_back(file_base); + } + + BOOST_FOREACH(const string& file_base, file_names) { + ofs << file_base << ".$(MEXENDING): " << file_base << ".cpp" << endl; + ofs << "\t$(MEX) $(mex_flags) " << file_base << ".cpp -output " << file_base << endl; + } + + // class target + ofs << "\n" << name << ": "; + BOOST_FOREACH(const string& file_base, file_names) { + ofs << file_base << ".$(MEXENDING) "; + } + ofs << "\n" << endl; +} + +/* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index 226872748..57572b4cf 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -49,6 +49,7 @@ struct Class { void matlab_make_fragment(std::ofstream& ofs, const std::string& toolboxPath, const std::string& mexFlags); ///< emit make fragment for global make script + void makefile_fragment(std::ofstream& ofs); ///< emit makefile fragment }; } // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 230453979..a46055bcc 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -207,12 +207,17 @@ void Module::matlab_code(const string& toolboxPath, system(installCmd.c_str()); // create make m-file - string makeFile = toolboxPath + "/make_" + name + ".m"; - ofstream ofs(makeFile.c_str()); - if(!ofs) throw CantOpenFile(makeFile); + string matlabMakeFile = toolboxPath + "/make_" + name + ".m"; + ofstream ofs(matlabMakeFile.c_str()); + if(!ofs) throw CantOpenFile(matlabMakeFile); - if (verbose_) cerr << "generating " << makeFile << endl; - wrap::emit_header_comment(ofs,"%"); + // create the (actual) make file + string makeFile = toolboxPath + "/Makefile"; + ofstream make_ofs(makeFile.c_str()); + if(!make_ofs) throw CantOpenFile(makeFile); + + if (verbose_) cerr << "generating " << matlabMakeFile << endl; + emit_header_comment(ofs,"%"); ofs << "echo on" << endl << endl; ofs << "toolboxpath = mfilename('fullpath');" << endl; ofs << "delims = find(toolboxpath == '/');" << endl; @@ -220,6 +225,12 @@ void Module::matlab_code(const string& toolboxPath, ofs << "clear delims" << endl; ofs << "addpath(toolboxpath);" << endl << endl; + if (verbose_) cerr << "generating " << makeFile << endl; + emit_header_comment(make_ofs,"#"); + make_ofs << "\nMEX = mex\n"; + make_ofs << "MEXENDING = mexa64\n"; + make_ofs << "mex_flags = " << mexFlags << "\n\n"; + // generate proxy classes and wrappers BOOST_FOREACH(Class cls, classes) { // create directory if needed @@ -240,12 +251,30 @@ void Module::matlab_code(const string& toolboxPath, ofs << "%% " << cls.name << endl; ofs << "cd(toolboxpath)" << endl; cls.matlab_make_fragment(ofs, toolboxPath, mexFlags); + + // add section to the (actual) make file + make_ofs << "# " << cls.name << endl; + cls.makefile_fragment(make_ofs); } // finish make m-file ofs << "cd(toolboxpath)" << endl << endl; ofs << "echo off" << endl; ofs.close(); + + // add 'all' and 'clean' to Makefile + make_ofs << "\nall: "; + BOOST_FOREACH(Class cls, classes) + make_ofs << cls.name << " "; + + make_ofs << "\n\nclean: \n"; + make_ofs << "\trm -rf *.$(MEXENDING)\n"; + BOOST_FOREACH(Class cls, classes) + make_ofs << "\trm -rf @" << cls.name << "/*.$(MEXENDING)\n"; + + // finish Makefile + make_ofs << "\n" << endl; + make_ofs.close(); } catch(exception &e) { cerr << "generate_matlab_toolbox failed because " << e.what() << endl; diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile new file mode 100644 index 000000000..940be4283 --- /dev/null +++ b/wrap/tests/expected/Makefile @@ -0,0 +1,78 @@ +# automatically generated by wrap on 2011-Dec-05 + +MEX = mex +MEXENDING = mexa64 +mex_flags = -O5 + +# Point2 +new_Point2_.$(MEXENDING): new_Point2_.cpp + $(MEX) $(mex_flags) new_Point2_.cpp -output new_Point2_ +new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp + $(MEX) $(mex_flags) new_Point2_dd.cpp -output new_Point2_dd +@Point2/x.$(MEXENDING): @Point2/x.cpp + $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x +@Point2/y.$(MEXENDING): @Point2/y.cpp + $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y +@Point2/dim.$(MEXENDING): @Point2/dim.cpp + $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim + +Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) + +# Point3 +new_Point3_ddd.$(MEXENDING): new_Point3_ddd.cpp + $(MEX) $(mex_flags) new_Point3_ddd.cpp -output new_Point3_ddd +Point3_StaticFunction.$(MEXENDING): Point3_StaticFunction.cpp + $(MEX) $(mex_flags) Point3_StaticFunction.cpp -output Point3_StaticFunction +@Point3/norm.$(MEXENDING): @Point3/norm.cpp + $(MEX) $(mex_flags) @Point3/norm.cpp -output @Point3/norm + +Point3: new_Point3_ddd.$(MEXENDING) Point3_StaticFunction.$(MEXENDING) @Point3/norm.$(MEXENDING) + +# Test +new_Test_.$(MEXENDING): new_Test_.cpp + $(MEX) $(mex_flags) new_Test_.cpp -output new_Test_ +@Test/return_bool.$(MEXENDING): @Test/return_bool.cpp + $(MEX) $(mex_flags) @Test/return_bool.cpp -output @Test/return_bool +@Test/return_size_t.$(MEXENDING): @Test/return_size_t.cpp + $(MEX) $(mex_flags) @Test/return_size_t.cpp -output @Test/return_size_t +@Test/return_int.$(MEXENDING): @Test/return_int.cpp + $(MEX) $(mex_flags) @Test/return_int.cpp -output @Test/return_int +@Test/return_double.$(MEXENDING): @Test/return_double.cpp + $(MEX) $(mex_flags) @Test/return_double.cpp -output @Test/return_double +@Test/return_string.$(MEXENDING): @Test/return_string.cpp + $(MEX) $(mex_flags) @Test/return_string.cpp -output @Test/return_string +@Test/return_vector1.$(MEXENDING): @Test/return_vector1.cpp + $(MEX) $(mex_flags) @Test/return_vector1.cpp -output @Test/return_vector1 +@Test/return_matrix1.$(MEXENDING): @Test/return_matrix1.cpp + $(MEX) $(mex_flags) @Test/return_matrix1.cpp -output @Test/return_matrix1 +@Test/return_vector2.$(MEXENDING): @Test/return_vector2.cpp + $(MEX) $(mex_flags) @Test/return_vector2.cpp -output @Test/return_vector2 +@Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp + $(MEX) $(mex_flags) @Test/return_matrix2.cpp -output @Test/return_matrix2 +@Test/return_pair.$(MEXENDING): @Test/return_pair.cpp + $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair +@Test/return_field.$(MEXENDING): @Test/return_field.cpp + $(MEX) $(mex_flags) @Test/return_field.cpp -output @Test/return_field +@Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp + $(MEX) $(mex_flags) @Test/return_TestPtr.cpp -output @Test/return_TestPtr +@Test/return_Point2Ptr.$(MEXENDING): @Test/return_Point2Ptr.cpp + $(MEX) $(mex_flags) @Test/return_Point2Ptr.cpp -output @Test/return_Point2Ptr +@Test/create_ptrs.$(MEXENDING): @Test/create_ptrs.cpp + $(MEX) $(mex_flags) @Test/create_ptrs.cpp -output @Test/create_ptrs +@Test/return_ptrs.$(MEXENDING): @Test/return_ptrs.cpp + $(MEX) $(mex_flags) @Test/return_ptrs.cpp -output @Test/return_ptrs +@Test/print.$(MEXENDING): @Test/print.cpp + $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print + +Test: new_Test_.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) + + +all: Point2 Point3 Test + +clean: + rm -rf *.$(MEXENDING) + rm -rf @Point2/*.$(MEXENDING) + rm -rf @Point3/*.$(MEXENDING) + rm -rf @Test/*.$(MEXENDING) + + diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 98af93425..96c73883c 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -115,6 +115,7 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/@Test/print.cpp" , "actual/@Test/print.cpp" )); EXPECT(files_equal(path + "/tests/expected/make_geometry.m" , "actual/make_geometry.m" )); + EXPECT(files_equal(path + "/tests/expected/Makefile" , "actual/Makefile" )); } /* ************************************************************************* */ From eec3f0f3703931a259ebf9676e10918cf67a2f45 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 5 Dec 2011 20:54:41 +0000 Subject: [PATCH 35/72] Adding support for mex extensions in wrap, configure scripts slightly broken --- configure.ac | 39 +++++++++++++++++++++++++++------------ wrap/Makefile.am | 29 ++++++++++++++++++++++++++--- wrap/Module.cpp | 3 ++- wrap/Module.h | 3 ++- wrap/tests/testWrap.cpp | 2 +- wrap/wrap.cpp | 11 +++++++---- 6 files changed, 65 insertions(+), 22 deletions(-) diff --git a/configure.ac b/configure.ac index fecc7f5f9..ee6950438 100644 --- a/configure.ac +++ b/configure.ac @@ -30,8 +30,11 @@ DX_PS_FEATURE(OFF) DX_INIT_DOXYGEN(gtsam) # Check for OS -AC_CANONICAL_HOST # needs to be called at some point earlier -AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) +# needs to be called at some point earlier +AC_CANONICAL_HOST +AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) +AM_CONDITIONAL([LINUX], [case $host_os in linux*) true;; *) false;; esac]) +AM_CONDITIONAL([IS_64BIT], [case $host_cpu in *x86_64*) true;; *) false;; esac]) # enable debug variable AC_ARG_ENABLE([debug], @@ -45,18 +48,19 @@ AC_ARG_ENABLE([debug], AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) -AC_CANONICAL_HOST +# AGC: isn't this redundant? +#AC_CANONICAL_HOST # why was this called twice? # We need to determine what os we are on to determine if we need to do # special things because we are on a mac -case $host_os in - darwin* ) - # Do something specific for mac - ISMAC=true - ;; - *) - ISMAC=false - ;; -esac +# case $host_os in +# darwin* ) +# # Do something specific for mac +# ISMAC=true +# ;; +# *) +# ISMAC=false +# ;; +# esac # enable profiling AC_ARG_ENABLE([profiling], @@ -156,6 +160,17 @@ AC_ARG_ENABLE([install_wrap], AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue]) +# enable unsafe mode for wrap +AC_ARG_ENABLE([unsafe_wrap], + [ --enable-unsafe-wrap Enable using unsafe mode in wrap], + [case "${enableval}" in + yes) unsafe_wrap=true ;; + no) unsafe_wrap=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-unsafe-wrap]) ;; + esac],[unsafe_wrap=false]) + +AM_CONDITIONAL([ENABLE_UNSAFE_WRAP], [test x$unsafe_wrap = xtrue]) + # wrap install path: optional flag to change location of wrap, defaults to install prefix/bin AC_ARG_WITH([wrap], [AS_HELP_STRING([--with-wrap], diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 5783ec433..d572b88e4 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -52,15 +52,38 @@ LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a # rule to run executable with valgrind %.valgrind: % $(LDADD) valgrind ./$^ - + # generate local toolbox dir interfacePath = $(top_srcdir) moduleName = gtsam toolboxpath = ../toolbox nameSpace = "gtsam" -mexFlags = "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +mexFlags = +mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +if ENABLE_UNSAFE_WRAP + mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +else + mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +endif + +# Find the extension for mex binaries - FIXME: this should be done with mexext with matlab +mexextension = mexa64 +if LINUX +if IS_64BIT + mexextension += mexa64 +else + mexextension += mexglx +endif +else # Linux +if DARWIN + mexextension += mexmaci64 +else + mexextension += mex_bin +endif +endif # Linux + all: - ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} + ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} wrap-install-bin: all install -d ${wrap} && \ diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a46055bcc..3e70fe947 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -200,6 +200,7 @@ Module::Module(const string& interfacePath, /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, const string& nameSpace, + const string& mexExt, const string& mexFlags) { try { @@ -228,7 +229,7 @@ void Module::matlab_code(const string& toolboxPath, if (verbose_) cerr << "generating " << makeFile << endl; emit_header_comment(make_ofs,"#"); make_ofs << "\nMEX = mex\n"; - make_ofs << "MEXENDING = mexa64\n"; + make_ofs << "MEXENDING = " << mexExt << "\n"; make_ofs << "mex_flags = " << mexFlags << "\n\n"; // generate proxy classes and wrappers diff --git a/wrap/Module.h b/wrap/Module.h index 8175bf719..e47057a63 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -33,13 +33,14 @@ struct Module { bool verbose_; ///< verbose flag /// constructor that parses interface file - Module(const std::string& interfacePath, + Module(const std::string& interfacePath, const std::string& moduleName, bool verbose=true); /// MATLAB code generation: void matlab_code(const std::string& path, const std::string& nameSpace, + const std::string& mexExt, const std::string& mexFlags); }; diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 96c73883c..e48ae34d7 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -91,7 +91,7 @@ TEST( wrap, matlab_code ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual", "", "-O5"); + module.matlab_code("mexa64", "actual", "", "-O5"); EXPECT(files_equal(path + "/tests/expected/@Point2/Point2.m" , "actual/@Point2/Point2.m" )); EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" )); diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 27dbd9237..4fdd2d3fa 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -24,13 +24,15 @@ using namespace std; /** * Top-level function to wrap a module + * @param mexExt is the extension for mex binaries for this os/cpu * @param interfacePath path to where interface file lives, e.g., borg/gtsam * @param moduleName name of the module to be generated e.g. gtsam * @param toolboxPath path where the toolbox should be generated, e.g. borg/gtsam/build * @param nameSpace e.g. gtsam * @param mexFlags extra arguments for mex script, i.e., include flags etc... */ -void generate_matlab_toolbox(const string& interfacePath, +void generate_matlab_toolbox(const string& mexExt, + const string& interfacePath, const string& moduleName, const string& toolboxPath, const string& nameSpace, @@ -41,7 +43,7 @@ void generate_matlab_toolbox(const string& interfacePath, wrap::Module module(interfacePath, moduleName, true); // Then emit MATLAB code - module.matlab_code(toolboxPath,nameSpace,mexFlags); + module.matlab_code(toolboxPath,nameSpace,mexExt,mexFlags); } /** @@ -49,9 +51,10 @@ void generate_matlab_toolbox(const string& interfacePath, * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { - if (argc<5 || argc>6) { + if (argc<6 || argc>7) { cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; cerr << "usage: wrap interfacePath moduleName toolboxPath" << endl; + cerr << " mexExtension : OS/CPU-dependent extension for MEX binaries" << endl; cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; @@ -59,6 +62,6 @@ int main(int argc, const char* argv[]) { cerr << " [mexFlags] : extra flags for the mex command" << endl; } else - generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argc==5 ? " " : argv[5]); + generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argv[5],argc==6 ? " " : argv[6]); return 0; } From 58ace25e33bdbd347246b25a79e6286425f9d4fc Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 5 Dec 2011 21:18:25 +0000 Subject: [PATCH 36/72] Fixed discarded qualifier with certain methods --- wrap/Method.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 9319f1036..fbfce29a7 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -78,7 +78,7 @@ void Method::matlab_wrapper(const string& classPath, // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - ofs << " shared_ptr<" << ((is_const_) ? "const " : "") << className << "> self = unwrap_shared_ptr< " << className + ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className << " >(in[0],\"" << className << "\");" << endl; // unwrap arguments, see Argument.cpp From e036dd460ea0440012f92b4449bf1eb50245f8da Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 6 Dec 2011 16:21:57 +0000 Subject: [PATCH 37/72] support cloning (deep copy) for ISAM2 --- gtsam/inference/BayesTree.h | 19 ++++++ gtsam/inference/Permutation.h | 7 +++ gtsam/linear/GaussianFactor.h | 3 + gtsam/linear/HessianFactor.h | 6 ++ gtsam/linear/JacobianFactor.h | 6 ++ gtsam/nonlinear/GaussianISAM2.h | 6 ++ gtsam/nonlinear/ISAM2.h | 23 +++++++ tests/testGaussianISAM2.cpp | 104 ++++++++++++++++++++++++++++++++ 8 files changed, 174 insertions(+) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index ebe59d135..1b762e210 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -81,6 +81,20 @@ namespace gtsam { Clique(const sharedConditional& conditional); + void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const { + sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_)); + sharedClique newClique = newTree.addClique(newConditional, parent_clique); + if (cachedFactor_) + newClique->cachedFactor_ = cachedFactor_->clone(); + else newClique->cachedFactor_ = typename FactorType::shared_ptr(); + if (!parent_clique) { + newTree.root_ = newClique; + } + BOOST_FOREACH(const shared_ptr& childClique, children_) { + childClique->cloneToBayesTree(newTree, newClique); + } + } + /** print this node */ void print(const std::string& s = "") const; @@ -264,6 +278,11 @@ namespace gtsam { /** check equality */ bool equals(const BayesTree& other, double tol = 1e-9) const; + /** deep copy from another tree */ + void cloneTo(shared_ptr& newTree) const { + root_->cloneToBayesTree(*newTree); + } + /** * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 421d03ad5..99d717681 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -174,6 +174,13 @@ public: /** Access the container through the permutation (const version) */ const value_type& operator[](size_t index) const { return container_[permutation_[index]]; } + /** Assignment operator for cloning in ISAM2 */ + Permuted operator=(const Permuted& other) { + permutation_ = other.permutation_; + container_ = other.container_; + return *this; + } + /** Permute this view by applying a permutation to the underlying permutation */ void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 7f4b90748..2d889ee64 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -88,6 +88,9 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** Clone a factor (make a deep copy) */ + virtual GaussianFactor::shared_ptr clone() const = 0; + /** * Permutes the GaussianFactor, but for efficiency requires the permutation * to already be inverted. This acts just as a change-of-name for each diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index f2de0be4c..88e4364ae 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -191,6 +191,12 @@ namespace gtsam { /** Destructor */ virtual ~HessianFactor() {} + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new HessianFactor(*this))); + } + /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "") const; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b8f40d6ef..09089df20 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -136,6 +136,12 @@ namespace gtsam { virtual ~JacobianFactor() {} + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new JacobianFactor(*this))); + } + // Implementing Testable interface virtual void print(const std::string& s = "") const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index d4d177815..fe29936cb 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -27,12 +27,18 @@ namespace gtsam { */ template > class GaussianISAM2 : public ISAM2 { + typedef ISAM2 Base; public: /** Create an empty ISAM2 instance */ GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ GaussianISAM2() : ISAM2() {} + + void cloneTo(boost::shared_ptr& newGaussianISAM2) const { + boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); + Base::cloneTo(isam2); + } }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 1fd077c3c..50b22f1c5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -167,6 +167,27 @@ public: /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ ISAM2(); + void cloneTo(boost::shared_ptr& newISAM2) const { + boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); + Base::cloneTo(bayesTree); + newISAM2->theta_ = theta_; + newISAM2->variableIndex_ = variableIndex_; + newISAM2->deltaUnpermuted_ = deltaUnpermuted_; + newISAM2->delta_ = delta_; + newISAM2->nonlinearFactors_ = nonlinearFactors_; + newISAM2->ordering_ = ordering_; + newISAM2->params_ = params_; +#ifndef NDEBUG + newISAM2->lastRelinVariables_ = lastRelinVariables_; +#endif + newISAM2->lastAffectedVariableCount = lastAffectedVariableCount; + newISAM2->lastAffectedFactorCount = lastAffectedFactorCount; + newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount; + newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount; + newISAM2->lastBacksubVariableCount = lastBacksubVariableCount; + newISAM2->lastNnzTop = lastNnzTop; + } + typedef typename BayesTree::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename BayesTree::Cliques Cliques; ///< List of Clique typedef from base class @@ -235,6 +256,8 @@ public: size_t lastBacksubVariableCount; size_t lastNnzTop; + ISAM2Params params() const { return params_; } + //@} private: diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index ad9855fa5..979f7c7c8 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution) EXPECT(isam_check(fullgraph, fullinit, isam)); } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, clone) { + + // Pose and landmark key types from planarSLAM + typedef planarSLAM::PoseKey PoseKey; + typedef planarSLAM::PointKey PointKey; + + // Set up parameters + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + GaussianISAM2 isam(ISAM2Params(0.001, 0.0, 0, false, true)); + planarSLAM::Values fullinit; + planarSLAM::Graph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + EXPECT(isam_check(fullgraph, fullinit, isam)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + + // CLONING... + boost::shared_ptr > isam2 + = boost::shared_ptr >(new GaussianISAM2()); + isam.cloneTo(isam2); + + CHECK(assert_equal(isam, *isam2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 58939b56843410ddc250be4513a2d2ec6a9aaec1 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 6 Dec 2011 16:50:27 +0000 Subject: [PATCH 38/72] Made the 'all' target work properly in the wrap-generated makefile --- wrap/Module.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 3e70fe947..5f150b8d2 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -232,6 +232,12 @@ void Module::matlab_code(const string& toolboxPath, make_ofs << "MEXENDING = " << mexExt << "\n"; make_ofs << "mex_flags = " << mexFlags << "\n\n"; + // add 'all' to Makefile + make_ofs << "all: "; + BOOST_FOREACH(Class cls, classes) + make_ofs << cls.name << " "; + make_ofs << "\n\n"; + // generate proxy classes and wrappers BOOST_FOREACH(Class cls, classes) { // create directory if needed @@ -263,11 +269,7 @@ void Module::matlab_code(const string& toolboxPath, ofs << "echo off" << endl; ofs.close(); - // add 'all' and 'clean' to Makefile - make_ofs << "\nall: "; - BOOST_FOREACH(Class cls, classes) - make_ofs << cls.name << " "; - + // make clean at end of Makefile make_ofs << "\n\nclean: \n"; make_ofs << "\trm -rf *.$(MEXENDING)\n"; BOOST_FOREACH(Class cls, classes) From dcc3e8d0f117ce4c3e1fb3bc8d95cc56fa3a1a7f Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Dec 2011 03:05:30 +0000 Subject: [PATCH 39/72] wrap: fixed errors in static function matlab codegen, added mechanism to allow for returning classes without shared_ptr --- .cproject | 275 ++++++++++-------- gtsam.h | 4 +- wrap/Module.cpp | 10 +- wrap/ReturnValue.cpp | 5 + wrap/ReturnValue.h | 5 +- wrap/StaticMethod.cpp | 8 +- wrap/geometry.h | 5 +- wrap/matlab.h | 1 + wrap/tests/expected/@Point2/dim.cpp | 4 +- wrap/tests/expected/@Point2/x.cpp | 4 +- wrap/tests/expected/@Point2/y.cpp | 4 +- wrap/tests/expected/@Point3/norm.cpp | 4 +- wrap/tests/expected/@Test/create_ptrs.cpp | 4 +- wrap/tests/expected/@Test/print.cpp | 4 +- .../tests/expected/@Test/return_Point2Ptr.cpp | 4 +- wrap/tests/expected/@Test/return_TestPtr.cpp | 4 +- wrap/tests/expected/@Test/return_bool.cpp | 4 +- wrap/tests/expected/@Test/return_double.cpp | 4 +- wrap/tests/expected/@Test/return_field.cpp | 4 +- wrap/tests/expected/@Test/return_int.cpp | 4 +- wrap/tests/expected/@Test/return_matrix1.cpp | 6 +- wrap/tests/expected/@Test/return_matrix2.cpp | 6 +- wrap/tests/expected/@Test/return_pair.cpp | 4 +- wrap/tests/expected/@Test/return_ptrs.cpp | 4 +- wrap/tests/expected/@Test/return_size_t.cpp | 4 +- wrap/tests/expected/@Test/return_string.cpp | 4 +- wrap/tests/expected/@Test/return_vector1.cpp | 6 +- wrap/tests/expected/@Test/return_vector2.cpp | 6 +- wrap/tests/expected/Makefile | 13 +- wrap/tests/expected/Point3_StaticFunction.m | 4 - .../expected/Point3_StaticFunctionRet.cpp | 9 + .../tests/expected/Point3_StaticFunctionRet.m | 4 + ...Function.cpp => Point3_staticFunction.cpp} | 6 +- wrap/tests/expected/Point3_staticFunction.m | 4 + wrap/tests/expected/make_geometry.m | 5 +- wrap/tests/expected/new_Point2_.cpp | 2 +- wrap/tests/expected/new_Point2_.m | 2 +- wrap/tests/expected/new_Point2_dd.cpp | 2 +- wrap/tests/expected/new_Point2_dd.m | 2 +- wrap/tests/expected/new_Point3_ddd.cpp | 2 +- wrap/tests/expected/new_Point3_ddd.m | 2 +- wrap/tests/expected/new_Test_.cpp | 2 +- wrap/tests/expected/new_Test_.m | 2 +- wrap/tests/testWrap.cpp | 10 +- 44 files changed, 273 insertions(+), 199 deletions(-) delete mode 100644 wrap/tests/expected/Point3_StaticFunction.m create mode 100644 wrap/tests/expected/Point3_StaticFunctionRet.cpp create mode 100644 wrap/tests/expected/Point3_StaticFunctionRet.m rename wrap/tests/expected/{Point3_StaticFunction.cpp => Point3_staticFunction.cpp} (51%) create mode 100644 wrap/tests/expected/Point3_staticFunction.m diff --git a/.cproject b/.cproject index bdbc3c0dd..05b53a07a 100644 --- a/.cproject +++ b/.cproject @@ -375,14 +375,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -409,6 +401,7 @@ make + tests/testBayesTree.run true false @@ -416,6 +409,7 @@ make + testBinaryBayesNet.run true false @@ -463,6 +457,7 @@ make + testSymbolicBayesNet.run true false @@ -470,6 +465,7 @@ make + tests/testSymbolicFactor.run true false @@ -477,6 +473,7 @@ make + testSymbolicFactorGraph.run true false @@ -492,11 +489,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -523,7 +529,6 @@ make - testGraph.run true false @@ -595,7 +600,6 @@ make - testInference.run true false @@ -603,7 +607,6 @@ make - testGaussianFactor.run true false @@ -611,7 +614,6 @@ make - testJunctionTree.run true false @@ -619,7 +621,6 @@ make - testSymbolicBayesNet.run true false @@ -627,7 +628,6 @@ make - testSymbolicFactorGraph.run true false @@ -681,20 +681,20 @@ true true - + make - -j2 + -j5 all true - true + false true - + make - -j2 - clean + -j5 + check true - true + false true @@ -713,6 +713,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -737,7 +753,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -745,6 +769,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -785,15 +817,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -801,14 +825,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -1131,6 +1147,7 @@ make + testErrors.run true false @@ -1538,7 +1555,6 @@ make - testSimulated2DOriented.run true false @@ -1578,7 +1594,6 @@ make - testSimulated2D.run true false @@ -1586,7 +1601,6 @@ make - testSimulated3D.run true false @@ -1834,7 +1848,6 @@ make - tests/testGaussianISAM2 true false @@ -1856,6 +1869,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 @@ -1952,78 +2005,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - make -j2 @@ -2064,6 +2045,70 @@ true true + + make + -j5 + check + true + false + true + + + make + -j5 + install + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + diff --git a/gtsam.h b/gtsam.h index 6aa6546f5..23fa08987 100644 --- a/gtsam.h +++ b/gtsam.h @@ -8,9 +8,8 @@ * Constructors must appear in a class before any methods * Methods can only return Matrix, Vector, double, int, void, and a shared_ptr to any other object * Comments can use either C++ or C style - * Static methods are not supported - FIXED * Methods must start with a lowercase letter - * Static methods must start with an uppercase letter + * Static methods must start with a letter (upper or lowercase) and use the "static" keyword * Classes must start with an uppercase letter */ @@ -66,6 +65,7 @@ class Rot3 { Rot3(Matrix R); static Rot3* Expmap_(Vector v); static Vector Logmap(const Rot3& p); + static Rot3 ypr(double y, double p, double r); Matrix matrix() const; Matrix transpose() const; Vector xyz() const; diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 5f150b8d2..da481b9d5 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -102,14 +102,18 @@ Module::Module(const string& interfacePath, [push_back_a(cls.constructors, constructor)] [assign_a(constructor,constructor0)]; + Rule returnClass1_p = className_p [assign_a(retVal.returns_class_, true)]; + + Rule returnClass2_p = className_p [assign_a(retVal.returns_class2_, true)]; + Rule returnType1_p = basisType_p[assign_a(retVal.returns_)] | - ((className_p | "Vector" | "Matrix")[assign_a(retVal.returns_)] >> + ((returnClass1_p | "Vector" | "Matrix")[assign_a(retVal.returns_)] >> !ch_p('*') [assign_a(retVal.returns_ptr_,true)]); Rule returnType2_p = basisType_p[assign_a(retVal.returns2_)] | - ((className_p | "Vector" | "Matrix")[assign_a(retVal.returns2_)] >> + ((returnClass2_p | "Vector" | "Matrix")[assign_a(retVal.returns2_)] >> !ch_p('*') [assign_a(retVal.returns_ptr2_,true)]); Rule pair_p = @@ -133,7 +137,7 @@ Module::Module(const string& interfacePath, [push_back_a(cls.methods, method)] [assign_a(method,method0)]; - Rule staticMethodName_p = lexeme_d[upper_p >> *(alnum_p | '_')]; + Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; Rule static_method_p = (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name_)] >> diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 7ffd3b372..005cfea40 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -25,10 +25,13 @@ string ReturnValue::return_type(bool add_ptr, pairing p) { /* ************************************************************************* */ void ReturnValue::wrap_result(std::ostream& ofs) { if (returns_pair_) { + // first return value in pair if (returns_ptr_) ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n"; else ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; + + // second return value in pair if (returns_ptr2_) ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n"; else @@ -36,6 +39,8 @@ void ReturnValue::wrap_result(std::ostream& ofs) { } else if (returns_ptr_) ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n"; + else if (returns_class_) + ofs << " out[0] = wrap_shared_ptr(make_shared< " << returns_ << " >(result),\"" << returns_ << "\");\n"; else if (returns_!="void") ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 02382aebb..68c0fb6da 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -16,12 +16,13 @@ namespace wrap { struct ReturnValue { ReturnValue(bool verbose = true) - : verbose_(verbose), returns_ptr_(false), returns_ptr2_(false), returns_pair_(false) + : verbose_(verbose), returns_ptr_(false), returns_ptr2_(false), + returns_pair_(false), returns_class_(false) {} bool verbose_; std::string returns_, returns2_; - bool returns_ptr_, returns_ptr2_, returns_pair_; + bool returns_ptr_, returns_ptr2_, returns_pair_, returns_class_, returns_class2_; typedef enum { arg1, arg2, pair diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 196ba639f..40ab9bcb8 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -38,9 +38,9 @@ void StaticMethod::matlab_mfile(const string& toolboxPath, const string& classNa // generate code string returnType = returnVal_.matlab_returnType(); ofs << "function " << returnType << " = " << full_name << "("; - if (args_.size()) ofs << "," << args_.names(); + if (args_.size()) ofs << args_.names(); ofs << ")" << endl; - ofs << "% usage: obj." << full_name << "(" << args_.names() << ")" << endl; + ofs << "% usage: x = " << full_name << "(" << args_.names() << ")" << endl; ofs << " error('need to compile " << full_name << ".cpp');" << endl; ofs << "end" << endl; @@ -79,9 +79,9 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, // unwrap arguments, see Argument.cpp args_.matlab_unwrap(ofs,0); // We start at 0 because there is no self object - // call method - // example: bool result = Point2::return_field(t); ofs << " "; + + // call method with default type if (returnVal_.returns_!="void") ofs << returnVal_.return_type(true,ReturnValue::pair) << " result = "; ofs << className << "::" << name_ << "(" << args_.names() << ");\n"; diff --git a/wrap/geometry.h b/wrap/geometry.h index e19f9f3cf..f89b7f132 100644 --- a/wrap/geometry.h +++ b/wrap/geometry.h @@ -12,8 +12,9 @@ class Point3 { Point3(double x, double y, double z); double norm() const; - // Testing: static functions - use static keyword and uppercase - static double StaticFunction(); + // static functions - use static keyword and uppercase + static double staticFunction(); + static Point3 StaticFunctionRet(double z); }; // another comment diff --git a/wrap/matlab.h b/wrap/matlab.h index 31ee4dc80..add125cb5 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -27,6 +27,7 @@ extern "C" { } #include +#include #include #include diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index c2002446b..1d6486356 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); int result = self->dim(); out[0] = wrap< int >(result); } diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp index 8777b3a56..4c2ef7b33 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); double result = self->x(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index f2e39d92e..525238ea9 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); double result = self->y(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 732f3f4c8..4c494a5f8 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("norm",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); + shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); double result = self->norm(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index 76e5ce40c..c09be35c1 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); pair< shared_ptr, shared_ptr > result = self->create_ptrs(); out[0] = wrap_shared_ptr(result.first,"Test"); out[1] = wrap_shared_ptr(result.second,"Test"); diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index 24c3a2258..0cdd5d5f1 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,9 +1,9 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); self->print(); } diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp index 0d6c35fce..6079eb972 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.cpp +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); bool value = unwrap< bool >(in[1]); shared_ptr result = self->return_Point2Ptr(value); out[0] = wrap_shared_ptr(result,"Point2"); diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index a9d4ef983..ae34da13b 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); shared_ptr result = self->return_TestPtr(value); out[0] = wrap_shared_ptr(result,"Test"); diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp index 3f0aaa8e4..5fd50cf3f 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); bool value = unwrap< bool >(in[1]); bool result = self->return_bool(value); out[0] = wrap< bool >(result); diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp index 9fd1e0edb..ef1704b07 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); double value = unwrap< double >(in[1]); double result = self->return_double(value); out[0] = wrap< double >(result); diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp index 5a211da3a..593d4b5fe 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Test& t = *unwrap_shared_ptr< Test >(in[1], "Test"); bool result = self->return_field(t); out[0] = wrap< bool >(result); diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp index c349622d7..429ef73ce 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); int value = unwrap< int >(in[1]); int result = self->return_int(value); out[0] = wrap< int >(result); diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp index 1dfcb53f0..0f3966361 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix value = unwrap< Matrix >(in[1]); Matrix result = self->return_matrix1(value); - out[0] = wrap< Matrix >(result); + out[0] = wrap_shared_ptr(make_shared< Matrix >(result),"Matrix"); } diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp index 422642917..1c16e910e 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix value = unwrap< Matrix >(in[1]); Matrix result = self->return_matrix2(value); - out[0] = wrap< Matrix >(result); + out[0] = wrap_shared_ptr(make_shared< Matrix >(result),"Matrix"); } diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp index 0a9a8378a..7224f5150 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector v = unwrap< Vector >(in[1]); Matrix A = unwrap< Matrix >(in[2]); pair< Vector, Matrix > result = self->return_pair(v,A); diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp index 556b5607c..59f8d8739 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "Test"); shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "Test"); pair< shared_ptr, shared_ptr > result = self->return_ptrs(p1,p2); diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp index e8358e1d0..887addc3f 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); size_t value = unwrap< size_t >(in[1]); size_t result = self->return_size_t(value); out[0] = wrap< size_t >(result); diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp index bd5eb9c22..9b6ee320f 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,10 +1,10 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); string value = unwrap< string >(in[1]); string result = self->return_string(value); out[0] = wrap< string >(result); diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp index 0977c7de9..4614e660d 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector value = unwrap< Vector >(in[1]); Vector result = self->return_vector1(value); - out[0] = wrap< Vector >(result); + out[0] = wrap_shared_ptr(make_shared< Vector >(result),"Vector"); } diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp index af65b8434..6fa6f7863 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector value = unwrap< Vector >(in[1]); Vector result = self->return_vector2(value); - out[0] = wrap< Vector >(result); + out[0] = wrap_shared_ptr(make_shared< Vector >(result),"Vector"); } diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile index 940be4283..7249daaba 100644 --- a/wrap/tests/expected/Makefile +++ b/wrap/tests/expected/Makefile @@ -1,9 +1,11 @@ -# automatically generated by wrap on 2011-Dec-05 +# automatically generated by wrap on 2011-Dec-06 MEX = mex MEXENDING = mexa64 mex_flags = -O5 +all: Point2 Point3 Test + # Point2 new_Point2_.$(MEXENDING): new_Point2_.cpp $(MEX) $(mex_flags) new_Point2_.cpp -output new_Point2_ @@ -21,12 +23,14 @@ Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDIN # Point3 new_Point3_ddd.$(MEXENDING): new_Point3_ddd.cpp $(MEX) $(mex_flags) new_Point3_ddd.cpp -output new_Point3_ddd -Point3_StaticFunction.$(MEXENDING): Point3_StaticFunction.cpp - $(MEX) $(mex_flags) Point3_StaticFunction.cpp -output Point3_StaticFunction +Point3_staticFunction.$(MEXENDING): Point3_staticFunction.cpp + $(MEX) $(mex_flags) Point3_staticFunction.cpp -output Point3_staticFunction +Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp + $(MEX) $(mex_flags) Point3_StaticFunctionRet.cpp -output Point3_StaticFunctionRet @Point3/norm.$(MEXENDING): @Point3/norm.cpp $(MEX) $(mex_flags) @Point3/norm.cpp -output @Point3/norm -Point3: new_Point3_ddd.$(MEXENDING) Point3_StaticFunction.$(MEXENDING) @Point3/norm.$(MEXENDING) +Point3: new_Point3_ddd.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_StaticFunctionRet.$(MEXENDING) @Point3/norm.$(MEXENDING) # Test new_Test_.$(MEXENDING): new_Test_.cpp @@ -67,7 +71,6 @@ new_Test_.$(MEXENDING): new_Test_.cpp Test: new_Test_.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) -all: Point2 Point3 Test clean: rm -rf *.$(MEXENDING) diff --git a/wrap/tests/expected/Point3_StaticFunction.m b/wrap/tests/expected/Point3_StaticFunction.m deleted file mode 100644 index cdb0d483d..000000000 --- a/wrap/tests/expected/Point3_StaticFunction.m +++ /dev/null @@ -1,4 +0,0 @@ -function result = Point3_StaticFunction(obj) -% usage: obj.Point3_StaticFunction() - error('need to compile Point3_StaticFunction.cpp'); -end diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.cpp b/wrap/tests/expected/Point3_StaticFunctionRet.cpp new file mode 100644 index 000000000..1877b7e99 --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunctionRet.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-06 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("Point3_StaticFunctionRet",nargout,nargin,0); + Point3 result = Point3::StaticFunctionRet(); + out[0] = wrap_shared_ptr(make_shared< Point3 >(result),"Point3"); +} diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.m b/wrap/tests/expected/Point3_StaticFunctionRet.m new file mode 100644 index 000000000..f2ba314f1 --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunctionRet.m @@ -0,0 +1,4 @@ +function result = Point3_StaticFunctionRet() +% usage: x = Point3_StaticFunctionRet() + error('need to compile Point3_StaticFunctionRet.cpp'); +end diff --git a/wrap/tests/expected/Point3_StaticFunction.cpp b/wrap/tests/expected/Point3_staticFunction.cpp similarity index 51% rename from wrap/tests/expected/Point3_StaticFunction.cpp rename to wrap/tests/expected/Point3_staticFunction.cpp index 7662b583b..869810709 100644 --- a/wrap/tests/expected/Point3_StaticFunction.cpp +++ b/wrap/tests/expected/Point3_staticFunction.cpp @@ -1,9 +1,9 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("Point3_StaticFunction",nargout,nargin,0); - double result = Point3::StaticFunction(); + checkArguments("Point3_staticFunction",nargout,nargin,0); + double result = Point3::staticFunction(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/Point3_staticFunction.m b/wrap/tests/expected/Point3_staticFunction.m new file mode 100644 index 000000000..441bb5bc6 --- /dev/null +++ b/wrap/tests/expected/Point3_staticFunction.m @@ -0,0 +1,4 @@ +function result = Point3_staticFunction() +% usage: x = Point3_staticFunction() + error('need to compile Point3_staticFunction.cpp'); +end diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index 864dc60e5..732f40fc9 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Dec-01 +% automatically generated by wrap on 2011-Dec-06 echo on toolboxpath = mfilename('fullpath'); @@ -20,7 +20,8 @@ mex -O5 dim.cpp %% Point3 cd(toolboxpath) mex -O5 new_Point3_ddd.cpp -mex -O5 Point3_StaticFunction.cpp +mex -O5 Point3_staticFunction.cpp +mex -O5 Point3_StaticFunctionRet.cpp cd @Point3 mex -O5 norm.cpp diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index da07147da..34ef89203 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Point2_.m b/wrap/tests/expected/new_Point2_.m index f6e31bbf2..dc02b426b 100644 --- a/wrap/tests/expected/new_Point2_.m +++ b/wrap/tests/expected/new_Point2_.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Dec-01 +% automatically generated by wrap on 2011-Dec-06 function result = new_Point2_(obj) error('need to compile new_Point2_.cpp'); end diff --git a/wrap/tests/expected/new_Point2_dd.cpp b/wrap/tests/expected/new_Point2_dd.cpp index e447c234e..61d4d1535 100644 --- a/wrap/tests/expected/new_Point2_dd.cpp +++ b/wrap/tests/expected/new_Point2_dd.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Point2_dd.m b/wrap/tests/expected/new_Point2_dd.m index 19e3b3ebd..7d4bf7b86 100644 --- a/wrap/tests/expected/new_Point2_dd.m +++ b/wrap/tests/expected/new_Point2_dd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Dec-01 +% automatically generated by wrap on 2011-Dec-06 function result = new_Point2_dd(obj,x,y) error('need to compile new_Point2_dd.cpp'); end diff --git a/wrap/tests/expected/new_Point3_ddd.cpp b/wrap/tests/expected/new_Point3_ddd.cpp index 3a572c26c..f9bed0d45 100644 --- a/wrap/tests/expected/new_Point3_ddd.cpp +++ b/wrap/tests/expected/new_Point3_ddd.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Point3_ddd.m b/wrap/tests/expected/new_Point3_ddd.m index 890a93bf5..b3004b4bb 100644 --- a/wrap/tests/expected/new_Point3_ddd.m +++ b/wrap/tests/expected/new_Point3_ddd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Dec-01 +% automatically generated by wrap on 2011-Dec-06 function result = new_Point3_ddd(obj,x,y,z) error('need to compile new_Point3_ddd.cpp'); end diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp index 77b74027f..39898b03c 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,4 +1,4 @@ -// automatically generated by wrap on 2011-Dec-01 +// automatically generated by wrap on 2011-Dec-06 #include #include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected/new_Test_.m b/wrap/tests/expected/new_Test_.m index 023f8a451..9064b5f23 100644 --- a/wrap/tests/expected/new_Test_.m +++ b/wrap/tests/expected/new_Test_.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Dec-01 +% automatically generated by wrap on 2011-Dec-06 function result = new_Test_(obj) error('need to compile new_Test_.cpp'); end diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index e48ae34d7..2040254ed 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -29,7 +29,7 @@ static bool verbose = false; #ifdef TOPSRCDIR static string topdir = TOPSRCDIR; #else -static string topdir = "notarealdirectory"; // If TOPSRCDIR is not defined, we error +static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error #endif /* ************************************************************************* */ @@ -62,7 +62,7 @@ TEST( wrap, parse ) { EXPECT(cls.name=="Point3"); EXPECT(cls.constructors.size()==1); EXPECT(cls.methods.size()==1); - EXPECT(cls.static_methods.size()==1); + EXPECT(cls.static_methods.size()==2); // first constructor takes 3 doubles Constructor c1 = cls.constructors.front(); @@ -91,15 +91,15 @@ TEST( wrap, matlab_code ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("mexa64", "actual", "", "-O5"); + module.matlab_code("actual", "", "mexa64", "-O5"); EXPECT(files_equal(path + "/tests/expected/@Point2/Point2.m" , "actual/@Point2/Point2.m" )); EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Point3/Point3.m" , "actual/@Point3/Point3.m" )); EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.m" , "actual/new_Point3_ddd.m" )); EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.cpp", "actual/new_Point3_ddd.cpp")); - EXPECT(files_equal(path + "/tests/expected/Point3_StaticFunction.m" , "actual/Point3_StaticFunction.m" )); - EXPECT(files_equal(path + "/tests/expected/Point3_StaticFunction.cpp", "actual/Point3_StaticFunction.cpp")); + EXPECT(files_equal(path + "/tests/expected/Point3_staticFunction.m" , "actual/Point3_staticFunction.m" )); + EXPECT(files_equal(path + "/tests/expected/Point3_staticFunction.cpp", "actual/Point3_staticFunction.cpp")); EXPECT(files_equal(path + "/tests/expected/@Point3/norm.m" , "actual/@Point3/norm.m" )); EXPECT(files_equal(path + "/tests/expected/@Point3/norm.cpp" , "actual/@Point3/norm.cpp" )); From f5f59bd213bb6de8799837daca8b9b4572238352 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Dec 2011 03:05:33 +0000 Subject: [PATCH 40/72] Added better handling for pairs of classes with/without pointers --- wrap/Module.cpp | 29 ++++++++++--------- wrap/Module.h | 4 +-- wrap/ReturnValue.cpp | 14 +++++---- wrap/ReturnValue.h | 13 +++++++-- wrap/geometry.h | 6 ++-- .../tests/expected/@Test/create_MixedPtrs.cpp | 11 +++++++ wrap/tests/expected/@Test/create_MixedPtrs.m | 4 +++ wrap/tests/expected/@Test/return_Test.cpp | 11 +++++++ wrap/tests/expected/@Test/return_Test.m | 4 +++ wrap/tests/expected/Makefile | 10 +++++-- wrap/tests/expected/make_geometry.m | 4 ++- wrap/tests/testWrap.cpp | 16 +++++++++- 12 files changed, 96 insertions(+), 30 deletions(-) create mode 100644 wrap/tests/expected/@Test/create_MixedPtrs.cpp create mode 100644 wrap/tests/expected/@Test/create_MixedPtrs.m create mode 100644 wrap/tests/expected/@Test/return_Test.cpp create mode 100644 wrap/tests/expected/@Test/return_Test.m diff --git a/wrap/Module.cpp b/wrap/Module.cpp index da481b9d5..efc45f187 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -82,14 +82,17 @@ Module::Module(const string& interfacePath, Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double"); - Rule eigenType = - (str_p("Vector") | "Matrix")[assign_a(arg.type)] >> - !ch_p('*')[assign_a(arg.is_ptr,true)]; + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule argEigenType = + eigenType_p[assign_a(arg.type)] >> + !ch_p('*')[assign_a(arg.is_ptr,true)]; Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; Rule argument_p = - ((basisType_p[assign_a(arg.type)] | eigenType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) + ((basisType_p[assign_a(arg.type)] | argEigenType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) [push_back_a(args, arg)] [assign_a(arg,arg0)]; @@ -102,19 +105,17 @@ Module::Module(const string& interfacePath, [push_back_a(cls.constructors, constructor)] [assign_a(constructor,constructor0)]; - Rule returnClass1_p = className_p [assign_a(retVal.returns_class_, true)]; - - Rule returnClass2_p = className_p [assign_a(retVal.returns_class2_, true)]; - Rule returnType1_p = - basisType_p[assign_a(retVal.returns_)] | - ((returnClass1_p | "Vector" | "Matrix")[assign_a(retVal.returns_)] >> - !ch_p('*') [assign_a(retVal.returns_ptr_,true)]); + (basisType_p[assign_a(retVal.returns_)][assign_a(retVal.return1, ReturnValue::BASIS)]) | + (eigenType_p[assign_a(retVal.returns_)][assign_a(retVal.return1, ReturnValue::EIGEN)]) | + (className_p[assign_a(retVal.returns_)][assign_a(retVal.return1, ReturnValue::CLASS)]) >> + !ch_p('*') [assign_a(retVal.returns_ptr_,true)]; Rule returnType2_p = - basisType_p[assign_a(retVal.returns2_)] | - ((returnClass2_p | "Vector" | "Matrix")[assign_a(retVal.returns2_)] >> - !ch_p('*') [assign_a(retVal.returns_ptr2_,true)]); + (basisType_p[assign_a(retVal.returns2_)][assign_a(retVal.return2, ReturnValue::BASIS)]) | + (eigenType_p[assign_a(retVal.returns2_)][assign_a(retVal.return2, ReturnValue::EIGEN)]) | + (className_p[assign_a(retVal.returns2_)][assign_a(retVal.return2, ReturnValue::CLASS)]) >> + !ch_p('*') [assign_a(retVal.returns_ptr2_,true)]; Rule pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') diff --git a/wrap/Module.h b/wrap/Module.h index e47057a63..a5aaad191 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include "Class.h" @@ -29,7 +29,7 @@ namespace wrap { */ struct Module { std::string name; ///< module name - std::list classes; ///< list of classes + std::vector classes; ///< list of classes bool verbose_; ///< verbose flag /// constructor that parses interface file diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 005cfea40..e7b87e0fa 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -16,7 +16,7 @@ string ReturnValue::return_type(bool add_ptr, pairing p) { if (p==pair && returns_pair_) { string str = "pair< " + wrap::maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + - wrap::maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >"; + wrap::maybe_shared_ptr(add_ptr && returns_ptr2_, returns2_) + " >"; return str; } else return wrap::maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); @@ -26,20 +26,24 @@ string ReturnValue::return_type(bool add_ptr, pairing p) { void ReturnValue::wrap_result(std::ostream& ofs) { if (returns_pair_) { // first return value in pair - if (returns_ptr_) + if (returns_ptr_) // if we already have a pointer ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n"; - else + else if (return1 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[0] = wrap_shared_ptr(make_shared< " << returns_ << " >(result.first),\"" << returns_ << "\");\n"; + else // if basis type ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; // second return value in pair - if (returns_ptr2_) + if (returns_ptr2_) // if we already have a pointer ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n"; + else if (return2 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[1] = wrap_shared_ptr(make_shared< " << returns2_ << " >(result.second),\"" << returns2_ << "\");\n"; else ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; } else if (returns_ptr_) ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n"; - else if (returns_class_) + else if (return1 == ReturnValue::CLASS) ofs << " out[0] = wrap_shared_ptr(make_shared< " << returns_ << " >(result),\"" << returns_ << "\");\n"; else if (returns_!="void") ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 68c0fb6da..a7137054f 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -15,14 +15,23 @@ namespace wrap { struct ReturnValue { + typedef enum { + CLASS, + EIGEN, + BASIS, + VOID + } return_category; + ReturnValue(bool verbose = true) : verbose_(verbose), returns_ptr_(false), returns_ptr2_(false), - returns_pair_(false), returns_class_(false) + returns_pair_(false), return1(VOID), return2(VOID) {} bool verbose_; std::string returns_, returns2_; - bool returns_ptr_, returns_ptr2_, returns_pair_, returns_class_, returns_class2_; + bool returns_ptr_, returns_ptr2_, returns_pair_; + + return_category return1, return2; typedef enum { arg1, arg2, pair diff --git a/wrap/geometry.h b/wrap/geometry.h index f89b7f132..2faa21504 100644 --- a/wrap/geometry.h +++ b/wrap/geometry.h @@ -24,6 +24,8 @@ class Test { // another comment Test(); + pair return_pair (Vector v, Matrix A) const; // intentionally the first method + bool return_bool (bool value) const; // comment after a line! size_t return_size_t (size_t value) const; int return_int (int value) const; @@ -39,15 +41,15 @@ class Test { Vector return_vector2(Vector value) const; Matrix return_matrix2(Matrix value) const; - pair return_pair (Vector v, Matrix A) const; - bool return_field(const Test& t) const; Test* return_TestPtr(Test* value) const; + Test return_Test(Test* value) const; Point2* return_Point2Ptr(bool value) const; pair create_ptrs () const; + pair create_MixedPtrs () const; pair return_ptrs (Test* p1, Test* p2) const; void print() const; diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp new file mode 100644 index 000000000..6e271486b --- /dev/null +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-06 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + pair< Test, shared_ptr > result = self->create_MixedPtrs(); + out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test"); + out[1] = wrap_shared_ptr(result.second,"Test"); +} diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.m b/wrap/tests/expected/@Test/create_MixedPtrs.m new file mode 100644 index 000000000..9062cabcb --- /dev/null +++ b/wrap/tests/expected/@Test/create_MixedPtrs.m @@ -0,0 +1,4 @@ +function [first,second] = create_MixedPtrs(obj) +% usage: obj.create_MixedPtrs() + error('need to compile create_MixedPtrs.cpp'); +end diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp new file mode 100644 index 000000000..e14aecb89 --- /dev/null +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-06 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Test",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); + Test result = self->return_Test(value); + out[0] = wrap_shared_ptr(make_shared< Test >(result),"Test"); +} diff --git a/wrap/tests/expected/@Test/return_Test.m b/wrap/tests/expected/@Test/return_Test.m new file mode 100644 index 000000000..01f4ec61b --- /dev/null +++ b/wrap/tests/expected/@Test/return_Test.m @@ -0,0 +1,4 @@ +function result = return_Test(obj,value) +% usage: obj.return_Test(value) + error('need to compile return_Test.cpp'); +end diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile index 7249daaba..f839827e3 100644 --- a/wrap/tests/expected/Makefile +++ b/wrap/tests/expected/Makefile @@ -35,6 +35,8 @@ Point3: new_Point3_ddd.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_St # Test new_Test_.$(MEXENDING): new_Test_.cpp $(MEX) $(mex_flags) new_Test_.cpp -output new_Test_ +@Test/return_pair.$(MEXENDING): @Test/return_pair.cpp + $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair @Test/return_bool.$(MEXENDING): @Test/return_bool.cpp $(MEX) $(mex_flags) @Test/return_bool.cpp -output @Test/return_bool @Test/return_size_t.$(MEXENDING): @Test/return_size_t.cpp @@ -53,22 +55,24 @@ new_Test_.$(MEXENDING): new_Test_.cpp $(MEX) $(mex_flags) @Test/return_vector2.cpp -output @Test/return_vector2 @Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp $(MEX) $(mex_flags) @Test/return_matrix2.cpp -output @Test/return_matrix2 -@Test/return_pair.$(MEXENDING): @Test/return_pair.cpp - $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair @Test/return_field.$(MEXENDING): @Test/return_field.cpp $(MEX) $(mex_flags) @Test/return_field.cpp -output @Test/return_field @Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp $(MEX) $(mex_flags) @Test/return_TestPtr.cpp -output @Test/return_TestPtr +@Test/return_Test.$(MEXENDING): @Test/return_Test.cpp + $(MEX) $(mex_flags) @Test/return_Test.cpp -output @Test/return_Test @Test/return_Point2Ptr.$(MEXENDING): @Test/return_Point2Ptr.cpp $(MEX) $(mex_flags) @Test/return_Point2Ptr.cpp -output @Test/return_Point2Ptr @Test/create_ptrs.$(MEXENDING): @Test/create_ptrs.cpp $(MEX) $(mex_flags) @Test/create_ptrs.cpp -output @Test/create_ptrs +@Test/create_MixedPtrs.$(MEXENDING): @Test/create_MixedPtrs.cpp + $(MEX) $(mex_flags) @Test/create_MixedPtrs.cpp -output @Test/create_MixedPtrs @Test/return_ptrs.$(MEXENDING): @Test/return_ptrs.cpp $(MEX) $(mex_flags) @Test/return_ptrs.cpp -output @Test/return_ptrs @Test/print.$(MEXENDING): @Test/print.cpp $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print -Test: new_Test_.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) +Test: new_Test_.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index 732f40fc9..d73022bab 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -31,6 +31,7 @@ cd(toolboxpath) mex -O5 new_Test_.cpp cd @Test +mex -O5 return_pair.cpp mex -O5 return_bool.cpp mex -O5 return_size_t.cpp mex -O5 return_int.cpp @@ -40,11 +41,12 @@ mex -O5 return_vector1.cpp mex -O5 return_matrix1.cpp mex -O5 return_vector2.cpp mex -O5 return_matrix2.cpp -mex -O5 return_pair.cpp mex -O5 return_field.cpp mex -O5 return_TestPtr.cpp +mex -O5 return_Test.cpp mex -O5 return_Point2Ptr.cpp mex -O5 create_ptrs.cpp +mex -O5 create_MixedPtrs.cpp mex -O5 return_ptrs.cpp mex -O5 print.cpp diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 2040254ed..b80412891 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -55,7 +55,7 @@ TEST( wrap, parse ) { string path = topdir + "/wrap"; Module module(path.c_str(), "geometry",verbose); - EXPECT(module.classes.size()==3); + CHECK(module.classes.size()==3); // check second class, Point3 Class cls = *(++module.classes.begin()); @@ -81,6 +81,18 @@ TEST( wrap, parse ) { EXPECT(m1.name_=="norm"); EXPECT(m1.args_.size()==0); EXPECT(m1.is_const_); + + // Test class is the third one + Class testCls = module.classes.at(2); + EXPECT_LONGS_EQUAL( 1, testCls.constructors.size()); + EXPECT_LONGS_EQUAL(18, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); + +// pair return_pair (Vector v, Matrix A) const; + Method m2 = testCls.methods.front(); + EXPECT(m2.returnVal_.returns_pair_); + EXPECT(m2.returnVal_.return1 == ReturnValue::EIGEN); + EXPECT(m2.returnVal_.return2 == ReturnValue::EIGEN); } /* ************************************************************************* */ @@ -107,8 +119,10 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/@Test/Test.m" , "actual/@Test/Test.m" )); EXPECT(files_equal(path + "/tests/expected/@Test/return_string.cpp" , "actual/@Test/return_string.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/return_pair.cpp" , "actual/@Test/return_pair.cpp" )); + EXPECT(files_equal(path + "/tests/expected/@Test/create_MixedPtrs.cpp", "actual/@Test/create_MixedPtrs.cpp")); EXPECT(files_equal(path + "/tests/expected/@Test/return_field.cpp" , "actual/@Test/return_field.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/return_TestPtr.cpp", "actual/@Test/return_TestPtr.cpp")); + EXPECT(files_equal(path + "/tests/expected/@Test/return_Test.cpp" , "actual/@Test/return_Test.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/return_Point2Ptr.cpp", "actual/@Test/return_Point2Ptr.cpp")); EXPECT(files_equal(path + "/tests/expected/@Test/return_ptrs.cpp" , "actual/@Test/return_ptrs.cpp" )); EXPECT(files_equal(path + "/tests/expected/@Test/print.m" , "actual/@Test/print.m" )); From 1e1511950cc0a8f050b3acef727f2699bfb352a9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Dec 2011 03:05:35 +0000 Subject: [PATCH 41/72] small tweaks, disabling broken function - need a parsing rule that distinguishes VectorValues and Vector --- gtsam.h | 8 +++++++- wrap/Module.cpp | 1 + 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 23fa08987..6d59ffc22 100644 --- a/gtsam.h +++ b/gtsam.h @@ -259,11 +259,17 @@ class PlanarSLAMOdometry { class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianBayesNet* eliminate() const; - VectorValues* optimize() const; +// VectorValues* optimize() const; // FAIL: parse error here GaussianFactor* marginalFactor(int j) const; Matrix marginalCovariance(int j) const; }; + + + + + + //// These are considered to be broken and will be added back as they start working //// It's assumed that there have been interface changes that might break this // diff --git a/wrap/Module.cpp b/wrap/Module.cpp index efc45f187..733e229ba 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -82,6 +82,7 @@ Module::Module(const string& interfacePath, Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double"); + // FAIL: this will match against VectorValues in returntype Rule eigenType_p = (str_p("Vector") | "Matrix"); From 92a0cf67c90577bd141958bed2a9f04f2049b5c1 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Dec 2011 03:05:37 +0000 Subject: [PATCH 42/72] Fixed ambiguity issues with returning non-ptr classes, added new copies of functions to gtsam.h and depreciated old ones --- gtsam.h | 96 ++++++++++++++++++++++------- wrap/Class.cpp | 4 +- wrap/Method.cpp | 2 +- wrap/Method.h | 2 - wrap/Module.cpp | 72 +++++++++++----------- wrap/Module.h | 4 +- wrap/ReturnValue.cpp | 36 +++++------ wrap/ReturnValue.h | 16 ++--- wrap/StaticMethod.cpp | 26 ++++---- wrap/StaticMethod.h | 12 ++-- wrap/geometry.h | 8 +++ wrap/tests/expected/@Test/Test.m | 1 + wrap/tests/expected/Makefile | 10 ++- wrap/tests/expected/make_geometry.m | 3 + wrap/tests/testSpirit.cpp | 34 ++++++++++ wrap/tests/testWrap.cpp | 24 ++++---- 16 files changed, 225 insertions(+), 125 deletions(-) diff --git a/gtsam.h b/gtsam.h index 6d59ffc22..b270a17ec 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1,37 +1,61 @@ /** - * GTSAM Wrap Module definition + * GTSAM Wrap Module Definition * * These are the current classes available through the matlab toolbox interface, * add more functions/classes as they are available. * * Requirements: - * Constructors must appear in a class before any methods - * Methods can only return Matrix, Vector, double, int, void, and a shared_ptr to any other object - * Comments can use either C++ or C style + * Classes must start with an uppercase letter + * Only one Method/Constructor per line + * Methods can return + * - Eigen types: Matrix, Vector + * - C/C++ basic types: string, bool, size_t, int, double + * - void + * - Any class with which be copied with boost::make_shared() + * - boost::shared_ptr of any object type + * Arguments to functions any of + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference + * - C/C++ basic types: string, bool, size_t, int, double + * - Any class with which be copied with boost::make_shared() (except Eigen) + * - boost::shared_ptr of any object type (except Eigen) + * Comments can use either C++ or C style, with multiple lines * Methods must start with a lowercase letter * Static methods must start with a letter (upper or lowercase) and use the "static" keyword - * Classes must start with an uppercase letter + */ + +/** + * Status: + * - Depreciated versions of functions that return a shared_ptr unnecessarily are still present + * - TODO: global functions + * - TODO: namespace detection to handle nested namespaces */ class Point2 { Point2(); Point2(double x, double y); - static Point2* Expmap_(Vector v); + static Point2 Expmap(Vector v); static Vector Logmap(const Point2& p); void print(string s) const; double x(); double y(); + Vector localCoordinates(const Point2& p); + Point2 compose(const Point2& p2); + Point2 between(const Point2& p2); + Point2 retract(Vector v); + + // Depreciated interface Point2* compose_(const Point2& p2); Point2* between_(const Point2& p2); - Vector localCoordinates(const Point2& p); Point2* retract_(Vector v); + static Point2* Expmap_(Vector v); }; class Point3 { Point3(); Point3(double x, double y, double z); Point3(Vector v); - static Point3* Expmap_(Vector v); + static Point3 Expmap(Vector v); static Vector Logmap(const Point3& p); void print(string s) const; bool equals(const Point3& p, double tol); @@ -39,31 +63,43 @@ class Point3 { double x(); double y(); double z(); + Vector localCoordinates(const Point3& p); + Point3 retract(Vector v); + Point3 compose(const Point3& p2); + Point3 between(const Point3& p2); + + // Depreciated interface + static Point3* Expmap_(Vector v); Point3* compose_(const Point3& p2); Point3* between_(const Point3& p2); - Vector localCoordinates(const Point3& p); Point3* retract_(Vector v); }; class Rot2 { Rot2(); Rot2(double theta); - static Rot2* Expmap_(Vector v); + static Rot2 Expmap(Vector v); static Vector Logmap(const Rot2& p); void print(string s) const; bool equals(const Rot2& rot, double tol) const; double c() const; double s() const; + Vector localCoordinates(const Rot2& p); + Rot2 retract(Vector v); + Rot2 compose(const Rot2& p2); + Rot2 between(const Rot2& p2); + + // Depreciated interface Rot2* compose_(const Rot2& p2); Rot2* between_(const Rot2& p2); - Vector localCoordinates(const Rot2& p); Rot2* retract_(Vector v); + static Rot2* Expmap_(Vector v); }; class Rot3 { Rot3(); Rot3(Matrix R); - static Rot3* Expmap_(Vector v); + static Rot3 Expmap(Vector v); static Vector Logmap(const Rot3& p); static Rot3 ypr(double y, double p, double r); Matrix matrix() const; @@ -72,10 +108,16 @@ class Rot3 { Vector ypr() const; void print(string s) const; bool equals(const Rot3& rot, double tol) const; + Vector localCoordinates(const Rot3& p); + Rot3 retract(Vector v); + Rot3 compose(const Rot3& p2); + Rot3 between(const Rot3& p2); + + // Depreciated interface Rot3* compose_(const Rot3& p2); Rot3* between_(const Rot3& p2); - Vector localCoordinates(const Rot3& p); Rot3* retract_(Vector v); + static Rot3* Expmap_(Vector v); }; class Pose2 { @@ -84,7 +126,7 @@ class Pose2 { Pose2(double theta, const Point2& t); Pose2(const Rot2& r, const Point2& t); Pose2(Vector v); - static Pose2* Expmap_(Vector v); + static Pose2 Expmap(Vector v); static Vector Logmap(const Pose2& p); void print(string s) const; bool equals(const Pose2& pose, double tol) const; @@ -92,10 +134,16 @@ class Pose2 { double y() const; double theta() const; int dim() const; + Vector localCoordinates(const Pose2& p); + Pose2 retract(Vector v); + Pose2 compose(const Pose2& p2); + Pose2 between(const Pose2& p2); + + // Depreciated interface Pose2* compose_(const Pose2& p2); Pose2* between_(const Pose2& p2); - Vector localCoordinates(const Pose2& p); Pose2* retract_(Vector v); + static Pose2* Expmap_(Vector v); }; class Pose3 { @@ -103,7 +151,7 @@ class Pose3 { Pose3(const Rot3& r, const Point3& t); Pose3(Vector v); Pose3(Matrix t); - static Pose3* Expmap_(Vector v); + static Pose3 Expmap(Vector v); static Vector Logmap(const Pose3& p); void print(string s) const; bool equals(const Pose3& pose, double tol) const; @@ -112,9 +160,16 @@ class Pose3 { double z() const; Matrix matrix() const; Matrix adjointMap() const; + Pose3 compose(const Pose3& p2); + Pose3 between(const Pose3& p2); + Pose3 retract(Vector v); + Point3 translation() const; + Rot3 rotation() const; + + // Depreciated interface + static Pose3* Expmap_(Vector v); Pose3* compose_(const Pose3& p2); Pose3* between_(const Pose3& p2); - Vector localCoordinates(const Pose3& p); Pose3* retract_(Vector v); Point3* translation_() const; Rot3* rotation_() const; @@ -259,17 +314,12 @@ class PlanarSLAMOdometry { class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianBayesNet* eliminate() const; -// VectorValues* optimize() const; // FAIL: parse error here + VectorValues* optimize() const; // FAIL: parse error here GaussianFactor* marginalFactor(int j) const; Matrix marginalCovariance(int j) const; }; - - - - - //// These are considered to be broken and will be added back as they start working //// It's assumed that there have been interface changes that might break this // diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 489a56e18..cf56f73a1 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -86,7 +86,7 @@ void Class::matlab_make_fragment(ofstream& ofs, BOOST_FOREACH(Constructor c, constructors) ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl; BOOST_FOREACH(StaticMethod sm, static_methods) - ofs << mex << name + "_" + sm.name_ << ".cpp" << endl; + ofs << mex << name + "_" + sm.name << ".cpp" << endl; ofs << endl << "cd @" << name << endl; BOOST_FOREACH(Method m, methods) ofs << mex << m.name_ << ".cpp" << endl; @@ -115,7 +115,7 @@ void Class::makefile_fragment(ofstream& ofs) { file_names.push_back(file_base); } BOOST_FOREACH(StaticMethod c, static_methods) { - string file_base = name + "_" + c.name_; + string file_base = name + "_" + c.name; file_names.push_back(file_base); } BOOST_FOREACH(Method c, methods) { diff --git a/wrap/Method.cpp b/wrap/Method.cpp index fbfce29a7..bf0426c3e 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -87,7 +87,7 @@ void Method::matlab_wrapper(const string& classPath, // call method // example: bool result = self->return_field(t); ofs << " "; - if (returnVal_.returns_!="void") + if (returnVal_.type1!="void") ofs << returnVal_.return_type(true,ReturnValue::pair) << " result = "; ofs << "self->" << name_ << "(" << args_.names() << ");\n"; diff --git a/wrap/Method.h b/wrap/Method.h index 9d3860ff1..843a2053b 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -39,8 +39,6 @@ struct Method { ArgumentList args_; ReturnValue returnVal_; -// std::string return_type(bool add_ptr, pairing p); - // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 733e229ba..364e172ea 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -40,17 +40,17 @@ typedef rule Rule; /* ************************************************************************* */ Module::Module(const string& interfacePath, - const string& moduleName, bool verbose) : name(moduleName), verbose_(verbose) + const string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) { // these variables will be imperatively updated to gradually build [cls] // The one with postfix 0 are used to reset the variables after parse. ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; - Constructor constructor0(verbose), constructor(verbose); - Method method0(verbose), method(verbose); - StaticMethod static_method0(verbose), static_method(verbose); - Class cls0(verbose),cls(verbose); + Constructor constructor0(enable_verbose), constructor(enable_verbose); + Method method0(enable_verbose), method(enable_verbose); + StaticMethod static_method0(enable_verbose), static_method(enable_verbose); + Class cls0(enable_verbose),cls(enable_verbose); //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -68,7 +68,13 @@ Module::Module(const string& interfacePath, // lexeme_d turns off white space skipping // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html - Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')]; + Rule basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p; Rule classPtr_p = className_p [assign_a(arg.type)] >> @@ -79,21 +85,19 @@ Module::Module(const string& interfacePath, className_p [assign_a(arg.type)] >> ch_p('&') [assign_a(arg.is_ref,true)]; - Rule basisType_p = - (str_p("string") | "bool" | "size_t" | "int" | "double"); - - // FAIL: this will match against VectorValues in returntype - Rule eigenType_p = - (str_p("Vector") | "Matrix"); - - Rule argEigenType = + Rule argEigenType_p = eigenType_p[assign_a(arg.type)] >> !ch_p('*')[assign_a(arg.is_ptr,true)]; + Rule eigenRef_p = + !str_p("const") [assign_a(arg.is_const,true)] >> + eigenType_p [assign_a(arg.type)] >> + ch_p('&') [assign_a(arg.is_ref,true)]; + Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; Rule argument_p = - ((basisType_p[assign_a(arg.type)] | argEigenType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) + ((basisType_p[assign_a(arg.type)] | argEigenType_p | classRef_p | eigenRef_p | classPtr_p) >> name_p[assign_a(arg.name)]) [push_back_a(args, arg)] [assign_a(arg,arg0)]; @@ -107,22 +111,22 @@ Module::Module(const string& interfacePath, [assign_a(constructor,constructor0)]; Rule returnType1_p = - (basisType_p[assign_a(retVal.returns_)][assign_a(retVal.return1, ReturnValue::BASIS)]) | - (eigenType_p[assign_a(retVal.returns_)][assign_a(retVal.return1, ReturnValue::EIGEN)]) | - (className_p[assign_a(retVal.returns_)][assign_a(retVal.return1, ReturnValue::CLASS)]) >> - !ch_p('*') [assign_a(retVal.returns_ptr_,true)]; + (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::BASIS)]) | + ((className_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::CLASS)]) >> + !ch_p('*')[assign_a(retVal.isPtr1,true)]) | + (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::EIGEN)]); Rule returnType2_p = - (basisType_p[assign_a(retVal.returns2_)][assign_a(retVal.return2, ReturnValue::BASIS)]) | - (eigenType_p[assign_a(retVal.returns2_)][assign_a(retVal.return2, ReturnValue::EIGEN)]) | - (className_p[assign_a(retVal.returns2_)][assign_a(retVal.return2, ReturnValue::CLASS)]) >> - !ch_p('*') [assign_a(retVal.returns_ptr2_,true)]; + (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::BASIS)]) | + ((className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >> + !ch_p('*') [assign_a(retVal.isPtr2,true)]) | + (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::EIGEN)]); Rule pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(retVal.returns_pair_,true)]; + [assign_a(retVal.isPair,true)]; - Rule void_p = str_p("void")[assign_a(retVal.returns_)]; + Rule void_p = str_p("void")[assign_a(retVal.type1)]; Rule returnType_p = void_p | returnType1_p | pair_p; @@ -142,23 +146,19 @@ Module::Module(const string& interfacePath, Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; Rule static_method_p = - (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name_)] >> + (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name)] >> '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [assign_a(static_method.args_,args)] + [assign_a(static_method.args,args)] [assign_a(args,args0)] - [assign_a(static_method.returnVal_,retVal)] + [assign_a(static_method.returnVal,retVal)] [assign_a(retVal,retVal0)] [push_back_a(cls.static_methods, static_method)] [assign_a(static_method,static_method0)]; - Rule methods_p = method_p | static_method_p; + Rule functions_p = constructor_p | method_p | static_method_p; Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> - *comments_p >> - *constructor_p >> - *comments_p >> - *methods_p >> - *comments_p >> + *(functions_p | comments_p) >> '}' >> ";"; Rule module_p = *comments_p >> +(class_p @@ -223,7 +223,7 @@ void Module::matlab_code(const string& toolboxPath, ofstream make_ofs(makeFile.c_str()); if(!make_ofs) throw CantOpenFile(makeFile); - if (verbose_) cerr << "generating " << matlabMakeFile << endl; + if (verbose) cerr << "generating " << matlabMakeFile << endl; emit_header_comment(ofs,"%"); ofs << "echo on" << endl << endl; ofs << "toolboxpath = mfilename('fullpath');" << endl; @@ -232,7 +232,7 @@ void Module::matlab_code(const string& toolboxPath, ofs << "clear delims" << endl; ofs << "addpath(toolboxpath);" << endl << endl; - if (verbose_) cerr << "generating " << makeFile << endl; + if (verbose) cerr << "generating " << makeFile << endl; emit_header_comment(make_ofs,"#"); make_ofs << "\nMEX = mex\n"; make_ofs << "MEXENDING = " << mexExt << "\n"; diff --git a/wrap/Module.h b/wrap/Module.h index a5aaad191..6ae9202cb 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -30,12 +30,12 @@ namespace wrap { struct Module { std::string name; ///< module name std::vector classes; ///< list of classes - bool verbose_; ///< verbose flag + bool verbose; ///< verbose flag /// constructor that parses interface file Module(const std::string& interfacePath, const std::string& moduleName, - bool verbose=true); + bool enable_verbose=true); /// MATLAB code generation: void matlab_code(const std::string& path, diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index e7b87e0fa..9d8606c5b 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -13,39 +13,39 @@ using namespace wrap; /* ************************************************************************* */ string ReturnValue::return_type(bool add_ptr, pairing p) { - if (p==pair && returns_pair_) { + if (p==pair && isPair) { string str = "pair< " + - wrap::maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + - wrap::maybe_shared_ptr(add_ptr && returns_ptr2_, returns2_) + " >"; + wrap::maybe_shared_ptr(add_ptr && isPtr1, type1) + ", " + + wrap::maybe_shared_ptr(add_ptr && isPtr2, type2) + " >"; return str; } else - return wrap::maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); + return wrap::maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? type2 : type1); } /* ************************************************************************* */ void ReturnValue::wrap_result(std::ostream& ofs) { - if (returns_pair_) { + if (isPair) { // first return value in pair - if (returns_ptr_) // if we already have a pointer - ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n"; - else if (return1 == ReturnValue::CLASS) // if we are going to make one - ofs << " out[0] = wrap_shared_ptr(make_shared< " << returns_ << " >(result.first),\"" << returns_ << "\");\n"; + if (isPtr1) // if we already have a pointer + ofs << " out[0] = wrap_shared_ptr(result.first,\"" << type1 << "\");\n"; + else if (category1 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[0] = wrap_shared_ptr(make_shared< " << type1 << " >(result.first),\"" << type1 << "\");\n"; else // if basis type ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; // second return value in pair - if (returns_ptr2_) // if we already have a pointer - ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n"; - else if (return2 == ReturnValue::CLASS) // if we are going to make one - ofs << " out[1] = wrap_shared_ptr(make_shared< " << returns2_ << " >(result.second),\"" << returns2_ << "\");\n"; + if (isPtr2) // if we already have a pointer + ofs << " out[1] = wrap_shared_ptr(result.second,\"" << type2 << "\");\n"; + else if (category2 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[1] = wrap_shared_ptr(make_shared< " << type2 << " >(result.second),\"" << type2 << "\");\n"; else ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; } - else if (returns_ptr_) - ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n"; - else if (return1 == ReturnValue::CLASS) - ofs << " out[0] = wrap_shared_ptr(make_shared< " << returns_ << " >(result),\"" << returns_ << "\");\n"; - else if (returns_!="void") + else if (isPtr1) + ofs << " out[0] = wrap_shared_ptr(result,\"" << type1 << "\");\n"; + else if (category1 == ReturnValue::CLASS) + ofs << " out[0] = wrap_shared_ptr(make_shared< " << type1 << " >(result),\"" << type1 << "\");\n"; + else if (type1!="void") ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index a7137054f..c6d7864d3 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -22,16 +22,16 @@ struct ReturnValue { VOID } return_category; - ReturnValue(bool verbose = true) - : verbose_(verbose), returns_ptr_(false), returns_ptr2_(false), - returns_pair_(false), return1(VOID), return2(VOID) + ReturnValue(bool enable_verbosity = true) + : verbose(enable_verbosity), isPtr1(false), isPtr2(false), + isPair(false), category1(VOID), category2(VOID) {} - bool verbose_; - std::string returns_, returns2_; - bool returns_ptr_, returns_ptr2_, returns_pair_; + bool verbose; + std::string type1, type2; + bool isPtr1, isPtr2, isPair; - return_category return1, return2; + return_category category1, category2; typedef enum { arg1, arg2, pair @@ -39,7 +39,7 @@ struct ReturnValue { std::string return_type(bool add_ptr, pairing p); - std::string matlab_returnType() const { return returns_pair_? "[first,second]" : "result"; } + std::string matlab_returnType() const { return isPair? "[first,second]" : "result"; } void wrap_result(std::ostream& ofs); diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 40ab9bcb8..763cb38f4 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -29,18 +29,18 @@ using namespace wrap; void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) { // open destination m-file - string full_name = className + "_" + name_; + string full_name = className + "_" + name; string wrapperFile = toolboxPath + "/" + full_name + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); - if(verbose_) cerr << "generating " << wrapperFile << endl; + if(verbose) cerr << "generating " << wrapperFile << endl; // generate code - string returnType = returnVal_.matlab_returnType(); + string returnType = returnVal.matlab_returnType(); ofs << "function " << returnType << " = " << full_name << "("; - if (args_.size()) ofs << args_.names(); + if (args.size()) ofs << args.names(); ofs << ")" << endl; - ofs << "% usage: x = " << full_name << "(" << args_.names() << ")" << endl; + ofs << "% usage: x = " << full_name << "(" << args.names() << ")" << endl; ofs << " error('need to compile " << full_name << ".cpp');" << endl; ofs << "end" << endl; @@ -53,11 +53,11 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, const string& nameSpace) { // open destination wrapperFile - string full_name = className + "_" + name_; + string full_name = className + "_" + name; string wrapperFile = toolboxPath + "/" + full_name + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); - if(verbose_) cerr << "generating " << wrapperFile << endl; + if(verbose) cerr << "generating " << wrapperFile << endl; // generate code @@ -74,21 +74,21 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, // check arguments // NOTE: for static functions, there is no object passed - ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args_.size() << ");\n"; + ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args_.matlab_unwrap(ofs,0); // We start at 0 because there is no self object + args.matlab_unwrap(ofs,0); // We start at 0 because there is no self object ofs << " "; // call method with default type - if (returnVal_.returns_!="void") - ofs << returnVal_.return_type(true,ReturnValue::pair) << " result = "; - ofs << className << "::" << name_ << "(" << args_.names() << ");\n"; + if (returnVal.type1!="void") + ofs << returnVal.return_type(true,ReturnValue::pair) << " result = "; + ofs << className << "::" << name << "(" << args.names() << ");\n"; // wrap result // example: out[0]=wrap(result); - returnVal_.wrap_result(ofs); + returnVal.wrap_result(ofs); // finish ofs << "}\n"; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 0366b64ef..c58b26fe8 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -30,14 +30,14 @@ namespace wrap { struct StaticMethod { /// Constructor creates empty object - StaticMethod(bool verbose = true) : - verbose_(verbose) {} + StaticMethod(bool verbosity = true) : + verbose(verbosity) {} // Then the instance variables are set directly by the Module constructor - bool verbose_; - std::string name_; - ArgumentList args_; - ReturnValue returnVal_; + bool verbose; + std::string name; + ArgumentList args; + ReturnValue returnVal; // MATLAB code generation // toolboxPath is the core toolbox directory, e.g., ../matlab diff --git a/wrap/geometry.h b/wrap/geometry.h index 2faa21504..8d598ba25 100644 --- a/wrap/geometry.h +++ b/wrap/geometry.h @@ -6,6 +6,7 @@ class Point2 { double x() const; double y() const; int dim() const; + VectorNotEigen vectorConfusion(); }; class Point3 { @@ -19,6 +20,10 @@ class Point3 { // another comment +/** + * A multi-line comment! + */ + class Test { /* a comment! */ // another comment @@ -31,6 +36,8 @@ class Test { int return_int (int value) const; double return_double (double value) const; + Test(double a, Matrix b); // a constructor in the middle of a class + // comments in the middle! // (more) comments in the middle! @@ -40,6 +47,7 @@ class Test { Matrix return_matrix1(Matrix value) const; Vector return_vector2(Vector value) const; Matrix return_matrix2(Matrix value) const; + void arg_EigenConstRef(const Matrix& value) const; bool return_field(const Test& t) const; diff --git a/wrap/tests/expected/@Test/Test.m b/wrap/tests/expected/@Test/Test.m index 19b4442e4..8574a0882 100644 --- a/wrap/tests/expected/@Test/Test.m +++ b/wrap/tests/expected/@Test/Test.m @@ -5,6 +5,7 @@ classdef Test methods function obj = Test(varargin) if nargin == 0, obj.self = new_Test_(); end + if nargin == 2, obj.self = new_Test_dM(varargin{1},varargin{2}); end if nargin ~= 13 && obj.self == 0, error('Test constructor failed'); end end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile index f839827e3..03f69a496 100644 --- a/wrap/tests/expected/Makefile +++ b/wrap/tests/expected/Makefile @@ -17,8 +17,10 @@ new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y @Point2/dim.$(MEXENDING): @Point2/dim.cpp $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim +@Point2/vectorConfusion.$(MEXENDING): @Point2/vectorConfusion.cpp + $(MEX) $(mex_flags) @Point2/vectorConfusion.cpp -output @Point2/vectorConfusion -Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) +Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) @Point2/vectorConfusion.$(MEXENDING) # Point3 new_Point3_ddd.$(MEXENDING): new_Point3_ddd.cpp @@ -35,6 +37,8 @@ Point3: new_Point3_ddd.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_St # Test new_Test_.$(MEXENDING): new_Test_.cpp $(MEX) $(mex_flags) new_Test_.cpp -output new_Test_ +new_Test_dM.$(MEXENDING): new_Test_dM.cpp + $(MEX) $(mex_flags) new_Test_dM.cpp -output new_Test_dM @Test/return_pair.$(MEXENDING): @Test/return_pair.cpp $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair @Test/return_bool.$(MEXENDING): @Test/return_bool.cpp @@ -55,6 +59,8 @@ new_Test_.$(MEXENDING): new_Test_.cpp $(MEX) $(mex_flags) @Test/return_vector2.cpp -output @Test/return_vector2 @Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp $(MEX) $(mex_flags) @Test/return_matrix2.cpp -output @Test/return_matrix2 +@Test/arg_EigenConstRef.$(MEXENDING): @Test/arg_EigenConstRef.cpp + $(MEX) $(mex_flags) @Test/arg_EigenConstRef.cpp -output @Test/arg_EigenConstRef @Test/return_field.$(MEXENDING): @Test/return_field.cpp $(MEX) $(mex_flags) @Test/return_field.cpp -output @Test/return_field @Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp @@ -72,7 +78,7 @@ new_Test_.$(MEXENDING): new_Test_.cpp @Test/print.$(MEXENDING): @Test/print.cpp $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print -Test: new_Test_.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) +Test: new_Test_.$(MEXENDING) new_Test_dM.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/arg_EigenConstRef.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index d73022bab..39123f65d 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -16,6 +16,7 @@ cd @Point2 mex -O5 x.cpp mex -O5 y.cpp mex -O5 dim.cpp +mex -O5 vectorConfusion.cpp %% Point3 cd(toolboxpath) @@ -29,6 +30,7 @@ mex -O5 norm.cpp %% Test cd(toolboxpath) mex -O5 new_Test_.cpp +mex -O5 new_Test_dM.cpp cd @Test mex -O5 return_pair.cpp @@ -41,6 +43,7 @@ mex -O5 return_vector1.cpp mex -O5 return_matrix1.cpp mex -O5 return_vector2.cpp mex -O5 return_matrix2.cpp +mex -O5 arg_EigenConstRef.cpp mex -O5 return_field.cpp mex -O5 return_TestPtr.cpp mex -O5 return_Test.cpp diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 9683a3372..d4a6d151e 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -102,6 +102,40 @@ TEST( spirit, constMethod_p ) { EXPECT(parse("double norm() const;", constMethod_p, space_p).full); } +/* ************************************************************************* */ +TEST( spirit, return_value_p ) { + bool isEigen = true; + string actual_return_type; + string actual_function_name; + + Rule basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p; + + Rule funcName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + + Rule returnType_p = + (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]) | + (className_p[assign_a(actual_return_type)][assign_a(isEigen,false)]) | + (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]); + + Rule testFunc_p = returnType_p >> funcName_p[assign_a(actual_function_name)] >> str_p("();"); + + EXPECT(parse("VectorNotEigen doesNotReturnAnEigenVector();", testFunc_p, space_p).full); + EXPECT(!isEigen); + EXPECT(actual_return_type == "VectorNotEigen"); + EXPECT(actual_function_name == "doesNotReturnAnEigenVector"); + + EXPECT(parse("Vector actuallyAVector();", testFunc_p, space_p).full); + EXPECT(isEigen); + EXPECT(actual_return_type == "Vector"); + EXPECT(actual_function_name == "actuallyAVector"); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index b80412891..4cc8b7cd2 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -25,7 +25,7 @@ using namespace std; using namespace wrap; -static bool verbose = false; +static bool enable_verbose = false; #ifdef TOPSRCDIR static string topdir = TOPSRCDIR; #else @@ -46,15 +46,15 @@ TEST( wrap, ArgumentList ) { /* ************************************************************************* */ TEST( wrap, check_exception ) { - THROWS_EXCEPTION(Module("/notarealpath", "geometry",verbose)); - CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",verbose), CantOpenFile); + THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); + CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); } /* ************************************************************************* */ TEST( wrap, parse ) { string path = topdir + "/wrap"; - Module module(path.c_str(), "geometry",verbose); + Module module(path.c_str(), "geometry",enable_verbose); CHECK(module.classes.size()==3); // check second class, Point3 @@ -77,29 +77,29 @@ TEST( wrap, parse ) { // check method Method m1 = cls.methods.front(); - EXPECT(m1.returnVal_.returns_=="double"); + EXPECT(m1.returnVal_.type1=="double"); EXPECT(m1.name_=="norm"); EXPECT(m1.args_.size()==0); EXPECT(m1.is_const_); // Test class is the third one Class testCls = module.classes.at(2); - EXPECT_LONGS_EQUAL( 1, testCls.constructors.size()); - EXPECT_LONGS_EQUAL(18, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); + EXPECT_LONGS_EQUAL(19, testCls.methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); -// pair return_pair (Vector v, Matrix A) const; + // function to parse: pair return_pair (Vector v, Matrix A) const; Method m2 = testCls.methods.front(); - EXPECT(m2.returnVal_.returns_pair_); - EXPECT(m2.returnVal_.return1 == ReturnValue::EIGEN); - EXPECT(m2.returnVal_.return2 == ReturnValue::EIGEN); + EXPECT(m2.returnVal_.isPair); + EXPECT(m2.returnVal_.category1 == ReturnValue::EIGEN); + EXPECT(m2.returnVal_.category2 == ReturnValue::EIGEN); } /* ************************************************************************* */ TEST( wrap, matlab_code ) { // Parse into class object string path = topdir + "/wrap"; - Module module(path,"geometry",verbose); + Module module(path,"geometry",enable_verbose); // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make From 886f9459b43bc937e2b2b9b90a881540347e148d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Dec 2011 15:23:20 +0000 Subject: [PATCH 43/72] Removed wrap function duplicates due to shared_ptr, removed the wrapping functions inside geometry classes, added named constructors to the wrappable functions --- gtsam.h | 62 +++++++++++++++-------------------------- gtsam/geometry/Point2.h | 18 ------------ gtsam/geometry/Point3.h | 18 ------------ gtsam/geometry/Pose2.h | 18 ------------ gtsam/geometry/Pose3.h | 18 ------------ gtsam/geometry/Rot2.h | 18 ------------ gtsam/geometry/Rot3M.h | 18 ------------ 7 files changed, 22 insertions(+), 148 deletions(-) diff --git a/gtsam.h b/gtsam.h index b270a17ec..432c95698 100644 --- a/gtsam.h +++ b/gtsam.h @@ -26,8 +26,10 @@ /** * Status: - * - Depreciated versions of functions that return a shared_ptr unnecessarily are still present * - TODO: global functions + * - TODO: default values for arguments + * - TODO: overloaded functions + * - TODO: Handle Rot3M conversions to quaternions * - TODO: namespace detection to handle nested namespaces */ @@ -43,12 +45,6 @@ class Point2 { Point2 compose(const Point2& p2); Point2 between(const Point2& p2); Point2 retract(Vector v); - - // Depreciated interface - Point2* compose_(const Point2& p2); - Point2* between_(const Point2& p2); - Point2* retract_(Vector v); - static Point2* Expmap_(Vector v); }; class Point3 { @@ -67,12 +63,6 @@ class Point3 { Point3 retract(Vector v); Point3 compose(const Point3& p2); Point3 between(const Point3& p2); - - // Depreciated interface - static Point3* Expmap_(Vector v); - Point3* compose_(const Point3& p2); - Point3* between_(const Point3& p2); - Point3* retract_(Vector v); }; class Rot2 { @@ -80,20 +70,21 @@ class Rot2 { Rot2(double theta); static Rot2 Expmap(Vector v); static Vector Logmap(const Rot2& p); + static Rot2 fromAngle(double theta); + static Rot2 fromDegrees(double theta); + static Rot2 fromCosSin(double c, double s); + static Rot2 relativeBearing(const Point2& d); // Ignoring derivative + static Rot2 atan2(double y, double x); void print(string s) const; bool equals(const Rot2& rot, double tol) const; + double theta() const; + double degrees() const; double c() const; double s() const; Vector localCoordinates(const Rot2& p); Rot2 retract(Vector v); Rot2 compose(const Rot2& p2); Rot2 between(const Rot2& p2); - - // Depreciated interface - Rot2* compose_(const Rot2& p2); - Rot2* between_(const Rot2& p2); - Rot2* retract_(Vector v); - static Rot2* Expmap_(Vector v); }; class Rot3 { @@ -102,22 +93,27 @@ class Rot3 { static Rot3 Expmap(Vector v); static Vector Logmap(const Rot3& p); static Rot3 ypr(double y, double p, double r); + static Rot3 Rx(double t); + static Rot3 Ry(double t); + static Rot3 Rz(double t); + static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(const Vector& xyz); + static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading) + static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft) + static Rot3 quaternion(double w, double x, double y, double z); + static Rot3 rodriguez(const Vector& v); Matrix matrix() const; Matrix transpose() const; Vector xyz() const; Vector ypr() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly void print(string s) const; bool equals(const Rot3& rot, double tol) const; Vector localCoordinates(const Rot3& p); Rot3 retract(Vector v); Rot3 compose(const Rot3& p2); Rot3 between(const Rot3& p2); - - // Depreciated interface - Rot3* compose_(const Rot3& p2); - Rot3* between_(const Rot3& p2); - Rot3* retract_(Vector v); - static Rot3* Expmap_(Vector v); }; class Pose2 { @@ -138,12 +134,6 @@ class Pose2 { Pose2 retract(Vector v); Pose2 compose(const Pose2& p2); Pose2 between(const Pose2& p2); - - // Depreciated interface - Pose2* compose_(const Pose2& p2); - Pose2* between_(const Pose2& p2); - Pose2* retract_(Vector v); - static Pose2* Expmap_(Vector v); }; class Pose3 { @@ -165,14 +155,6 @@ class Pose3 { Pose3 retract(Vector v); Point3 translation() const; Rot3 rotation() const; - - // Depreciated interface - static Pose3* Expmap_(Vector v); - Pose3* compose_(const Pose3& p2); - Pose3* between_(const Pose3& p2); - Pose3* retract_(Vector v); - Point3* translation_() const; - Rot3* rotation_() const; }; class SharedGaussian { @@ -314,7 +296,7 @@ class PlanarSLAMOdometry { class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianBayesNet* eliminate() const; - VectorValues* optimize() const; // FAIL: parse error here + VectorValues* optimize() const; GaussianFactor* marginalFactor(int j) const; Matrix marginalCovariance(int j) const; }; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 18a390d7b..12f299637 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -72,11 +72,6 @@ public: return *this + p2; } - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Point2& p2) { - return boost::shared_ptr(new Point2(compose(p2))); - } - /** operators */ inline Point2 operator- () const {return Point2(-x_,-y_);} inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} @@ -97,11 +92,6 @@ public: /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Point2(retract(v))); - } - /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } @@ -111,9 +101,6 @@ public: /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Point2(Expmap(v))); - } /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } @@ -149,11 +136,6 @@ public: return p2 - (*this); } - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Point2& p2) { - return boost::shared_ptr(new Point2(between(p2))); - } - /** get functions for x, y */ double x() const {return x_;} double y() const {return y_;} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7b1520a16..a01f5009b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -76,11 +76,6 @@ namespace gtsam { return *this + p2; } - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Point3& p2) { - return boost::shared_ptr(new Point3(compose(p2))); - } - /// @} /// @name Manifold /// @{ @@ -94,11 +89,6 @@ namespace gtsam { /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Point3(retract(v))); - } - /// Returns inverse retraction inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } @@ -108,9 +98,6 @@ namespace gtsam { /** Exponential map at identity - just create a Point3 from x,y,z */ static inline Point3 Expmap(const Vector& v) { return Point3(v); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Point3(Expmap(v))); - } /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } @@ -151,11 +138,6 @@ namespace gtsam { return p2 - *this; } - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Point3& p2) { - return boost::shared_ptr(new Point3(between(p2))); - } - /** return vectorized form (column-wise)*/ Vector vector() const { Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index cee792ea6..74cb2453d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -103,11 +103,6 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(compose(p2))); - } - /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); @@ -126,11 +121,6 @@ public: /// Retraction from R^3 to Pose2 manifold neighborhood around current pose Pose2 retract(const Vector& v) const; - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Pose2(retract(v))); - } - /// Local 3D coordinates of Pose2 manifold neighborhood around current pose Vector localCoordinates(const Pose2& p2) const; @@ -140,9 +130,6 @@ public: /// Exponential map from Lie algebra se(2) to SE(2) static Pose2 Expmap(const Vector& xi); - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Pose2(Expmap(v))); - } /// Exponential map from SE(2) to Lie algebra se(2) static Vector Logmap(const Pose2& p); @@ -159,11 +146,6 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(between(p2))); - } - /** * Return point coordinates in pose coordinate frame */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index be9966c60..3bcda0c00 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -96,11 +96,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose3& p2) { - return boost::shared_ptr(new Pose3(compose(p2))); - } - /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); @@ -120,11 +115,6 @@ namespace gtsam { /// Retraction from R^6 to Pose3 manifold neighborhood around current pose Pose3 retract(const Vector& d) const; - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Pose3(retract(v))); - } - /// Local 6D coordinates of Pose3 manifold neighborhood around current pose Vector localCoordinates(const Pose3& T2) const; @@ -134,9 +124,6 @@ namespace gtsam { /// Exponential map from Lie algebra se(3) to SE(3) static Pose3 Expmap(const Vector& xi); - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Pose3(Expmap(v))); - } /// Exponential map from SE(3) to Lie algebra se(3) static Vector Logmap(const Pose3& p); @@ -164,11 +151,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose3& p2) { - return boost::shared_ptr(new Pose3(between(p2))); - } - /** * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index c52776cb9..1a6d91b20 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -138,11 +138,6 @@ namespace gtsam { return *this * R1; } - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Rot2& p2) { - return boost::shared_ptr(new Rot2(compose(p2))); - } - /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); @@ -170,11 +165,6 @@ namespace gtsam { /// Updates a with tangent space delta inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Rot2(retract(v))); - } - /// Returns inverse retraction inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } @@ -189,9 +179,6 @@ namespace gtsam { else return Rot2::fromAngle(v(0)); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Rot2(Expmap(v))); - } /// Logmap around identity - return the angle of the rotation static inline Vector Logmap(const Rot2& r) { @@ -219,11 +206,6 @@ namespace gtsam { return between_default(*this, p2); } - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Rot2& p2) { - return boost::shared_ptr(new Rot2(between(p2))); - } - /** return 2*2 rotation matrix */ Matrix matrix() const; diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 5f71b0668..e41e01ef3 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -152,11 +152,6 @@ namespace gtsam { Rot3M compose(const Rot3M& R2, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Rot3M& p2) { - return boost::shared_ptr(new Rot3M(compose(p2))); - } - /// rotate point from rotated coordinate frame to world = R*p inline Point3 operator*(const Point3& p) const { return rotate(p);} @@ -182,11 +177,6 @@ namespace gtsam { /// Updates a with tangent space delta Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Rot3M(retract(v))); - } - /// Returns inverse retraction Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } @@ -202,9 +192,6 @@ namespace gtsam { if(zero(v)) return Rot3M(); else return rodriguez(v); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Rot3M(Expmap(v))); - } /** * Log map at identity - return the canonical coordinates of this rotation @@ -260,11 +247,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Rot3M& p2) { - return boost::shared_ptr(new Rot3M(between(p2))); - } - /** compose two rotations */ Rot3M operator*(const Rot3M& R2) const { return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); From f9ef0e479c1dc5ed3821c165c9217c2bb1c611b2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 7 Dec 2011 16:41:50 +0000 Subject: [PATCH 44/72] Added syntactic sugar for access to yaw, pitch, roll from Rot3M with wrap access --- gtsam.h | 5 +++++ gtsam/geometry/Rot3M.h | 12 +++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 432c95698..356914127 100644 --- a/gtsam.h +++ b/gtsam.h @@ -107,6 +107,9 @@ class Rot3 { Matrix transpose() const; Vector xyz() const; Vector ypr() const; + double roll() const; + double pitch() const; + double yaw() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly void print(string s) const; bool equals(const Rot3& rot, double tol) const; @@ -134,6 +137,8 @@ class Pose2 { Pose2 retract(Vector v); Pose2 compose(const Pose2& p2); Pose2 between(const Pose2& p2); + Rot2 bearing(const Point2& point); + double range(const Point2& point); }; class Pose3 { diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index e41e01ef3..02ca733d1 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -226,10 +226,20 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3M::ypr(y,p,r) */ Vector rpy() const; + /** + * Accessors to get to components of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient + */ + inline double roll() const { return ypr()(2); } + inline double pitch() const { return ypr()(1); } + inline double yaw() const { return ypr()(0); } + /** Compute the quaternion representation of this rotation. * @return The quaternion */ From 56818da2243f388223741a25254c563e1cdc7082 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Wed, 7 Dec 2011 17:55:38 +0000 Subject: [PATCH 45/72] Added dependency checking to wrapper. Wrapper will now throw an exception if an object depends on another object which has not been wrapped. Tests for dependency checking added. Moved geometry.h to tests folder. --- wrap/Module.cpp | 46 +++++++++++++++++++++++++++++++------ wrap/{ => tests}/geometry.h | 0 wrap/tests/testWrap.cpp | 13 +++++++++-- wrap/tests/testWrap1.h | 23 +++++++++++++++++++ wrap/utilities.h | 16 +++++++++++++ 5 files changed, 89 insertions(+), 9 deletions(-) rename wrap/{ => tests}/geometry.h (100%) create mode 100644 wrap/tests/testWrap1.h diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 364e172ea..c378e01aa 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -209,7 +209,6 @@ void Module::matlab_code(const string& toolboxPath, const string& mexExt, const string& mexFlags) { - try { string installCmd = "install -d " + toolboxPath; system(installCmd.c_str()); @@ -238,12 +237,27 @@ void Module::matlab_code(const string& toolboxPath, make_ofs << "MEXENDING = " << mexExt << "\n"; make_ofs << "mex_flags = " << mexFlags << "\n\n"; + //Dependency check list + std::vector validArgs; + validArgs.push_back("string"); + validArgs.push_back("int"); + validArgs.push_back("bool"); + validArgs.push_back("size_t"); + validArgs.push_back("double"); + validArgs.push_back("Vector"); + validArgs.push_back("Matrix"); + // add 'all' to Makefile make_ofs << "all: "; - BOOST_FOREACH(Class cls, classes) + BOOST_FOREACH(Class cls, classes) { make_ofs << cls.name << " "; + //Create a list of parsed classes for dependency checking + validArgs.push_back(cls.name); + } make_ofs << "\n\n"; + + // generate proxy classes and wrappers BOOST_FOREACH(Class cls, classes) { // create directory if needed @@ -256,8 +270,31 @@ void Module::matlab_code(const string& toolboxPath, cls.matlab_proxy(classFile); // create constructor and method wrappers + BOOST_FOREACH(Constructor con, cls.constructors) { + BOOST_FOREACH(Argument arg, con.args) { + if(std::find(validArgs.begin(), validArgs.end(), arg.type) + == validArgs.end()) + throw DependencyMissing(arg.type, cls.name); + } + } cls.matlab_constructors(toolboxPath,nameSpace); + + BOOST_FOREACH(StaticMethod stMth, cls.static_methods) { + BOOST_FOREACH(Argument arg, stMth.args) { + if(std::find(validArgs.begin(), validArgs.end(), arg.type) + == validArgs.end()) + throw DependencyMissing(arg.type, stMth.name); + } + } cls.matlab_static_methods(toolboxPath,nameSpace); + + BOOST_FOREACH(Method mth, cls.methods) { + BOOST_FOREACH(Argument arg, mth.args_) { + if(std::find(validArgs.begin(), validArgs.end(), arg.type) + == validArgs.end()) + throw DependencyMissing(arg.type, mth.name_); + } + } cls.matlab_methods(classPath,nameSpace); // add lines to make m-file @@ -285,10 +322,5 @@ void Module::matlab_code(const string& toolboxPath, make_ofs << "\n" << endl; make_ofs.close(); } - catch(exception &e) { - cerr << "generate_matlab_toolbox failed because " << e.what() << endl; - } - -} /* ************************************************************************* */ diff --git a/wrap/geometry.h b/wrap/tests/geometry.h similarity index 100% rename from wrap/geometry.h rename to wrap/tests/geometry.h diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 4cc8b7cd2..01dc8280a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -48,14 +48,22 @@ TEST( wrap, ArgumentList ) { TEST( wrap, check_exception ) { THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); + + string path = topdir + "/wrap/tests"; + Module module(path.c_str(), "testWrap1",enable_verbose); + THROWS_EXCEPTION(throw DependencyMissing("a", "b")); + CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); + } /* ************************************************************************* */ TEST( wrap, parse ) { - string path = topdir + "/wrap"; + string path = topdir + "/wrap/tests"; Module module(path.c_str(), "geometry",enable_verbose); CHECK(module.classes.size()==3); + //Hack to solve issues with instantiating Modules + path = topdir + "/wrap"; // check second class, Point3 Class cls = *(++module.classes.begin()); @@ -98,8 +106,9 @@ TEST( wrap, parse ) { /* ************************************************************************* */ TEST( wrap, matlab_code ) { // Parse into class object - string path = topdir + "/wrap"; + string path = topdir + "/wrap/tests"; Module module(path,"geometry",enable_verbose); + path = topdir + "/wrap"; // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make diff --git a/wrap/tests/testWrap1.h b/wrap/tests/testWrap1.h new file mode 100644 index 000000000..0cc16fc71 --- /dev/null +++ b/wrap/tests/testWrap1.h @@ -0,0 +1,23 @@ +//Header file to test dependency checking +// +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t);//What is Rot3? Throw here + Pose3(Vector v); + Pose3(Matrix t); + static Pose3 Expmap(Vector v); + static Vector Logmap(const Pose3& p); + static Rot3 testStaticDep(Rot3& r);//What is Rot3? Throw here + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + Matrix adjointMap() const; + Pose3 compose(const Pose3& p2); + Pose3 between(const Pose3& p2); + Pose3 retract(Vector v); + Point3 translation() const; + Rot3 rotation() const; //What is Rot3? Throw here +}; diff --git a/wrap/utilities.h b/wrap/utilities.h index ee636b58c..f43b9d9fb 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -46,6 +46,22 @@ class ParseFailed : public std::exception { } }; +class DependencyMissing : public std::exception { + private: + std::string dependency_; + std::string location_; + public: + DependencyMissing(const std::string& dep, const std::string& loc) { + dependency_ = dep; + location_ = loc; + } + ~DependencyMissing() throw() {} + virtual const char* what() const throw() { + return ("Missing dependency " + dependency_ + " in " + location_).c_str(); + } +}; + + /** * read contents of a file into a std::string */ From 076ae3d8059893c6bd571ff7930131feaeef2492 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 01:16:03 +0000 Subject: [PATCH 46/72] Added SharedNoiseModel to gtsam.h to pacify parse error, but needs actual constructors --- gtsam.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam.h b/gtsam.h index 356914127..b7fdfacdb 100644 --- a/gtsam.h +++ b/gtsam.h @@ -263,6 +263,11 @@ class Ordering { void push_back(string key); }; +class SharedNoiseModel { + SharedNoiseModel(); + // FIXME: this needs actual constructors +}; + class PlanarSLAMValues { PlanarSLAMValues(); void print(string s) const; From b9017198dbbcaa08014d6984f1f3a76e8e54e270 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 01:16:04 +0000 Subject: [PATCH 47/72] Removed extraneous matlab-related functions and duplicated, commented tests --- gtsam/geometry/Pose3.h | 7 --- gtsam/geometry/tests/testPose3.cpp | 84 ------------------------------ 2 files changed, 91 deletions(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 3bcda0c00..17b547e79 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -56,14 +56,7 @@ namespace gtsam { T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} const Rot3& rotation() const { return R_; } - boost::shared_ptr rotation_() const { - return boost::shared_ptr(new Rot3(R_)); - } - const Point3& translation() const { return t_; } - boost::shared_ptr translation_() const { - return boost::shared_ptr(new Point3(t_)); - } double x() const { return t_.x(); } double y() const { return t_.y(); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index afc177256..94b81cb0c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -51,7 +51,6 @@ TEST( Pose3, expmap_a) v(0) = 0.3; EXPECT(assert_equal(id.retract(v), Pose3(R, Point3()))); #ifdef CORRECT_POSE3_EXMAP - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; #else v(3)=0.2;v(4)=0.7;v(5)=-2; @@ -100,89 +99,6 @@ namespace screw { Pose3 expected(expectedR, expectedT); } -#ifdef CORRECT_POSE3_EXMAP -/* ************************************************************************* */ -TEST(Pose3, expmap_c) -{ - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); -} - -/* ************************************************************************* */ -// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) -TEST(Pose3, Adjoint) -{ - Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); - Vector xiprime = Adjoint(T, screw::xi); - EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - - Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); - Vector xiprime2 = Adjoint(T2, screw::xi); - EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - - Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); - Vector xiprime3 = Adjoint(T3, screw::xi); - EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); -} - -/* ************************************************************************* */ -/** Agrawal06iros version */ -Pose3 Agrawal06iros(const Vector& xi) { - Vector w = vector_range(xi, range(0,3)); - Vector v = vector_range(xi, range(3,6)); - double t = norm_2(w); - if (t < 1e-5) - return Pose3(Rot3(), expmap (v)); - else { - Matrix W = skewSymmetric(w/t); - Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), expmap (A * v)); - } -} - -/* ************************************************************************* */ -TEST(Pose3, expmaps_galore) -{ - Vector xi; Pose3 actual; - xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); - actual = Pose3::Expmap(xi); - EXPECT(assert_equal(expm(xi), actual,1e-6)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); - - xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); - for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { - Vector txi = xi*theta; - actual = Pose3::Expmap(txi); - EXPECT(assert_equal(expm(txi,30), actual,1e-6)); - EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); - Vector log = localCoordinates(actual); - EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6)); - EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps - } - - // Works with large v as well, but expm needs 10 iterations! - xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); - actual = Pose3::Expmap(xi); - EXPECT(assert_equal(expm(xi,10), actual,1e-5)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); -} - -/* ************************************************************************* */ -TEST(Pose3, Adjoint_compose) -{ - // To debug derivatives of compose, assert that - // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 - const Pose3& T1 = T; - Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); - Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = Adjoint(inverse(T2), x); - Pose3 actual = T1 * T2 * Pose3::Expmap(y); - EXPECT(assert_equal(expected, actual, 1e-6)); -} -#endif // SLOW_BUT_CORRECT_EXMAP - /* ************************************************************************* */ TEST(Pose3, expmap_c_full) { From 3cb1d27d4bc15a06ea9a43d5f37d51c3511584fa Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 8 Dec 2011 18:57:19 +0000 Subject: [PATCH 48/72] Made LDL debugging slightly more verbose --- gtsam/base/cholesky.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 16f07f5ab..3f015e9d3 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -164,13 +164,14 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) if(debug) { cout << "Partial LDL with " << nFrontal << " frontal scalars, "; - print(ABC, "ABC: "); + print(ABC, "LDL ABC: "); } // Compute Cholesky factorization of A, overwrites A = sqrt(D)*R // tic(1, "ldl"); Eigen::LDLT ldlt; ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView()); + if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl; if(ldlt.vectorD().unaryExpr(boost::bind(less(), _1, 0.0)).any()) { if(ISDEBUG("detailed_exceptions")) @@ -180,14 +181,15 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) } Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL - if (debug) cout << "Dsqrt: " << sqrtD << endl; + if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl; // U = sqrtD * L^ Matrix U = ldlt.matrixU(); + if(debug) cout << "LDL U:\n" << U << endl; // we store the permuted upper triangular matrix ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky - if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; + if(debug) cout << "LDL R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; // toc(1, "ldl"); // Compute S = inv(R') * B = inv(P'U')*B = inv(U')*P*B @@ -196,20 +198,19 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) ABC.topRightCorner(nFrontal, n-nFrontal) = ldlt.transpositionsP() * ABC.topRightCorner(nFrontal, n-nFrontal); ABC.block(0,0,nFrontal,nFrontal).triangularView().transpose().solveInPlace(ABC.topRightCorner(nFrontal, n-nFrontal)); } - if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; + if(debug) cout << "LDL S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; // toc(2, "compute S"); // Compute L = C - S' * S // tic(3, "compute L"); - if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if(debug) cout << "LDL C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); - if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if(debug) cout << "LDL L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; // toc(3, "compute L"); - if(debug) cout << "P: " << ldlt.transpositionsP().indices() << endl; - + if(debug) cout << "LDL P: " << ldlt.transpositionsP().indices() << endl; return ldlt.transpositionsP(); } From 1130cf43e05cfca6dc1be6646ac03daee6bec327 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:50:38 +0000 Subject: [PATCH 49/72] Limited namespace handling in wrap parser --- wrap/Class.h | 1 + wrap/Module.cpp | 43 ++++++++++------ wrap/tests/geometry.h | 6 +++ wrap/tests/testWrap.cpp | 107 ++++++++++++++++++++++------------------ wrap/utilities.cpp | 8 ++- 5 files changed, 101 insertions(+), 64 deletions(-) diff --git a/wrap/Class.h b/wrap/Class.h index 57572b4cf..f02456f89 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -36,6 +36,7 @@ struct Class { std::list constructors; ///< Class constructors std::list methods; ///< Class methods std::list static_methods; ///< Static methods + std::vector namespaces; ///< Stack of namespaces bool verbose_; ///< verbose flag // And finally MATLAB code is emitted, methods below called by Module::matlab_code diff --git a/wrap/Module.cpp b/wrap/Module.cpp index c378e01aa..bfb6a3145 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -51,19 +51,16 @@ Module::Module(const string& interfacePath, Method method0(enable_verbose), method(enable_verbose); StaticMethod static_method0(enable_verbose), static_method(enable_verbose); Class cls0(enable_verbose),cls(enable_verbose); + vector namespaces, namespaces_parent; //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are // defined within the square brackets [] and are executed whenever a // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug. - // The grammar is allows a very restricted C++ header: - // - No comments allowed. - // -Only types allowed are string, bool, size_t int, double, Vector, and Matrix - // as well as class names that start with an uppercase letter - // - The types unsigned int and bool should be specified as int. + // The grammar is allows a very restricted C++ header // ---------------------------------------------------------------------------- - Rule comments_p = comment_p("/*", "*/") | comment_p("//"); + Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); // lexeme_d turns off white space skipping // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html @@ -157,16 +154,22 @@ Module::Module(const string& interfacePath, Rule functions_p = constructor_p | method_p | static_method_p; - Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> + Rule class_p = (str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> *(functions_p | comments_p) >> - '}' >> ";"; + str_p("};"))[assign_a(cls.namespaces, namespaces)][push_back_a(classes,cls)][assign_a(cls,cls0)]; - Rule module_p = *comments_p >> +(class_p - [push_back_a(classes,cls)] - [assign_a(cls,cls0)] - >> *comments_p) - >> *comments_p - >> !end_p; + Rule namespace_name_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + Rule namespace_p = str_p("namespace") >> + namespace_name_p[assign_a(namespaces_parent, namespaces)][push_back_a(namespaces)] // save previous state + >> confix_p('{', // start namespace + +(comments_p | class_p | namespace_p), + ('}' >> (*anychar_p - ch_p(';'))) // end namespace, avoid confusion with classes + )[assign_a(namespaces, namespaces_parent)]; // switch back to parent namespace + + Rule module_content_p = comments_p | namespace_p | class_p; + + Rule module_p = *module_content_p >> !end_p; //---------------------------------------------------------------------------- // for debugging, define BOOST_SPIRIT_DEBUG @@ -193,7 +196,7 @@ Module::Module(const string& interfacePath, // read interface file string interfaceFile = interfacePath + "/" + moduleName + ".h"; - string contents = wrap::file_contents(interfaceFile); + string contents = file_contents(interfaceFile); // and parse contents parse_info info = parse(contents.c_str(), module_p, space_p); @@ -201,6 +204,16 @@ Module::Module(const string& interfacePath, printf("parsing stopped at \n%.20s\n",info.stop); throw ParseFailed(info.length); } + + if (!namespaces.empty()) { + cout << "Namespaces not closed, remaining: "; + BOOST_FOREACH(const string& ns, namespaces) + cout << ns << " "; + cout << endl; + } + + if (!cls.name.empty()) + cout << "\nClass name: " << cls.name << endl; } /* ************************************************************************* */ diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 8d598ba25..843190e5c 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -9,6 +9,8 @@ class Point2 { VectorNotEigen vectorConfusion(); }; +namespace ns_inner { + class Point3 { Point3(double x, double y, double z); double norm() const; @@ -20,6 +22,8 @@ class Point3 { // another comment +// another comment + /** * A multi-line comment! */ @@ -67,6 +71,8 @@ class Test { // even more comments at the end! }; +} // \ns_inner + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 01dc8280a..de1935f50 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -34,26 +34,24 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define /* ************************************************************************* */ TEST( wrap, ArgumentList ) { - ArgumentList args; - Argument arg; arg.type = "double"; arg.name = "x"; - args.push_back(arg); - args.push_back(arg); - args.push_back(arg); - CHECK(args.signature()=="ddd"); - EXPECT(args.types()=="double,double,double"); - EXPECT(args.names()=="x,x,x"); + ArgumentList args; + Argument arg; arg.type = "double"; arg.name = "x"; + args.push_back(arg); + args.push_back(arg); + args.push_back(arg); + CHECK(args.signature()=="ddd"); + EXPECT(args.types()=="double,double,double"); + EXPECT(args.names()=="x,x,x"); } /* ************************************************************************* */ TEST( wrap, check_exception ) { THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); - - string path = topdir + "/wrap/tests"; - Module module(path.c_str(), "testWrap1",enable_verbose); - THROWS_EXCEPTION(throw DependencyMissing("a", "b")); - CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); + string path = topdir + "/wrap/tests"; + Module module(path.c_str(), "testWrap1",enable_verbose); +// CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); } /* ************************************************************************* */ @@ -61,46 +59,61 @@ TEST( wrap, parse ) { string path = topdir + "/wrap/tests"; Module module(path.c_str(), "geometry",enable_verbose); - CHECK(module.classes.size()==3); + EXPECT_LONGS_EQUAL(3, module.classes.size()); //Hack to solve issues with instantiating Modules path = topdir + "/wrap"; + // check first class, Point2 + { + Class cls = module.classes.at(0); + EXPECT(assert_equal("Point2", cls.name)); + EXPECT_LONGS_EQUAL(2, cls.constructors.size()); + EXPECT_LONGS_EQUAL(4, cls.methods.size()); + EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); + EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + } + // check second class, Point3 - Class cls = *(++module.classes.begin()); - EXPECT(cls.name=="Point3"); - EXPECT(cls.constructors.size()==1); - EXPECT(cls.methods.size()==1); - EXPECT(cls.static_methods.size()==2); + { + Class cls = module.classes.at(1); + EXPECT(assert_equal("Point3", cls.name)); + EXPECT_LONGS_EQUAL(1, cls.constructors.size()); + EXPECT_LONGS_EQUAL(1, cls.methods.size()); + EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); + EXPECT_LONGS_EQUAL(1, cls.namespaces.size()); + EXPECT(assert_equal("ns_inner", cls.namespaces.front())); - // first constructor takes 3 doubles - Constructor c1 = cls.constructors.front(); - EXPECT(c1.args.size()==3); + // first constructor takes 3 doubles + Constructor c1 = cls.constructors.front(); + EXPECT_LONGS_EQUAL(3, c1.args.size()); - // check first double argument - Argument a1 = c1.args.front(); - EXPECT(!a1.is_const); - EXPECT(a1.type=="double"); - EXPECT(!a1.is_ref); - EXPECT(a1.name=="x"); + // check first double argument + Argument a1 = c1.args.front(); + EXPECT(!a1.is_const); + EXPECT(assert_equal("double", a1.type)); + EXPECT(!a1.is_ref); + EXPECT(assert_equal("x", a1.name)); - // check method - Method m1 = cls.methods.front(); - EXPECT(m1.returnVal_.type1=="double"); - EXPECT(m1.name_=="norm"); - EXPECT(m1.args_.size()==0); - EXPECT(m1.is_const_); + // check method + Method m1 = cls.methods.front(); + EXPECT(assert_equal("double", m1.returnVal_.type1)); + EXPECT(assert_equal("norm", m1.name_)); + EXPECT_LONGS_EQUAL(0, m1.args_.size()); + EXPECT(m1.is_const_); + } - // Test class is the third one - Class testCls = module.classes.at(2); - EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); - EXPECT_LONGS_EQUAL(19, testCls.methods.size()); - EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); - - // function to parse: pair return_pair (Vector v, Matrix A) const; - Method m2 = testCls.methods.front(); - EXPECT(m2.returnVal_.isPair); - EXPECT(m2.returnVal_.category1 == ReturnValue::EIGEN); - EXPECT(m2.returnVal_.category2 == ReturnValue::EIGEN); + // // Test class is the third one + // LONGS_EQUAL(3, module.classes.size()); + // Class testCls = module.classes.at(2); + // EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); + // EXPECT_LONGS_EQUAL(19, testCls.methods.size()); + // EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); + // + // // function to parse: pair return_pair (Vector v, Matrix A) const; + // Method m2 = testCls.methods.front(); + // EXPECT(m2.returnVal_.isPair); + // EXPECT(m2.returnVal_.category1 == ReturnValue::EIGEN); + // EXPECT(m2.returnVal_.category2 == ReturnValue::EIGEN); } /* ************************************************************************* */ @@ -137,8 +150,8 @@ TEST( wrap, matlab_code ) { EXPECT(files_equal(path + "/tests/expected/@Test/print.m" , "actual/@Test/print.m" )); EXPECT(files_equal(path + "/tests/expected/@Test/print.cpp" , "actual/@Test/print.cpp" )); - EXPECT(files_equal(path + "/tests/expected/make_geometry.m" , "actual/make_geometry.m" )); - EXPECT(files_equal(path + "/tests/expected/Makefile" , "actual/Makefile" )); + EXPECT(files_equal(path + "/tests/expected/make_geometry.m", "actual/make_geometry.m")); + EXPECT(files_equal(path + "/tests/expected/Makefile" , "actual/Makefile" )); } /* ************************************************************************* */ diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 8a30ed3bc..2dc93b49d 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -54,8 +54,8 @@ bool assert_equal(const std::string& expected, const std::string& actual) { /* ************************************************************************* */ bool files_equal(const string& expected, const string& actual, bool skipheader) { try { - string expected_contents = wrap::file_contents(expected, skipheader); - string actual_contents = wrap::file_contents(actual, skipheader); + string expected_contents = file_contents(expected, skipheader); + string actual_contents = file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { stringstream command; @@ -68,6 +68,10 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "expection: " << reason << endl; return false; } + catch (CantOpenFile& e) { + cerr << "file opening error: " << e.what() << endl; + return false; + } return true; } From 1aecb58807ef4678c9947f0dddb88ac9ed0fbc7e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:51:05 +0000 Subject: [PATCH 50/72] Simple namespace works --- wrap/Module.cpp | 14 ++++++------- wrap/tests/geometry.h | 5 +++-- wrap/tests/testWrap.cpp | 44 +++++++++++++++++++++++------------------ 3 files changed, 35 insertions(+), 28 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index bfb6a3145..bc2bc5afe 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -160,14 +160,14 @@ Module::Module(const string& interfacePath, Rule namespace_name_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - Rule namespace_p = str_p("namespace") >> - namespace_name_p[assign_a(namespaces_parent, namespaces)][push_back_a(namespaces)] // save previous state - >> confix_p('{', // start namespace - +(comments_p | class_p | namespace_p), - ('}' >> (*anychar_p - ch_p(';'))) // end namespace, avoid confusion with classes - )[assign_a(namespaces, namespaces_parent)]; // switch back to parent namespace + Rule namespace_p = str_p("namespace") >> + namespace_name_p[assign_a(namespaces_parent, namespaces)][push_back_a(namespaces)] // save previous state + >> ch_p('{') >> + *(class_p | namespace_p | comments_p) >> + str_p("}//\\namespace") // end namespace, avoid confusion with classes + [assign_a(namespaces, namespaces_parent)]; // switch back to parent namespace - Rule module_content_p = comments_p | namespace_p | class_p; + Rule module_content_p = comments_p | class_p | namespace_p ; Rule module_p = *module_content_p >> !end_p; diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 843190e5c..c6d770e5e 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -22,6 +22,9 @@ class Point3 { // another comment +// NOTE: you *must* end namespaces as follows: +}//\namespace + // another comment /** @@ -71,8 +74,6 @@ class Test { // even more comments at the end! }; -} // \ns_inner - // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index de1935f50..e49fb88d9 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,6 +15,7 @@ * @author Frank Dellaert **/ +#include #include #include #include @@ -56,12 +57,11 @@ TEST( wrap, check_exception ) { /* ************************************************************************* */ TEST( wrap, parse ) { - string path = topdir + "/wrap/tests"; + string header_path = topdir + "/wrap/tests"; - Module module(path.c_str(), "geometry",enable_verbose); + Module module(header_path.c_str(), "geometry",enable_verbose); EXPECT_LONGS_EQUAL(3, module.classes.size()); - //Hack to solve issues with instantiating Modules - path = topdir + "/wrap"; + string path = topdir + "/wrap"; // check first class, Point2 { @@ -102,26 +102,32 @@ TEST( wrap, parse ) { EXPECT(m1.is_const_); } - // // Test class is the third one - // LONGS_EQUAL(3, module.classes.size()); - // Class testCls = module.classes.at(2); - // EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); - // EXPECT_LONGS_EQUAL(19, testCls.methods.size()); - // EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); - // - // // function to parse: pair return_pair (Vector v, Matrix A) const; - // Method m2 = testCls.methods.front(); - // EXPECT(m2.returnVal_.isPair); - // EXPECT(m2.returnVal_.category1 == ReturnValue::EIGEN); - // EXPECT(m2.returnVal_.category2 == ReturnValue::EIGEN); + // Test class is the third one + { + LONGS_EQUAL(3, module.classes.size()); + Class testCls = module.classes.at(2); + EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); + EXPECT_LONGS_EQUAL(19, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); + + // function to parse: pair return_pair (Vector v, Matrix A) const; + Method m2 = testCls.methods.front(); + EXPECT(m2.returnVal_.isPair); + EXPECT(m2.returnVal_.category1 == ReturnValue::EIGEN); + EXPECT(m2.returnVal_.category2 == ReturnValue::EIGEN); + } } /* ************************************************************************* */ TEST( wrap, matlab_code ) { // Parse into class object - string path = topdir + "/wrap/tests"; - Module module(path,"geometry",enable_verbose); - path = topdir + "/wrap"; + string header_path = topdir + "/wrap/tests"; + Module module(header_path,"geometry",enable_verbose); + string path = topdir + "/wrap"; + + // clean out previous generated code + string cleanCmd = "rm -rf actual"; + system(cleanCmd.c_str()); // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make From 90e9426d9c2185ba9e4fc5c430353b601b83a9f4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:51:07 +0000 Subject: [PATCH 51/72] Added more tests for namespaces, parser now supports nested namespaces --- wrap/Module.cpp | 20 +++++++++---------- wrap/tests/geometry.h | 6 +++++- wrap/tests/testNamespaces.h | 31 +++++++++++++++++++++++++++++ wrap/tests/testWrap.cpp | 39 ++++++++++++++++++++++++++++++++----- 4 files changed, 80 insertions(+), 16 deletions(-) create mode 100644 wrap/tests/testNamespaces.h diff --git a/wrap/Module.cpp b/wrap/Module.cpp index bc2bc5afe..2b6c6ffe2 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -164,7 +164,7 @@ Module::Module(const string& interfacePath, namespace_name_p[assign_a(namespaces_parent, namespaces)][push_back_a(namespaces)] // save previous state >> ch_p('{') >> *(class_p | namespace_p | comments_p) >> - str_p("}//\\namespace") // end namespace, avoid confusion with classes + str_p("}///\\namespace") // end namespace, avoid confusion with classes [assign_a(namespaces, namespaces_parent)]; // switch back to parent namespace Rule module_content_p = comments_p | class_p | namespace_p ; @@ -205,15 +205,15 @@ Module::Module(const string& interfacePath, throw ParseFailed(info.length); } - if (!namespaces.empty()) { - cout << "Namespaces not closed, remaining: "; - BOOST_FOREACH(const string& ns, namespaces) - cout << ns << " "; - cout << endl; - } - - if (!cls.name.empty()) - cout << "\nClass name: " << cls.name << endl; +// if (!namespaces.empty()) { +// cout << "Namespaces not closed, remaining: "; +// BOOST_FOREACH(const string& ns, namespaces) +// cout << ns << " "; +// cout << endl; +// } +// +// if (!cls.name.empty()) +// cout << "\nClass name: " << cls.name << endl; } /* ************************************************************************* */ diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index c6d770e5e..b8427faa3 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -9,6 +9,8 @@ class Point2 { VectorNotEigen vectorConfusion(); }; +namespace ns_outer { + namespace ns_inner { class Point3 { @@ -23,7 +25,7 @@ class Point3 { // another comment // NOTE: you *must* end namespaces as follows: -}//\namespace +}///\namespace // another comment @@ -74,6 +76,8 @@ class Test { // even more comments at the end! }; +}///\namespace + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h new file mode 100644 index 000000000..59c80201c --- /dev/null +++ b/wrap/tests/testNamespaces.h @@ -0,0 +1,31 @@ +/** + * This is a wrap header to verify permutations on namespaces + */ + +namespace ns1 { + +class ClassA { + +}; + +class ClassB { + +}; + +}///\namespace + +namespace ns2 { + +class ClassA { +}; + +namespace ns3 { + +class ClassB { +}; + +}///\namespace + +}///\namespace + + diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index e49fb88d9..9241a8ec6 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -52,16 +52,14 @@ TEST( wrap, check_exception ) { string path = topdir + "/wrap/tests"; Module module(path.c_str(), "testWrap1",enable_verbose); -// CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); + CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); } /* ************************************************************************* */ TEST( wrap, parse ) { string header_path = topdir + "/wrap/tests"; - Module module(header_path.c_str(), "geometry",enable_verbose); EXPECT_LONGS_EQUAL(3, module.classes.size()); - string path = topdir + "/wrap"; // check first class, Point2 { @@ -80,8 +78,9 @@ TEST( wrap, parse ) { EXPECT_LONGS_EQUAL(1, cls.constructors.size()); EXPECT_LONGS_EQUAL(1, cls.methods.size()); EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); - EXPECT_LONGS_EQUAL(1, cls.namespaces.size()); - EXPECT(assert_equal("ns_inner", cls.namespaces.front())); + EXPECT_LONGS_EQUAL(2, cls.namespaces.size()); + EXPECT(assert_equal("ns_outer", cls.namespaces.front())); + EXPECT(assert_equal("ns_inner", cls.namespaces.back())); // first constructor takes 3 doubles Constructor c1 = cls.constructors.front(); @@ -109,6 +108,8 @@ TEST( wrap, parse ) { EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); EXPECT_LONGS_EQUAL(19, testCls.methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); + EXPECT_LONGS_EQUAL( 1, testCls.namespaces.size()); + EXPECT(assert_equal("ns_outer", testCls.namespaces.front())); // function to parse: pair return_pair (Vector v, Matrix A) const; Method m2 = testCls.methods.front(); @@ -118,6 +119,34 @@ TEST( wrap, parse ) { } } +/* ************************************************************************* */ +TEST( wrap, parse_namespaces ) { + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "testNamespaces",enable_verbose); + EXPECT_LONGS_EQUAL(4, module.classes.size()); + + Class cls1 = module.classes.at(0); + EXPECT(assert_equal("ClassA", cls1.name)); + EXPECT_LONGS_EQUAL(1, cls1.namespaces.size()); + EXPECT(assert_equal("ns1", cls1.namespaces.front())); + + Class cls2 = module.classes.at(1); + EXPECT(assert_equal("ClassB", cls2.name)); + EXPECT_LONGS_EQUAL(1, cls2.namespaces.size()); + EXPECT(assert_equal("ns1", cls2.namespaces.front())); + + Class cls3 = module.classes.at(2); + EXPECT(assert_equal("ClassA", cls3.name)); + EXPECT_LONGS_EQUAL(1, cls3.namespaces.size()); + EXPECT(assert_equal("ns2", cls3.namespaces.front())); + + Class cls4 = module.classes.at(3); + EXPECT(assert_equal("ClassB", cls4.name)); + EXPECT_LONGS_EQUAL(2, cls4.namespaces.size()); + EXPECT(assert_equal("ns2", cls4.namespaces.front())); + EXPECT(assert_equal("ns3", cls4.namespaces.back())); +} + /* ************************************************************************* */ TEST( wrap, matlab_code ) { // Parse into class object From aa2eccbcb411cf27909091c9041d3fe052131e37 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:51:08 +0000 Subject: [PATCH 52/72] Parsing for namespaces now works in all tests --- wrap/Module.cpp | 17 +++-------- wrap/pop_actor.h | 59 +++++++++++++++++++++++++++++++++++++ wrap/tests/testNamespaces.h | 6 ++++ wrap/tests/testWrap.cpp | 13 +++++++- 4 files changed, 81 insertions(+), 14 deletions(-) create mode 100644 wrap/pop_actor.h diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 2b6c6ffe2..f1b6c4acb 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -16,6 +16,7 @@ #include "Module.h" #include "utilities.h" +#include "pop_actor.h" //#define BOOST_SPIRIT_DEBUG #include @@ -51,7 +52,7 @@ Module::Module(const string& interfacePath, Method method0(enable_verbose), method(enable_verbose); StaticMethod static_method0(enable_verbose), static_method(enable_verbose); Class cls0(enable_verbose),cls(enable_verbose); - vector namespaces, namespaces_parent; + vector namespaces, namespaces_parent, namespaces_temp; //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -161,11 +162,11 @@ Module::Module(const string& interfacePath, Rule namespace_name_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; Rule namespace_p = str_p("namespace") >> - namespace_name_p[assign_a(namespaces_parent, namespaces)][push_back_a(namespaces)] // save previous state + namespace_name_p[push_back_a(namespaces)] >> ch_p('{') >> *(class_p | namespace_p | comments_p) >> str_p("}///\\namespace") // end namespace, avoid confusion with classes - [assign_a(namespaces, namespaces_parent)]; // switch back to parent namespace + [pop_a(namespaces)]; Rule module_content_p = comments_p | class_p | namespace_p ; @@ -204,16 +205,6 @@ Module::Module(const string& interfacePath, printf("parsing stopped at \n%.20s\n",info.stop); throw ParseFailed(info.length); } - -// if (!namespaces.empty()) { -// cout << "Namespaces not closed, remaining: "; -// BOOST_FOREACH(const string& ns, namespaces) -// cout << ns << " "; -// cout << endl; -// } -// -// if (!cls.name.empty()) -// cout << "\nClass name: " << cls.name << endl; } /* ************************************************************************* */ diff --git a/wrap/pop_actor.h b/wrap/pop_actor.h new file mode 100644 index 000000000..97bfdcca6 --- /dev/null +++ b/wrap/pop_actor.h @@ -0,0 +1,59 @@ +/** + * @file pop_actor.h + * + * @brief An actor to pop a vector/container, based off of the clear_actor + * + * @date Dec 8, 2011 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace boost { namespace spirit { + +BOOST_SPIRIT_CLASSIC_NAMESPACE_BEGIN + + /////////////////////////////////////////////////////////////////////////// + // Summary: + // A semantic action policy that calls pop_back method. + // (This doc uses convention available in actors.hpp) + // + // Actions (what it does): + // ref.pop_back(); + // + // Policy name: + // clear_action + // + // Policy holder, corresponding helper method: + // ref_actor, clear_a( ref ); + // + // () operators: both. + // + // See also ref_actor for more details. + /////////////////////////////////////////////////////////////////////////// + struct pop_action + { + template< + typename T + > + void act(T& ref_) const + { + ref_.pop_back(); + } + }; + + /////////////////////////////////////////////////////////////////////////// + // helper method that creates a and_assign_actor. + /////////////////////////////////////////////////////////////////////////// + template + inline ref_actor pop_a(T& ref_) + { + return ref_actor(ref_); + } + +BOOST_SPIRIT_CLASSIC_NAMESPACE_END + +}} + diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 59c80201c..f95e08a74 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -26,6 +26,12 @@ class ClassB { }///\namespace +class ClassC { +}; + }///\namespace +class ClassD { +}; + diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 9241a8ec6..7b60e9ad2 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -123,7 +123,7 @@ TEST( wrap, parse ) { TEST( wrap, parse_namespaces ) { string header_path = topdir + "/wrap/tests"; Module module(header_path.c_str(), "testNamespaces",enable_verbose); - EXPECT_LONGS_EQUAL(4, module.classes.size()); + EXPECT_LONGS_EQUAL(6, module.classes.size()); Class cls1 = module.classes.at(0); EXPECT(assert_equal("ClassA", cls1.name)); @@ -145,6 +145,17 @@ TEST( wrap, parse_namespaces ) { EXPECT_LONGS_EQUAL(2, cls4.namespaces.size()); EXPECT(assert_equal("ns2", cls4.namespaces.front())); EXPECT(assert_equal("ns3", cls4.namespaces.back())); + + Class cls5 = module.classes.at(4); + EXPECT(assert_equal("ClassC", cls5.name)); + EXPECT_LONGS_EQUAL(1, cls5.namespaces.size()); + EXPECT(assert_equal("ns2", cls5.namespaces.front())); + + Class cls6 = module.classes.at(5); + EXPECT(assert_equal("ClassD", cls6.name)); + EXPECT_LONGS_EQUAL(0, cls6.namespaces.size()); + if (!cls6.namespaces.empty()) + cout << "Extraneous namespace: " << cls6.namespaces.front() << endl; } /* ************************************************************************* */ From 48a205602071b74f92a5724f6f47774ef2391b13 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:51:13 +0000 Subject: [PATCH 53/72] Added codegen for namespace handling, examples exercising namespaces --- wrap/Class.cpp | 50 ++++++++----- wrap/Class.h | 8 +-- wrap/Constructor.cpp | 27 +++---- wrap/Constructor.h | 7 +- wrap/Method.cpp | 32 +++++---- wrap/Method.h | 10 +-- wrap/Module.cpp | 71 +++++++++---------- wrap/StaticMethod.cpp | 8 +-- wrap/StaticMethod.h | 3 +- .../expected_namespaces/@ClassD/ClassD.m | 13 ++++ .../@ns1ClassA/ns1ClassA.m | 13 ++++ .../@ns1ClassB/ns1ClassB.m | 13 ++++ .../@ns2ClassA/memberFunction.cpp | 10 +++ .../@ns2ClassA/memberFunction.m | 4 ++ .../@ns2ClassA/ns2ClassA.m | 13 ++++ .../@ns2ClassC/ns2ClassC.m | 13 ++++ .../@ns2ns3ClassB/ns2ns3ClassB.m | 13 ++++ wrap/tests/expected_namespaces/Makefile | 60 ++++++++++++++++ .../expected_namespaces/make_testNamespaces.m | 50 +++++++++++++ .../tests/expected_namespaces/new_ClassD_.cpp | 9 +++ wrap/tests/expected_namespaces/new_ClassD_.m | 4 ++ .../expected_namespaces/new_ns1ClassA_.cpp | 9 +++ .../expected_namespaces/new_ns1ClassA_.m | 4 ++ .../expected_namespaces/new_ns1ClassB_.cpp | 9 +++ .../expected_namespaces/new_ns1ClassB_.m | 4 ++ .../expected_namespaces/new_ns2ClassA_.cpp | 9 +++ .../expected_namespaces/new_ns2ClassA_.m | 4 ++ .../expected_namespaces/new_ns2ClassC_.cpp | 9 +++ .../expected_namespaces/new_ns2ClassC_.m | 4 ++ .../expected_namespaces/new_ns2ns3ClassB_.cpp | 9 +++ .../expected_namespaces/new_ns2ns3ClassB_.m | 4 ++ .../ns2ClassA_afunction.cpp | 9 +++ .../expected_namespaces/ns2ClassA_afunction.m | 4 ++ wrap/tests/geometry.h | 9 --- wrap/tests/testNamespaces.h | 16 +++-- wrap/tests/testWrap.cpp | 45 ++++++++---- 36 files changed, 453 insertions(+), 126 deletions(-) create mode 100644 wrap/tests/expected_namespaces/@ClassD/ClassD.m create mode 100644 wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m create mode 100644 wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m create mode 100644 wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp create mode 100644 wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m create mode 100644 wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m create mode 100644 wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m create mode 100644 wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m create mode 100644 wrap/tests/expected_namespaces/Makefile create mode 100644 wrap/tests/expected_namespaces/make_testNamespaces.m create mode 100644 wrap/tests/expected_namespaces/new_ClassD_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ClassD_.m create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassA_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassA_.m create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassB_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassB_.m create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassA_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassA_.m create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassC_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassC_.m create mode 100644 wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m create mode 100644 wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp create mode 100644 wrap/tests/expected_namespaces/ns2ClassA_afunction.m diff --git a/wrap/Class.cpp b/wrap/Class.cpp index cf56f73a1..08ce392a0 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -33,16 +33,19 @@ void Class::matlab_proxy(const string& classFile) { if(!ofs) throw CantOpenFile(classFile); if(verbose_) cerr << "generating " << classFile << endl; + // get the name of actual matlab object + string matlabName = qualifiedName(); + // emit class proxy code - ofs << "classdef " << name << endl; + ofs << "classdef " << matlabName << endl; ofs << " properties" << endl; ofs << " self = 0" << endl; ofs << " end" << endl; ofs << " methods" << endl; - ofs << " function obj = " << name << "(varargin)" << endl; + ofs << " function obj = " << matlabName << "(varargin)" << endl; BOOST_FOREACH(Constructor c, constructors) - c.matlab_proxy_fragment(ofs,name); - ofs << " if nargin ~= 13 && obj.self == 0, error('" << name << " constructor failed'); end" << endl; + c.matlab_proxy_fragment(ofs,matlabName); + ofs << " if nargin ~= 13 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl; ofs << " end" << endl; ofs << " function display(obj), obj.print(''); end" << endl; ofs << " function disp(obj), obj.display; end" << endl; @@ -56,24 +59,26 @@ void Class::matlab_proxy(const string& classFile) { /* ************************************************************************* */ void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) { BOOST_FOREACH(Constructor c, constructors) { - c.matlab_mfile (toolboxPath, name); - c.matlab_wrapper(toolboxPath, name, nameSpace); + c.matlab_mfile (toolboxPath, qualifiedName()); + c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), nameSpace); } } /* ************************************************************************* */ void Class::matlab_methods(const string& classPath, const string& nameSpace) { + string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(Method m, methods) { m.matlab_mfile (classPath); - m.matlab_wrapper(classPath, name, nameSpace); + m.matlab_wrapper(classPath, name, cppName, matlabName, nameSpace); } } /* ************************************************************************* */ void Class::matlab_static_methods(const string& toolboxPath, const string& nameSpace) { + string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(StaticMethod& m, static_methods) { - m.matlab_mfile (toolboxPath, name); - m.matlab_wrapper(toolboxPath, name, nameSpace); + m.matlab_mfile (toolboxPath, qualifiedName()); + m.matlab_wrapper(toolboxPath, name, matlabName, cppName, nameSpace); } } @@ -83,13 +88,14 @@ void Class::matlab_make_fragment(ofstream& ofs, const string& mexFlags) { string mex = "mex " + mexFlags + " "; + string matlabClassName = qualifiedName(); BOOST_FOREACH(Constructor c, constructors) - ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl; + ofs << mex << c.matlab_wrapper_name(matlabClassName) << ".cpp" << endl; BOOST_FOREACH(StaticMethod sm, static_methods) - ofs << mex << name + "_" + sm.name << ".cpp" << endl; - ofs << endl << "cd @" << name << endl; + ofs << mex << matlabClassName + "_" + sm.name << ".cpp" << endl; + ofs << endl << "cd @" << matlabClassName << endl; BOOST_FOREACH(Method m, methods) - ofs << mex << m.name_ << ".cpp" << endl; + ofs << mex << m.name << ".cpp" << endl; ofs << endl; } @@ -108,18 +114,20 @@ void Class::makefile_fragment(ofstream& ofs) { // // Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) + string matlabName = qualifiedName(); + // collect names vector file_names; BOOST_FOREACH(Constructor c, constructors) { - string file_base = c.matlab_wrapper_name(name); + string file_base = c.matlab_wrapper_name(matlabName); file_names.push_back(file_base); } BOOST_FOREACH(StaticMethod c, static_methods) { - string file_base = name + "_" + c.name; + string file_base = matlabName + "_" + c.name; file_names.push_back(file_base); } BOOST_FOREACH(Method c, methods) { - string file_base = "@" + name + "/" + c.name_; + string file_base = "@" + matlabName + "/" + c.name; file_names.push_back(file_base); } @@ -129,7 +137,7 @@ void Class::makefile_fragment(ofstream& ofs) { } // class target - ofs << "\n" << name << ": "; + ofs << "\n" << matlabName << ": "; BOOST_FOREACH(const string& file_base, file_names) { ofs << file_base << ".$(MEXENDING) "; } @@ -137,3 +145,11 @@ void Class::makefile_fragment(ofstream& ofs) { } /* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; + return result + name; +} + +/* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index f02456f89..5938dc578 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include "Constructor.h" #include "Method.h" @@ -33,9 +32,9 @@ struct Class { // Then the instance variables are set directly by the Module constructor std::string name; ///< Class name - std::list constructors; ///< Class constructors - std::list methods; ///< Class methods - std::list static_methods; ///< Static methods + std::vector constructors; ///< Class constructors + std::vector methods; ///< Class methods + std::vector static_methods; ///< Static methods std::vector namespaces; ///< Stack of namespaces bool verbose_; ///< verbose flag @@ -51,6 +50,7 @@ struct Class { const std::string& toolboxPath, const std::string& mexFlags); ///< emit make fragment for global make script void makefile_fragment(std::ofstream& ofs); ///< emit makefile fragment + std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter }; } // \namespace wrap diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 346fc45da..c4458e198 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -45,22 +45,22 @@ void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) } /* ************************************************************************* */ -void Constructor::matlab_mfile(const string& toolboxPath, const string& className) { +void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) { - string name = matlab_wrapper_name(className); + string matlabName = matlab_wrapper_name(qualifiedMatlabName); // open destination m-file - string wrapperFile = toolboxPath + "/" + name + ".m"; + string wrapperFile = toolboxPath + "/" + matlabName + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code wrap::emit_header_comment(ofs, "%"); - ofs << "function result = " << name << "(obj"; + ofs << "function result = " << matlabName << "(obj"; if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; - ofs << " error('need to compile " << name << ".cpp');" << endl; + ofs << " error('need to compile " << matlabName << ".cpp');" << endl; ofs << "end" << endl; // close file @@ -68,15 +68,16 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& classNam } /* ************************************************************************* */ -void Constructor::matlab_wrapper(const string& toolboxPath, - const string& className, +void Constructor::matlab_wrapper(const string& toolboxPath, + const string& cppClassName, + const string& matlabClassName, const string& nameSpace) { - string name = matlab_wrapper_name(className); + string matlabName = matlab_wrapper_name(matlabClassName); // open destination wrapperFile - string wrapperFile = toolboxPath + "/" + name + ".cpp"; + string wrapperFile = toolboxPath + "/" + matlabName + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; @@ -84,14 +85,14 @@ void Constructor::matlab_wrapper(const string& toolboxPath, // generate code wrap::emit_header_comment(ofs, "//"); ofs << "#include " << endl; - ofs << "#include <" << className << ".h>" << endl; + ofs << "#include <" << name << ".h>" << endl; if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; ofs << "{" << endl; - ofs << " checkArguments(\"" << name << "\",nargout,nargin," << args.size() << ");" << endl; + ofs << " checkArguments(\"" << matlabName << "\",nargout,nargin," << args.size() << ");" << endl; args.matlab_unwrap(ofs); // unwrap arguments - ofs << " " << className << "* self = new " << className << "(" << args.names() << ");" << endl; - ofs << " out[0] = wrap_constructed(self,\"" << className << "\");" << endl; + ofs << " " << cppClassName << "* self = new " << cppClassName << "(" << args.names() << ");" << endl; // need qualified name, delim: "::" + ofs << " out[0] = wrap_constructed(self,\"" << matlabClassName << "\");" << endl; // need matlab qualified name ofs << "}" << endl; // close file diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 93f9b145b..7ad8cd9d5 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -34,6 +34,7 @@ struct Constructor { // Then the instance variables are set directly by the Module constructor ArgumentList args; + std::string name; bool verbose_; // MATLAB code generation @@ -48,11 +49,13 @@ struct Constructor { /// m-file void matlab_mfile(const std::string& toolboxPath, - const std::string& className); + const std::string& qualifiedMatlabName); /// wrapper void matlab_wrapper(const std::string& toolboxPath, - const std::string& className, const std::string& nameSpace); + const std::string& cppClassName, + const std::string& matlabClassName, + const std::string& nameSpace); }; } // \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index bf0426c3e..8dbc3cb4f 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -29,18 +29,18 @@ using namespace wrap; void Method::matlab_mfile(const string& classPath) { // open destination m-file - string wrapperFile = classPath + "/" + name_ + ".m"; + string wrapperFile = classPath + "/" + name + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - string returnType = returnVal_.matlab_returnType(); - ofs << "function " << returnType << " = " << name_ << "(obj"; - if (args_.size()) ofs << "," << args_.names(); + string returnType = returnVal.matlab_returnType(); + ofs << "function " << returnType << " = " << name << "(obj"; + if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; - ofs << "% usage: obj." << name_ << "(" << args_.names() << ")" << endl; - ofs << " error('need to compile " << name_ << ".cpp');" << endl; + ofs << "% usage: obj." << name << "(" << args.names() << ")" << endl; + ofs << " error('need to compile " << name << ".cpp');" << endl; ofs << "end" << endl; // close file @@ -50,10 +50,12 @@ void Method::matlab_mfile(const string& classPath) { /* ************************************************************************* */ void Method::matlab_wrapper(const string& classPath, const string& className, + const string& cppClassName, + const string& matlabClassName, const string& nameSpace) { // open destination wrapperFile - string wrapperFile = classPath + "/" + name_ + ".cpp"; + string wrapperFile = classPath + "/" + name + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; @@ -74,26 +76,26 @@ void Method::matlab_wrapper(const string& classPath, // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - ofs << " checkArguments(\"" << name_ << "\",nargout,nargin-1," << args_.size() << ");\n"; + ofs << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className - << " >(in[0],\"" << className << "\");" << endl; + ofs << " shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName + << " >(in[0],\"" << matlabClassName << "\");" << endl; // unwrap arguments, see Argument.cpp - args_.matlab_unwrap(ofs,1); + args.matlab_unwrap(ofs,1); // call method // example: bool result = self->return_field(t); ofs << " "; - if (returnVal_.type1!="void") - ofs << returnVal_.return_type(true,ReturnValue::pair) << " result = "; - ofs << "self->" << name_ << "(" << args_.names() << ");\n"; + if (returnVal.type1!="void") + ofs << returnVal.return_type(true,ReturnValue::pair) << " result = "; + ofs << "self->" << name << "(" << args.names() << ");\n"; // wrap result // example: out[0]=wrap(result); - returnVal_.wrap_result(ofs); + returnVal.wrap_result(ofs); // finish ofs << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index 843a2053b..c3a9f1e00 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -35,16 +35,18 @@ struct Method { // Then the instance variables are set directly by the Module constructor bool verbose_; bool is_const_; - std::string name_; - ArgumentList args_; - ReturnValue returnVal_; + std::string name; + ArgumentList args; + ReturnValue returnVal; // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void matlab_mfile(const std::string& classPath); ///< m-file void matlab_wrapper(const std::string& classPath, - const std::string& className, const std::string& nameSpace); ///< wrapper + const std::string& className, + const std::string& cppClassName, + const std::string& matlabClassname,const std::string& nameSpace); ///< wrapper }; } // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index f1b6c4acb..998f1992d 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -104,6 +104,7 @@ Module::Module(const string& interfacePath, Rule constructor_p = (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) [assign_a(constructor.args,args)] + [assign_a(constructor.name,cls.name)] [assign_a(args,args0)] [push_back_a(cls.constructors, constructor)] [assign_a(constructor,constructor0)]; @@ -131,12 +132,12 @@ Module::Module(const string& interfacePath, Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; Rule method_p = - (returnType_p >> methodName_p[assign_a(method.name_)] >> + (returnType_p >> methodName_p[assign_a(method.name)] >> '(' >> argumentList_p >> ')' >> !str_p("const")[assign_a(method.is_const_,true)] >> ';' >> *comments_p) - [assign_a(method.args_,args)] + [assign_a(method.args,args)] [assign_a(args,args0)] - [assign_a(method.returnVal_,retVal)] + [assign_a(method.returnVal,retVal)] [assign_a(retVal,retVal0)] [push_back_a(cls.methods, method)] [assign_a(method,method0)]; @@ -165,7 +166,7 @@ Module::Module(const string& interfacePath, namespace_name_p[push_back_a(namespaces)] >> ch_p('{') >> *(class_p | namespace_p | comments_p) >> - str_p("}///\\namespace") // end namespace, avoid confusion with classes + str_p("}///\\namespace") >> !namespace_name_p // end namespace, avoid confusion with classes [pop_a(namespaces)]; Rule module_content_p = comments_p | class_p | namespace_p ; @@ -207,6 +208,18 @@ Module::Module(const string& interfacePath, } } +template +void verifyArguments(const vector& validArgs, const vector& vt) { + BOOST_FOREACH(const T& t, vt) { + BOOST_FOREACH(Argument arg, t.args) { + if(std::find(validArgs.begin(), validArgs.end(), arg.type) + == validArgs.end()) + throw DependencyMissing(arg.type, t.name); + } + } +} + + /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, const string& nameSpace, @@ -254,60 +267,40 @@ void Module::matlab_code(const string& toolboxPath, // add 'all' to Makefile make_ofs << "all: "; BOOST_FOREACH(Class cls, classes) { - make_ofs << cls.name << " "; - //Create a list of parsed classes for dependency checking - validArgs.push_back(cls.name); + make_ofs << cls.qualifiedName() << " "; + //Create a list of parsed classes for dependency checking + validArgs.push_back(cls.name); } make_ofs << "\n\n"; - - // generate proxy classes and wrappers BOOST_FOREACH(Class cls, classes) { // create directory if needed - string classPath = toolboxPath + "/@" + cls.name; + string classPath = toolboxPath + "/@" + cls.qualifiedName(); string installCmd = "install -d " + classPath; system(installCmd.c_str()); // create proxy class - string classFile = classPath + "/" + cls.name + ".m"; + string classFile = classPath + "/" + cls.qualifiedName() + ".m"; cls.matlab_proxy(classFile); - // create constructor and method wrappers - BOOST_FOREACH(Constructor con, cls.constructors) { - BOOST_FOREACH(Argument arg, con.args) { - if(std::find(validArgs.begin(), validArgs.end(), arg.type) - == validArgs.end()) - throw DependencyMissing(arg.type, cls.name); - } - } - cls.matlab_constructors(toolboxPath,nameSpace); - - BOOST_FOREACH(StaticMethod stMth, cls.static_methods) { - BOOST_FOREACH(Argument arg, stMth.args) { - if(std::find(validArgs.begin(), validArgs.end(), arg.type) - == validArgs.end()) - throw DependencyMissing(arg.type, stMth.name); - } - } - cls.matlab_static_methods(toolboxPath,nameSpace); + // verify all of the function arguments + verifyArguments(validArgs, cls.constructors); + verifyArguments(validArgs, cls.static_methods); + verifyArguments(validArgs, cls.methods); - BOOST_FOREACH(Method mth, cls.methods) { - BOOST_FOREACH(Argument arg, mth.args_) { - if(std::find(validArgs.begin(), validArgs.end(), arg.type) - == validArgs.end()) - throw DependencyMissing(arg.type, mth.name_); - } - } + // create constructor and method wrappers + cls.matlab_constructors(toolboxPath,nameSpace); + cls.matlab_static_methods(toolboxPath,nameSpace); cls.matlab_methods(classPath,nameSpace); // add lines to make m-file - ofs << "%% " << cls.name << endl; + ofs << "%% " << cls.qualifiedName() << endl; ofs << "cd(toolboxpath)" << endl; cls.matlab_make_fragment(ofs, toolboxPath, mexFlags); // add section to the (actual) make file - make_ofs << "# " << cls.name << endl; + make_ofs << "# " << cls.qualifiedName() << endl; cls.makefile_fragment(make_ofs); } @@ -320,7 +313,7 @@ void Module::matlab_code(const string& toolboxPath, make_ofs << "\n\nclean: \n"; make_ofs << "\trm -rf *.$(MEXENDING)\n"; BOOST_FOREACH(Class cls, classes) - make_ofs << "\trm -rf @" << cls.name << "/*.$(MEXENDING)\n"; + make_ofs << "\trm -rf @" << cls.qualifiedName() << "/*.$(MEXENDING)\n"; // finish Makefile make_ofs << "\n" << endl; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 763cb38f4..352c9c680 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -49,11 +49,11 @@ void StaticMethod::matlab_mfile(const string& toolboxPath, const string& classNa } /* ************************************************************************* */ -void StaticMethod::matlab_wrapper(const string& toolboxPath, - const string& className, const string& nameSpace) +void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, + const string& matlabClassName, const string& cppClassName, const string& nameSpace) { // open destination wrapperFile - string full_name = className + "_" + name; + string full_name = matlabClassName + "_" + name; string wrapperFile = toolboxPath + "/" + full_name + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); @@ -84,7 +84,7 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, // call method with default type if (returnVal.type1!="void") ofs << returnVal.return_type(true,ReturnValue::pair) << " result = "; - ofs << className << "::" << name << "(" << args.names() << ");\n"; + ofs << cppClassName << "::" << name << "(" << args.names() << ");\n"; // wrap result // example: out[0]=wrap(result); diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index c58b26fe8..c50959d02 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -46,7 +46,8 @@ struct StaticMethod { void matlab_mfile(const std::string& toolboxPath, const std::string& className); ///< m-file void matlab_wrapper(const std::string& toolboxPath, - const std::string& className, const std::string& nameSpace); ///< wrapper + const std::string& className, const std::string& matlabClassName, + const std::string& cppClassName, const std::string& nameSpace); ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/tests/expected_namespaces/@ClassD/ClassD.m b/wrap/tests/expected_namespaces/@ClassD/ClassD.m new file mode 100644 index 000000000..d6b5b452a --- /dev/null +++ b/wrap/tests/expected_namespaces/@ClassD/ClassD.m @@ -0,0 +1,13 @@ +classdef ClassD + properties + self = 0 + end + methods + function obj = ClassD(varargin) + if nargin == 0, obj.self = new_ClassD_(); end + if nargin ~= 13 && obj.self == 0, error('ClassD constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m new file mode 100644 index 000000000..f5de15c80 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m @@ -0,0 +1,13 @@ +classdef ns1ClassA + properties + self = 0 + end + methods + function obj = ns1ClassA(varargin) + if nargin == 0, obj.self = new_ns1ClassA_(); end + if nargin ~= 13 && obj.self == 0, error('ns1ClassA constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m new file mode 100644 index 000000000..189dab25b --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m @@ -0,0 +1,13 @@ +classdef ns1ClassB + properties + self = 0 + end + methods + function obj = ns1ClassB(varargin) + if nargin == 0, obj.self = new_ns1ClassB_(); end + if nargin ~= 13 && obj.self == 0, error('ns1ClassB constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp new file mode 100644 index 000000000..d051c74ec --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("memberFunction",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); + double result = self->memberFunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m new file mode 100644 index 000000000..8e8d24bd7 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m @@ -0,0 +1,4 @@ +function result = memberFunction(obj) +% usage: obj.memberFunction() + error('need to compile memberFunction.cpp'); +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m new file mode 100644 index 000000000..f0521377e --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m @@ -0,0 +1,13 @@ +classdef ns2ClassA + properties + self = 0 + end + methods + function obj = ns2ClassA(varargin) + if nargin == 0, obj.self = new_ns2ClassA_(); end + if nargin ~= 13 && obj.self == 0, error('ns2ClassA constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m new file mode 100644 index 000000000..78fc02073 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m @@ -0,0 +1,13 @@ +classdef ns2ClassC + properties + self = 0 + end + methods + function obj = ns2ClassC(varargin) + if nargin == 0, obj.self = new_ns2ClassC_(); end + if nargin ~= 13 && obj.self == 0, error('ns2ClassC constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m new file mode 100644 index 000000000..35891c5a7 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m @@ -0,0 +1,13 @@ +classdef ns2ns3ClassB + properties + self = 0 + end + methods + function obj = ns2ns3ClassB(varargin) + if nargin == 0, obj.self = new_ns2ns3ClassB_(); end + if nargin ~= 13 && obj.self == 0, error('ns2ns3ClassB constructor failed'); end + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile new file mode 100644 index 000000000..844bf9692 --- /dev/null +++ b/wrap/tests/expected_namespaces/Makefile @@ -0,0 +1,60 @@ +# automatically generated by wrap on 2011-Dec-08 + +MEX = mex +MEXENDING = mexa64 +mex_flags = -O5 + +all: ns1ClassA ns1ClassB ns2ClassA ns2ns3ClassB ns2ClassC ClassD + +# ns1ClassA +new_ns1ClassA_.$(MEXENDING): new_ns1ClassA_.cpp + $(MEX) $(mex_flags) new_ns1ClassA_.cpp -output new_ns1ClassA_ + +ns1ClassA: new_ns1ClassA_.$(MEXENDING) + +# ns1ClassB +new_ns1ClassB_.$(MEXENDING): new_ns1ClassB_.cpp + $(MEX) $(mex_flags) new_ns1ClassB_.cpp -output new_ns1ClassB_ + +ns1ClassB: new_ns1ClassB_.$(MEXENDING) + +# ns2ClassA +new_ns2ClassA_.$(MEXENDING): new_ns2ClassA_.cpp + $(MEX) $(mex_flags) new_ns2ClassA_.cpp -output new_ns2ClassA_ +ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp + $(MEX) $(mex_flags) ns2ClassA_afunction.cpp -output ns2ClassA_afunction +@ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp + $(MEX) $(mex_flags) @ns2ClassA/memberFunction.cpp -output @ns2ClassA/memberFunction + +ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) + +# ns2ns3ClassB +new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp + $(MEX) $(mex_flags) new_ns2ns3ClassB_.cpp -output new_ns2ns3ClassB_ + +ns2ns3ClassB: new_ns2ns3ClassB_.$(MEXENDING) + +# ns2ClassC +new_ns2ClassC_.$(MEXENDING): new_ns2ClassC_.cpp + $(MEX) $(mex_flags) new_ns2ClassC_.cpp -output new_ns2ClassC_ + +ns2ClassC: new_ns2ClassC_.$(MEXENDING) + +# ClassD +new_ClassD_.$(MEXENDING): new_ClassD_.cpp + $(MEX) $(mex_flags) new_ClassD_.cpp -output new_ClassD_ + +ClassD: new_ClassD_.$(MEXENDING) + + + +clean: + rm -rf *.$(MEXENDING) + rm -rf @ns1ClassA/*.$(MEXENDING) + rm -rf @ns1ClassB/*.$(MEXENDING) + rm -rf @ns2ClassA/*.$(MEXENDING) + rm -rf @ns2ns3ClassB/*.$(MEXENDING) + rm -rf @ns2ClassC/*.$(MEXENDING) + rm -rf @ClassD/*.$(MEXENDING) + + diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m new file mode 100644 index 000000000..76cb43fbb --- /dev/null +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -0,0 +1,50 @@ +% automatically generated by wrap on 2011-Dec-08 +echo on + +toolboxpath = mfilename('fullpath'); +delims = find(toolboxpath == '/'); +toolboxpath = toolboxpath(1:(delims(end)-1)); +clear delims +addpath(toolboxpath); + +%% ns1ClassA +cd(toolboxpath) +mex -O5 new_ns1ClassA_.cpp + +cd @ns1ClassA + +%% ns1ClassB +cd(toolboxpath) +mex -O5 new_ns1ClassB_.cpp + +cd @ns1ClassB + +%% ns2ClassA +cd(toolboxpath) +mex -O5 new_ns2ClassA_.cpp +mex -O5 ns2ClassA_afunction.cpp + +cd @ns2ClassA +mex -O5 memberFunction.cpp + +%% ns2ns3ClassB +cd(toolboxpath) +mex -O5 new_ns2ns3ClassB_.cpp + +cd @ns2ns3ClassB + +%% ns2ClassC +cd(toolboxpath) +mex -O5 new_ns2ClassC_.cpp + +cd @ns2ClassC + +%% ClassD +cd(toolboxpath) +mex -O5 new_ClassD_.cpp + +cd @ClassD + +cd(toolboxpath) + +echo off diff --git a/wrap/tests/expected_namespaces/new_ClassD_.cpp b/wrap/tests/expected_namespaces/new_ClassD_.cpp new file mode 100644 index 000000000..ec7212786 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ClassD_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ClassD_",nargout,nargin,0); + ClassD* self = new ClassD(); + out[0] = wrap_constructed(self,"ClassD"); +} diff --git a/wrap/tests/expected_namespaces/new_ClassD_.m b/wrap/tests/expected_namespaces/new_ClassD_.m new file mode 100644 index 000000000..c5f53f130 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ClassD_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ClassD_(obj) + error('need to compile new_ClassD_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp new file mode 100644 index 000000000..2db8ef767 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns1ClassA_",nargout,nargin,0); + ns1::ClassA* self = new ns1::ClassA(); + out[0] = wrap_constructed(self,"ns1ClassA"); +} diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.m b/wrap/tests/expected_namespaces/new_ns1ClassA_.m new file mode 100644 index 000000000..89cd8b0a2 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns1ClassA_(obj) + error('need to compile new_ns1ClassA_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp new file mode 100644 index 000000000..b4ac7038a --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns1ClassB_",nargout,nargin,0); + ns1::ClassB* self = new ns1::ClassB(); + out[0] = wrap_constructed(self,"ns1ClassB"); +} diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.m b/wrap/tests/expected_namespaces/new_ns1ClassB_.m new file mode 100644 index 000000000..5430f85aa --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns1ClassB_(obj) + error('need to compile new_ns1ClassB_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp new file mode 100644 index 000000000..cc4ec309b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns2ClassA_",nargout,nargin,0); + ns2::ClassA* self = new ns2::ClassA(); + out[0] = wrap_constructed(self,"ns2ClassA"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.m b/wrap/tests/expected_namespaces/new_ns2ClassA_.m new file mode 100644 index 000000000..bb8b2a24a --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ClassA_(obj) + error('need to compile new_ns2ClassA_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp new file mode 100644 index 000000000..b43a7cd6b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns2ClassC_",nargout,nargin,0); + ns2::ClassC* self = new ns2::ClassC(); + out[0] = wrap_constructed(self,"ns2ClassC"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.m b/wrap/tests/expected_namespaces/new_ns2ClassC_.m new file mode 100644 index 000000000..91e643c4b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ClassC_(obj) + error('need to compile new_ns2ClassC_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp new file mode 100644 index 000000000..3916ed3ff --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_ns2ns3ClassB_",nargout,nargin,0); + ns2::ns3::ClassB* self = new ns2::ns3::ClassB(); + out[0] = wrap_constructed(self,"ns2ns3ClassB"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m new file mode 100644 index 000000000..54b38a16c --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ns3ClassB_(obj) + error('need to compile new_ns2ns3ClassB_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp new file mode 100644 index 000000000..dff68090a --- /dev/null +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("ns2ClassA_afunction",nargout,nargin,0); + double result = ns2::ClassA::afunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.m b/wrap/tests/expected_namespaces/ns2ClassA_afunction.m new file mode 100644 index 000000000..0482d1548 --- /dev/null +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.m @@ -0,0 +1,4 @@ +function result = ns2ClassA_afunction() +% usage: x = ns2ClassA_afunction() + error('need to compile ns2ClassA_afunction.cpp'); +end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index b8427faa3..2eac86bfe 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -9,10 +9,6 @@ class Point2 { VectorNotEigen vectorConfusion(); }; -namespace ns_outer { - -namespace ns_inner { - class Point3 { Point3(double x, double y, double z); double norm() const; @@ -24,9 +20,6 @@ class Point3 { // another comment -// NOTE: you *must* end namespaces as follows: -}///\namespace - // another comment /** @@ -76,8 +69,6 @@ class Test { // even more comments at the end! }; -}///\namespace - // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index f95e08a74..5b46f99ac 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -5,33 +5,39 @@ namespace ns1 { class ClassA { - + ClassA(); }; class ClassB { - + ClassB(); }; -}///\namespace +}///\namespace ns1 namespace ns2 { class ClassA { + ClassA(); + static double afunction(); + double memberFunction(); }; namespace ns3 { class ClassB { + ClassB(); }; -}///\namespace +}///\namespace ns3 class ClassC { + ClassC(); }; -}///\namespace +}///\namespace ns2 class ClassD { + ClassD(); }; diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 7b60e9ad2..f9151dba2 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -50,6 +50,10 @@ TEST( wrap, check_exception ) { THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); + // clean out previous generated code + string cleanCmd = "rm -rf actual"; + system(cleanCmd.c_str()); + string path = topdir + "/wrap/tests"; Module module(path.c_str(), "testWrap1",enable_verbose); CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); @@ -78,9 +82,7 @@ TEST( wrap, parse ) { EXPECT_LONGS_EQUAL(1, cls.constructors.size()); EXPECT_LONGS_EQUAL(1, cls.methods.size()); EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); - EXPECT_LONGS_EQUAL(2, cls.namespaces.size()); - EXPECT(assert_equal("ns_outer", cls.namespaces.front())); - EXPECT(assert_equal("ns_inner", cls.namespaces.back())); + EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); // first constructor takes 3 doubles Constructor c1 = cls.constructors.front(); @@ -95,9 +97,9 @@ TEST( wrap, parse ) { // check method Method m1 = cls.methods.front(); - EXPECT(assert_equal("double", m1.returnVal_.type1)); - EXPECT(assert_equal("norm", m1.name_)); - EXPECT_LONGS_EQUAL(0, m1.args_.size()); + EXPECT(assert_equal("double", m1.returnVal.type1)); + EXPECT(assert_equal("norm", m1.name)); + EXPECT_LONGS_EQUAL(0, m1.args.size()); EXPECT(m1.is_const_); } @@ -108,14 +110,13 @@ TEST( wrap, parse ) { EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); EXPECT_LONGS_EQUAL(19, testCls.methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); - EXPECT_LONGS_EQUAL( 1, testCls.namespaces.size()); - EXPECT(assert_equal("ns_outer", testCls.namespaces.front())); + EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); // function to parse: pair return_pair (Vector v, Matrix A) const; Method m2 = testCls.methods.front(); - EXPECT(m2.returnVal_.isPair); - EXPECT(m2.returnVal_.category1 == ReturnValue::EIGEN); - EXPECT(m2.returnVal_.category2 == ReturnValue::EIGEN); + EXPECT(m2.returnVal.isPair); + EXPECT(m2.returnVal.category1 == ReturnValue::EIGEN); + EXPECT(m2.returnVal.category2 == ReturnValue::EIGEN); } } @@ -154,8 +155,26 @@ TEST( wrap, parse_namespaces ) { Class cls6 = module.classes.at(5); EXPECT(assert_equal("ClassD", cls6.name)); EXPECT_LONGS_EQUAL(0, cls6.namespaces.size()); - if (!cls6.namespaces.empty()) - cout << "Extraneous namespace: " << cls6.namespaces.front() << endl; +} + +/* ************************************************************************* */ +TEST( wrap, matlab_code_namespaces ) { + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "testNamespaces",enable_verbose); + EXPECT_LONGS_EQUAL(6, module.classes.size()); + string path = topdir + "/wrap"; + + // clean out previous generated code + string cleanCmd = "rm -rf actual_namespaces"; + system(cleanCmd.c_str()); + + // emit MATLAB code + string exp_path = path + "/tests/expected_namespaces/"; + string act_path = "actual_namespaces/"; + module.matlab_code("actual_namespaces", "", "mexa64", "-O5"); + + EXPECT(files_equal(exp_path + "make_testNamespaces.m", act_path + "make_testNamespaces.m")); + EXPECT(files_equal(exp_path + "Makefile" , act_path + "Makefile" )); } /* ************************************************************************* */ From 66711e1faab230f363a4eb366d8df8699874cff7 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:51:15 +0000 Subject: [PATCH 54/72] Added namespace support to arguments --- wrap/Argument.cpp | 20 ++++++++++---- wrap/Argument.h | 9 ++++--- wrap/Module.cpp | 26 ++++++++++++------- wrap/tests/expected_namespaces/Makefile | 4 ++- .../expected_namespaces/make_testNamespaces.m | 1 + wrap/tests/testNamespaces.h | 1 + 6 files changed, 42 insertions(+), 19 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index c196212ef..e01da3db3 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -30,25 +30,35 @@ void Argument::matlab_unwrap(ofstream& ofs, { ofs << " "; + string cppType = qualifiedType("::"); + string matlabType = qualifiedType(); + if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - ofs << "shared_ptr<" << type << "> " << name << " = unwrap_shared_ptr< "; + ofs << "shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer - ofs << type << "& " << name << " = *unwrap_shared_ptr< "; + ofs << cppType << "& " << name << " = *unwrap_shared_ptr< "; else // Not a pointer or a reference: emit an "unwrap" call // unwrap is specified in matlab.h as a series of template specializations // that know how to unpack the expected MATLAB object // example: double tol = unwrap< double >(in[2]); // example: Vector v = unwrap< Vector >(in[1]); - ofs << type << " " << name << " = unwrap< "; + ofs << cppType << " " << name << " = unwrap< "; - ofs << type << " >(" << matlabName; - if (is_ptr || is_ref) ofs << ", \"" << type << "\""; + ofs << cppType << " >(" << matlabName; + if (is_ptr || is_ref) ofs << ", \"" << matlabType << "\""; ofs << ");" << endl; } +/* ************************************************************************* */ +string Argument::qualifiedType(const string& delim) { + string result; + BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + return result + type; +} + /* ************************************************************************* */ string ArgumentList::types() { string str; diff --git a/wrap/Argument.h b/wrap/Argument.h index 3ebfa505e..07c11a5cc 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace wrap { @@ -27,17 +27,20 @@ struct Argument { bool is_const, is_ref, is_ptr; std::string type; std::string name; + std::vector namespaces; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + std::string qualifiedType(const std::string& delim = ""); // adds namespaces to type + /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName); }; /// Argument list -struct ArgumentList: public std::list { - std::list args; +struct ArgumentList: public std::vector { + std::vector args; // why does it contain this? std::string types(); std::string signature(); std::string names(); diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 998f1992d..fdd550f5a 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -74,12 +74,18 @@ Module::Module(const string& interfacePath, Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p; + Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + + Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::"); + Rule classPtr_p = + *namespace_arg_p >> className_p [assign_a(arg.type)] >> ch_p('*') [assign_a(arg.is_ptr,true)]; Rule classRef_p = !str_p("const") [assign_a(arg.is_const,true)] >> + *namespace_arg_p >> className_p [assign_a(arg.type)] >> ch_p('&') [assign_a(arg.is_ref,true)]; @@ -95,7 +101,8 @@ Module::Module(const string& interfacePath, Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; Rule argument_p = - ((basisType_p[assign_a(arg.type)] | argEigenType_p | classRef_p | eigenRef_p | classPtr_p) >> name_p[assign_a(arg.name)]) + ((basisType_p[assign_a(arg.type)] | argEigenType_p | classRef_p | eigenRef_p | classPtr_p) + >> name_p[assign_a(arg.name)]) [push_back_a(args, arg)] [assign_a(arg,arg0)]; @@ -160,16 +167,14 @@ Module::Module(const string& interfacePath, *(functions_p | comments_p) >> str_p("};"))[assign_a(cls.namespaces, namespaces)][push_back_a(classes,cls)][assign_a(cls,cls0)]; - Rule namespace_name_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - Rule namespace_p = str_p("namespace") >> + Rule namespace_def_p = str_p("namespace") >> namespace_name_p[push_back_a(namespaces)] >> ch_p('{') >> - *(class_p | namespace_p | comments_p) >> + *(class_p | namespace_def_p | comments_p) >> str_p("}///\\namespace") >> !namespace_name_p // end namespace, avoid confusion with classes [pop_a(namespaces)]; - Rule module_content_p = comments_p | class_p | namespace_p ; + Rule module_content_p = comments_p | class_p | namespace_def_p ; Rule module_p = *module_content_p >> !end_p; @@ -208,18 +213,19 @@ Module::Module(const string& interfacePath, } } +/* ************************************************************************* */ template void verifyArguments(const vector& validArgs, const vector& vt) { BOOST_FOREACH(const T& t, vt) { BOOST_FOREACH(Argument arg, t.args) { - if(std::find(validArgs.begin(), validArgs.end(), arg.type) + string fullType = arg.qualifiedType("::"); + if(std::find(validArgs.begin(), validArgs.end(), fullType) == validArgs.end()) - throw DependencyMissing(arg.type, t.name); + throw DependencyMissing(fullType, t.name); } } } - /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, const string& nameSpace, @@ -269,7 +275,7 @@ void Module::matlab_code(const string& toolboxPath, BOOST_FOREACH(Class cls, classes) { make_ofs << cls.qualifiedName() << " "; //Create a list of parsed classes for dependency checking - validArgs.push_back(cls.name); + validArgs.push_back(cls.qualifiedName("::")); } make_ofs << "\n\n"; diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile index 844bf9692..d15278938 100644 --- a/wrap/tests/expected_namespaces/Makefile +++ b/wrap/tests/expected_namespaces/Makefile @@ -25,8 +25,10 @@ ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp $(MEX) $(mex_flags) ns2ClassA_afunction.cpp -output ns2ClassA_afunction @ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp $(MEX) $(mex_flags) @ns2ClassA/memberFunction.cpp -output @ns2ClassA/memberFunction +@ns2ClassA/nsArg.$(MEXENDING): @ns2ClassA/nsArg.cpp + $(MEX) $(mex_flags) @ns2ClassA/nsArg.cpp -output @ns2ClassA/nsArg -ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) +ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) # ns2ns3ClassB new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m index 76cb43fbb..95b4633de 100644 --- a/wrap/tests/expected_namespaces/make_testNamespaces.m +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -26,6 +26,7 @@ mex -O5 ns2ClassA_afunction.cpp cd @ns2ClassA mex -O5 memberFunction.cpp +mex -O5 nsArg.cpp %% ns2ns3ClassB cd(toolboxpath) diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 5b46f99ac..f6b4efa95 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -20,6 +20,7 @@ class ClassA { ClassA(); static double afunction(); double memberFunction(); + int nsArg(const ns1::ClassB& arg); }; namespace ns3 { From ead8247bd777e4331748636104ab08d48b994c02 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:51:17 +0000 Subject: [PATCH 55/72] Added namespace support to return classes --- wrap/Module.cpp | 18 ++++++--- wrap/ReturnValue.cpp | 38 +++++++++++++++---- wrap/ReturnValue.h | 7 +++- wrap/tests/expected_namespaces/Makefile | 4 +- .../expected_namespaces/make_testNamespaces.m | 1 + wrap/tests/testNamespaces.h | 1 + 6 files changed, 55 insertions(+), 14 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index fdd550f5a..24257c96b 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -21,6 +21,7 @@ //#define BOOST_SPIRIT_DEBUG #include #include +#include #include #include @@ -52,7 +53,7 @@ Module::Module(const string& interfacePath, Method method0(enable_verbose), method(enable_verbose); StaticMethod static_method0(enable_verbose), static_method(enable_verbose); Class cls0(enable_verbose),cls(enable_verbose); - vector namespaces, namespaces_parent, namespaces_temp; + vector namespaces, namespaces_return; //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -69,12 +70,15 @@ Module::Module(const string& interfacePath, Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double"); + Rule keywords_p = + (str_p("const") | "static" | "namespace" | basisType_p); + Rule eigenType_p = (str_p("Vector") | "Matrix"); - Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p; + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p; - Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::"); @@ -116,15 +120,19 @@ Module::Module(const string& interfacePath, [push_back_a(cls.constructors, constructor)] [assign_a(constructor,constructor0)]; + Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); + Rule returnType1_p = (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::BASIS)]) | - ((className_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::CLASS)]) >> + ((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::CLASS)]) >> !ch_p('*')[assign_a(retVal.isPtr1,true)]) | (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::EIGEN)]); Rule returnType2_p = (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::BASIS)]) | - ((className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >> + ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >> !ch_p('*') [assign_a(retVal.isPtr2,true)]) | (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::EIGEN)]); diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 9d8606c5b..c10c173d7 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -5,6 +5,8 @@ * @author Alex Cunningham */ +#include + #include "ReturnValue.h" #include "utilities.h" @@ -15,21 +17,43 @@ using namespace wrap; string ReturnValue::return_type(bool add_ptr, pairing p) { if (p==pair && isPair) { string str = "pair< " + - wrap::maybe_shared_ptr(add_ptr && isPtr1, type1) + ", " + - wrap::maybe_shared_ptr(add_ptr && isPtr2, type2) + " >"; + maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::")) + ", " + + maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::")) + " >"; return str; } else - return wrap::maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? type2 : type1); + return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::")); +} + +/* ************************************************************************* */ +string ReturnValue::matlab_returnType() const { + return isPair? "[first,second]" : "result"; +} + +/* ************************************************************************* */ +string ReturnValue::qualifiedType1(const string& delim) { + string result; + BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim; + return result + type1; +} + +/* ************************************************************************* */ +string ReturnValue::qualifiedType2(const string& delim) { + string result; + BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim; + return result + type2; } /* ************************************************************************* */ void ReturnValue::wrap_result(std::ostream& ofs) { + string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(); + string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(); + if (isPair) { // first return value in pair if (isPtr1) // if we already have a pointer - ofs << " out[0] = wrap_shared_ptr(result.first,\"" << type1 << "\");\n"; + ofs << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n"; else if (category1 == ReturnValue::CLASS) // if we are going to make one - ofs << " out[0] = wrap_shared_ptr(make_shared< " << type1 << " >(result.first),\"" << type1 << "\");\n"; + ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n"; else // if basis type ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; @@ -37,14 +61,14 @@ void ReturnValue::wrap_result(std::ostream& ofs) { if (isPtr2) // if we already have a pointer ofs << " out[1] = wrap_shared_ptr(result.second,\"" << type2 << "\");\n"; else if (category2 == ReturnValue::CLASS) // if we are going to make one - ofs << " out[1] = wrap_shared_ptr(make_shared< " << type2 << " >(result.second),\"" << type2 << "\");\n"; + ofs << " out[1] = wrap_shared_ptr(make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n"; else ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; } else if (isPtr1) ofs << " out[0] = wrap_shared_ptr(result,\"" << type1 << "\");\n"; else if (category1 == ReturnValue::CLASS) - ofs << " out[0] = wrap_shared_ptr(make_shared< " << type1 << " >(result),\"" << type1 << "\");\n"; + ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n"; else if (type1!="void") ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index c6d7864d3..ba606ad9c 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -7,6 +7,7 @@ * @author Alex Cunningham */ +#include #include #pragma once @@ -30,6 +31,7 @@ struct ReturnValue { bool verbose; std::string type1, type2; bool isPtr1, isPtr2, isPair; + std::vector namespaces1, namespaces2; return_category category1, category2; @@ -39,7 +41,10 @@ struct ReturnValue { std::string return_type(bool add_ptr, pairing p); - std::string matlab_returnType() const { return isPair? "[first,second]" : "result"; } + std::string qualifiedType1(const std::string& delim = ""); + std::string qualifiedType2(const std::string& delim = ""); + + std::string matlab_returnType() const; void wrap_result(std::ostream& ofs); diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile index d15278938..4fe9b0655 100644 --- a/wrap/tests/expected_namespaces/Makefile +++ b/wrap/tests/expected_namespaces/Makefile @@ -27,8 +27,10 @@ ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp $(MEX) $(mex_flags) @ns2ClassA/memberFunction.cpp -output @ns2ClassA/memberFunction @ns2ClassA/nsArg.$(MEXENDING): @ns2ClassA/nsArg.cpp $(MEX) $(mex_flags) @ns2ClassA/nsArg.cpp -output @ns2ClassA/nsArg +@ns2ClassA/nsReturn.$(MEXENDING): @ns2ClassA/nsReturn.cpp + $(MEX) $(mex_flags) @ns2ClassA/nsReturn.cpp -output @ns2ClassA/nsReturn -ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) +ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) @ns2ClassA/nsReturn.$(MEXENDING) # ns2ns3ClassB new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m index 95b4633de..59d850c42 100644 --- a/wrap/tests/expected_namespaces/make_testNamespaces.m +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -27,6 +27,7 @@ mex -O5 ns2ClassA_afunction.cpp cd @ns2ClassA mex -O5 memberFunction.cpp mex -O5 nsArg.cpp +mex -O5 nsReturn.cpp %% ns2ns3ClassB cd(toolboxpath) diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index f6b4efa95..f27a518c6 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -21,6 +21,7 @@ class ClassA { static double afunction(); double memberFunction(); int nsArg(const ns1::ClassB& arg); + ns2::ns3::ClassB nsReturn(double q); }; namespace ns3 { From d745c85f13514a10c3b836dc4540d02ec890cc89 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Dec 2011 20:58:06 +0000 Subject: [PATCH 56/72] Added documentation on namespaces to gtsam.h --- gtsam.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index b7fdfacdb..c0ecea575 100644 --- a/gtsam.h +++ b/gtsam.h @@ -20,6 +20,15 @@ * - Any class with which be copied with boost::make_shared() (except Eigen) * - boost::shared_ptr of any object type (except Eigen) * Comments can use either C++ or C style, with multiple lines + * Namespace definitions + * - Names of namespaces must start with a lowercase letter + * - start a namespace with "namespace {" + * - end a namespace with exactly "}///\namespace [namespace_name]", optionally adding the name of the namespace + * - This ending is not C++ standard, and must contain "}///\namespace" to parse + * - Namespaces can be nested + * Namespace usage + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" * Methods must start with a lowercase letter * Static methods must start with a letter (upper or lowercase) and use the "static" keyword */ @@ -30,7 +39,6 @@ * - TODO: default values for arguments * - TODO: overloaded functions * - TODO: Handle Rot3M conversions to quaternions - * - TODO: namespace detection to handle nested namespaces */ class Point2 { From 9dff4c35bd6e124f12ce2604f4b26c6a02504f14 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 9 Dec 2011 15:44:35 +0000 Subject: [PATCH 57/72] Added include overrides to parser --- wrap/Class.cpp | 6 +++--- wrap/Class.h | 7 ++++--- wrap/Constructor.cpp | 13 +++++++++---- wrap/Constructor.h | 6 +++--- wrap/Method.cpp | 9 +++++++-- wrap/Method.h | 3 ++- wrap/Module.cpp | 4 +++- wrap/StaticMethod.cpp | 10 ++++++++-- wrap/StaticMethod.h | 3 ++- wrap/tests/expected/@Test/arg_EigenConstRef.cpp | 10 ++++++++++ wrap/tests/expected/@Test/create_MixedPtrs.cpp | 4 ++-- wrap/tests/expected/@Test/create_ptrs.cpp | 4 ++-- wrap/tests/expected/@Test/print.cpp | 4 ++-- wrap/tests/expected/@Test/return_Point2Ptr.cpp | 4 ++-- wrap/tests/expected/@Test/return_Test.cpp | 4 ++-- wrap/tests/expected/@Test/return_TestPtr.cpp | 4 ++-- wrap/tests/expected/@Test/return_bool.cpp | 4 ++-- wrap/tests/expected/@Test/return_double.cpp | 4 ++-- wrap/tests/expected/@Test/return_field.cpp | 4 ++-- wrap/tests/expected/@Test/return_int.cpp | 4 ++-- wrap/tests/expected/@Test/return_matrix1.cpp | 6 +++--- wrap/tests/expected/@Test/return_matrix2.cpp | 6 +++--- wrap/tests/expected/@Test/return_pair.cpp | 4 ++-- wrap/tests/expected/@Test/return_ptrs.cpp | 4 ++-- wrap/tests/expected/@Test/return_size_t.cpp | 4 ++-- wrap/tests/expected/@Test/return_string.cpp | 4 ++-- wrap/tests/expected/@Test/return_vector1.cpp | 6 +++--- wrap/tests/expected/@Test/return_vector2.cpp | 6 +++--- wrap/tests/expected/new_Test_.cpp | 4 ++-- wrap/tests/expected/new_Test_dM.cpp | 11 +++++++++++ wrap/tests/geometry.h | 2 ++ wrap/tests/testWrap.cpp | 2 ++ 32 files changed, 108 insertions(+), 62 deletions(-) create mode 100644 wrap/tests/expected/@Test/arg_EigenConstRef.cpp create mode 100644 wrap/tests/expected/new_Test_dM.cpp diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 08ce392a0..76554e608 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -60,7 +60,7 @@ void Class::matlab_proxy(const string& classFile) { void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) { BOOST_FOREACH(Constructor c, constructors) { c.matlab_mfile (toolboxPath, qualifiedName()); - c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), nameSpace); + c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), nameSpace, includes); } } @@ -69,7 +69,7 @@ void Class::matlab_methods(const string& classPath, const string& nameSpace) { string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(Method m, methods) { m.matlab_mfile (classPath); - m.matlab_wrapper(classPath, name, cppName, matlabName, nameSpace); + m.matlab_wrapper(classPath, name, cppName, matlabName, nameSpace, includes); } } @@ -78,7 +78,7 @@ void Class::matlab_static_methods(const string& toolboxPath, const string& nameS string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(StaticMethod& m, static_methods) { m.matlab_mfile (toolboxPath, qualifiedName()); - m.matlab_wrapper(toolboxPath, name, matlabName, cppName, nameSpace); + m.matlab_wrapper(toolboxPath, name, matlabName, cppName, nameSpace, includes); } } diff --git a/wrap/Class.h b/wrap/Class.h index 5938dc578..db9c14c8c 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -31,12 +31,13 @@ struct Class { Class(bool verbose=true) : verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - std::string name; ///< Class name + std::string name; ///< Class name std::vector constructors; ///< Class constructors std::vector methods; ///< Class methods std::vector static_methods; ///< Static methods - std::vector namespaces; ///< Stack of namespaces - bool verbose_; ///< verbose flag + std::vector namespaces; ///< Stack of namespaces + std::vector includes; ///< header include overrides + bool verbose_; ///< verbose flag // And finally MATLAB code is emitted, methods below called by Module::matlab_code void matlab_proxy(const std::string& classFile); ///< emit proxy class diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index c4458e198..4bc233ca2 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -56,7 +56,7 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifie if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - wrap::emit_header_comment(ofs, "%"); + emit_header_comment(ofs, "%"); ofs << "function result = " << matlabName << "(obj"; if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; @@ -71,7 +71,7 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifie void Constructor::matlab_wrapper(const string& toolboxPath, const string& cppClassName, const string& matlabClassName, - const string& nameSpace) + const string& nameSpace, const vector& includes) { string matlabName = matlab_wrapper_name(matlabClassName); @@ -83,9 +83,14 @@ void Constructor::matlab_wrapper(const string& toolboxPath, if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - wrap::emit_header_comment(ofs, "//"); + emit_header_comment(ofs, "//"); ofs << "#include " << endl; - ofs << "#include <" << name << ".h>" << endl; + if (includes.empty()) // add a default include + ofs << "#include <" << name << ".h>" << endl; + else { + BOOST_FOREACH(const string& s, includes) + ofs << "#include <" << s << ">" << endl; + } if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; ofs << "{" << endl; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 7ad8cd9d5..08906dbbd 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include "Argument.h" @@ -51,11 +51,11 @@ struct Constructor { void matlab_mfile(const std::string& toolboxPath, const std::string& qualifiedMatlabName); - /// wrapper + /// cpp wrapper void matlab_wrapper(const std::string& toolboxPath, const std::string& cppClassName, const std::string& matlabClassName, - const std::string& nameSpace); + const std::string& nameSpace, const std::vector& includes); }; } // \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 8dbc3cb4f..c5f01abcf 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -52,7 +52,7 @@ void Method::matlab_wrapper(const string& classPath, const string& className, const string& cppClassName, const string& matlabClassName, - const string& nameSpace) + const string& nameSpace, const std::vector& includes) { // open destination wrapperFile string wrapperFile = classPath + "/" + name + ".cpp"; @@ -65,7 +65,12 @@ void Method::matlab_wrapper(const string& classPath, // header wrap::emit_header_comment(ofs, "//"); ofs << "#include \n"; - ofs << "#include <" << className << ".h>\n"; + if (includes.empty()) // add a default include + ofs << "#include <" << className << ".h>" << endl; + else { + BOOST_FOREACH(const string& s, includes) + ofs << "#include <" << s << ">" << endl; + } if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; // call diff --git a/wrap/Method.h b/wrap/Method.h index c3a9f1e00..95dc83924 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -46,7 +46,8 @@ struct Method { void matlab_wrapper(const std::string& classPath, const std::string& className, const std::string& cppClassName, - const std::string& matlabClassname,const std::string& nameSpace); ///< wrapper + const std::string& matlabClassname,const std::string& nameSpace, + const std::vector& includes); ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 24257c96b..9c0feff69 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -169,7 +169,9 @@ Module::Module(const string& interfacePath, [push_back_a(cls.static_methods, static_method)] [assign_a(static_method,static_method0)]; - Rule functions_p = constructor_p | method_p | static_method_p; + Rule includes_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(cls.includes)] >> ch_p('>'); + + Rule functions_p = includes_p | constructor_p | method_p | static_method_p; Rule class_p = (str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> *(functions_p | comments_p) >> diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 352c9c680..17f3e2a60 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -50,7 +50,8 @@ void StaticMethod::matlab_mfile(const string& toolboxPath, const string& classNa /* ************************************************************************* */ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, - const string& matlabClassName, const string& cppClassName, const string& nameSpace) + const string& matlabClassName, const string& cppClassName, const string& nameSpace, + const std::vector& includes) { // open destination wrapperFile string full_name = matlabClassName + "_" + name; @@ -64,7 +65,12 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& class // header wrap::emit_header_comment(ofs, "//"); ofs << "#include \n"; - ofs << "#include <" << className << ".h>\n"; + if (includes.empty()) // add a default include + ofs << "#include <" << className << ".h>" << endl; + else { + BOOST_FOREACH(const string& s, includes) + ofs << "#include <" << s << ">" << endl; + } if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; // call diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index c50959d02..4038a398e 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -47,7 +47,8 @@ struct StaticMethod { void matlab_mfile(const std::string& toolboxPath, const std::string& className); ///< m-file void matlab_wrapper(const std::string& toolboxPath, const std::string& className, const std::string& matlabClassName, - const std::string& cppClassName, const std::string& nameSpace); ///< cpp wrapper + const std::string& cppClassName, const std::string& nameSpace, + const std::vector& includes); ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp new file mode 100644 index 000000000..b2b779c9f --- /dev/null +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("arg_EigenConstRef",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); + self->arg_EigenConstRef(value); +} diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp index 6e271486b..19a0eb740 100644 --- a/wrap/tests/expected/@Test/create_MixedPtrs.cpp +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_MixedPtrs",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index c09be35c1..26e901123 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index 0cdd5d5f1..90e1b0965 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp index 6079eb972..400a0f9df 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.cpp +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp index e14aecb89..c7a91cbe3 100644 --- a/wrap/tests/expected/@Test/return_Test.cpp +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index ae34da13b..2f2fd7ef4 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp index 5fd50cf3f..6d57ad149 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp index ef1704b07..adf1847b3 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp index 593d4b5fe..f4de80c8a 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp index 429ef73ce..736b9c981 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp index 0f3966361..9dfc56332 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix value = unwrap< Matrix >(in[1]); Matrix result = self->return_matrix1(value); - out[0] = wrap_shared_ptr(make_shared< Matrix >(result),"Matrix"); + out[0] = wrap< Matrix >(result); } diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp index 1c16e910e..c55b3c56c 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix value = unwrap< Matrix >(in[1]); Matrix result = self->return_matrix2(value); - out[0] = wrap_shared_ptr(make_shared< Matrix >(result),"Matrix"); + out[0] = wrap< Matrix >(result); } diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp index 7224f5150..a629cd757 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp index 59f8d8739..bcd24536d 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp index 887addc3f..f0a05cdcc 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp index 9b6ee320f..28dab1870 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp index 4614e660d..38b8f1b0b 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector value = unwrap< Vector >(in[1]); Vector result = self->return_vector1(value); - out[0] = wrap_shared_ptr(make_shared< Vector >(result),"Vector"); + out[0] = wrap< Vector >(result); } diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp index 6fa6f7863..c629d2eb2 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,11 +1,11 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Vector value = unwrap< Vector >(in[1]); Vector result = self->return_vector2(value); - out[0] = wrap_shared_ptr(make_shared< Vector >(result),"Vector"); + out[0] = wrap< Vector >(result); } diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp index 39898b03c..d3cf0d658 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,6 +1,6 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-08 #include -#include +#include void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Test_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Test_dM.cpp b/wrap/tests/expected/new_Test_dM.cpp new file mode 100644 index 000000000..94c473061 --- /dev/null +++ b/wrap/tests/expected/new_Test_dM.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_Test_dM",nargout,nargin,2); + double a = unwrap< double >(in[0]); + Matrix b = unwrap< Matrix >(in[1]); + Test* self = new Test(a,b); + out[0] = wrap_constructed(self,"Test"); +} diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 2eac86bfe..f4adb8eb6 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -27,6 +27,8 @@ class Point3 { */ class Test { +#include + /* a comment! */ // another comment Test(); diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index f9151dba2..d94834aed 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -111,6 +111,8 @@ TEST( wrap, parse ) { EXPECT_LONGS_EQUAL(19, testCls.methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); + EXPECT_LONGS_EQUAL( 1, testCls.includes.size()); + EXPECT(assert_equal("folder/path/to/Test.h", testCls.includes.front())); // function to parse: pair return_pair (Vector v, Matrix A) const; Method m2 = testCls.methods.front(); From c302a50146b253dcf7e86c50fb235dfc746f9012 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 9 Dec 2011 15:44:37 +0000 Subject: [PATCH 58/72] Added an implementation of the planarSLAM to gtsam.h using manual includes and namespaces, removed old duplicate header files --- configure.ac | 74 +++++++++++++++---------------- gtsam.h | 41 +++++++++--------- gtsam/slam/Landmark2.h | 27 ------------ gtsam/slam/Makefile.am | 8 ++-- gtsam/slam/PlanarSLAMGraph.h | 28 ------------ gtsam/slam/PlanarSLAMOdometry.h | 28 ------------ gtsam/slam/PlanarSLAMValues.h | 27 ------------ gtsam/slam/planarSLAM.h | 19 ++++---- wrap/Makefile.am | 77 +++++++++++++++++++-------------- 9 files changed, 112 insertions(+), 217 deletions(-) delete mode 100644 gtsam/slam/Landmark2.h delete mode 100644 gtsam/slam/PlanarSLAMGraph.h delete mode 100644 gtsam/slam/PlanarSLAMOdometry.h delete mode 100644 gtsam/slam/PlanarSLAMValues.h diff --git a/configure.ac b/configure.ac index ee6950438..5929b22ee 100644 --- a/configure.ac +++ b/configure.ac @@ -47,21 +47,6 @@ AC_ARG_ENABLE([debug], AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) - -# AGC: isn't this redundant? -#AC_CANONICAL_HOST # why was this called twice? -# We need to determine what os we are on to determine if we need to do -# special things because we are on a mac -# case $host_os in -# darwin* ) -# # Do something specific for mac -# ISMAC=true -# ;; -# *) -# ISMAC=false -# ;; -# esac - # enable profiling AC_ARG_ENABLE([profiling], [ --enable-profiling Enable profiling], @@ -95,30 +80,6 @@ AC_ARG_ENABLE([install_cppunitlite], AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue]) -# Checks for programs. -AC_PROG_CXX -AC_PROG_CC - -# Checks for libraries. -LT_INIT - -# Checks for header files. -AC_HEADER_STDC -AC_CHECK_HEADERS([string.h]) - -# Checks for typedefs, structures, and compiler characteristics. -AC_HEADER_STDBOOL -AC_C_CONST -AC_C_INLINE -AC_TYPE_SIZE_T - -# Checks for library functions. -AC_FUNC_ERROR_AT_LINE -AC_CHECK_FUNCS([pow sqrt]) - -# Check for boost -AX_BOOST_BASE([1.40]) - # enable matlab toolbox generation AC_ARG_ENABLE([build_toolbox], [ --enable-build-toolbox Enable building of the Matlab toolbox], @@ -141,6 +102,17 @@ AC_ARG_ENABLE([install_matlab_tests], AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue]) +# enable installation of matlab examples +AC_ARG_ENABLE([install_matlab_examples], + [ --enable-install-matlab-examples Enable installation of examples for the Matlab toolbox], + [case "${enableval}" in + yes) install_matlab_examples=true ;; + no) install_matlab_examples=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-examples]) ;; + esac],[install_matlab_examples=true]) + +AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_EXAMPLES], [test x$install_matlab_examples = xtrue]) + # Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix AC_ARG_WITH([toolbox], [AS_HELP_STRING([--with-toolbox], @@ -179,6 +151,30 @@ AC_ARG_WITH([wrap], [wrap=$prefix/bin]) AC_SUBST([wrap]) +# Checks for programs. +AC_PROG_CXX +AC_PROG_CC + +# Checks for libraries. +LT_INIT + +# Checks for header files. +AC_HEADER_STDC +AC_CHECK_HEADERS([string.h]) + +# Checks for typedefs, structures, and compiler characteristics. +AC_HEADER_STDBOOL +AC_C_CONST +AC_C_INLINE +AC_TYPE_SIZE_T + +# Checks for library functions. +AC_FUNC_ERROR_AT_LINE +AC_CHECK_FUNCS([pow sqrt]) + +# Check for boost +AX_BOOST_BASE([1.40]) + AC_CONFIG_FILES([CppUnitLite/Makefile \ wrap/Makefile \ gtsam/3rdparty/Makefile \ diff --git a/gtsam.h b/gtsam.h index c0ecea575..4f9dd5e46 100644 --- a/gtsam.h +++ b/gtsam.h @@ -256,14 +256,6 @@ class KalmanFilter { void update(Matrix H, Vector z, const SharedDiagonal& model); }; -class Landmark2 { - Landmark2(); - Landmark2(double x, double y); - void print(string s) const; - double x(); - double y(); -}; - class Ordering { Ordering(); void print(string s) const; @@ -276,22 +268,28 @@ class SharedNoiseModel { // FIXME: this needs actual constructors }; -class PlanarSLAMValues { - PlanarSLAMValues(); +// Planar SLAM example domain +namespace planarSLAM { + +class Values { +#include + Values(); void print(string s) const; - Pose2* pose(int key); + Pose2 pose(int key) const; + Point2 point(int key) const; void insertPose(int key, const Pose2& pose); void insertPoint(int key, const Point2& point); }; -class PlanarSLAMGraph { - PlanarSLAMGraph(); +class Graph { +#include + Graph(); void print(string s) const; - double error(const PlanarSLAMValues& values) const; - Ordering* orderingCOLAMD(const PlanarSLAMValues& values) const; - GaussianFactorGraph* linearize(const PlanarSLAMValues& values, + double error(const planarSLAM::Values& values) const; + Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; + GaussianFactorGraph* linearize(const planarSLAM::Values& values, const Ordering& ordering) const; void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); @@ -301,16 +299,19 @@ class PlanarSLAMGraph { void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel); void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, const SharedNoiseModel& noiseModel); - PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate); + planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); }; -class PlanarSLAMOdometry { - PlanarSLAMOdometry(int key1, int key2, const Pose2& measured, +class Odometry { +#include + Odometry(int key1, int key2, const Pose2& measured, const SharedNoiseModel& model); void print(string s) const; - GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; + GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const; }; +}///\namespace planarSLAM + class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianBayesNet* eliminate() const; diff --git a/gtsam/slam/Landmark2.h b/gtsam/slam/Landmark2.h deleted file mode 100644 index f67cfed56..000000000 --- a/gtsam/slam/Landmark2.h +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Landmark2.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::Point2 Landmark2; - -} - diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 406431edd..d55d40666 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -51,10 +51,10 @@ sources += planarSLAM.cpp check_PROGRAMS += tests/testPlanarSLAM # MATLAB Wrap headers for planarSLAM -headers += Landmark2.h -headers += PlanarSLAMGraph.h -headers += PlanarSLAMValues.h -headers += PlanarSLAMOdometry.h +# headers += Landmark2.h +# headers += PlanarSLAMGraph.h +# headers += PlanarSLAMValues.h +# headers += PlanarSLAMOdometry.h # 3D Pose constraints sources += pose3SLAM.cpp diff --git a/gtsam/slam/PlanarSLAMGraph.h b/gtsam/slam/PlanarSLAMGraph.h deleted file mode 100644 index 98eb9aaef..000000000 --- a/gtsam/slam/PlanarSLAMGraph.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file PlanarSLAMGraph.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Graph PlanarSLAMGraph; - -} - diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h deleted file mode 100644 index 941eda285..000000000 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file PlanarSLAMOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Odometry PlanarSLAMOdometry; - -} - diff --git a/gtsam/slam/PlanarSLAMValues.h b/gtsam/slam/PlanarSLAMValues.h deleted file mode 100644 index d174229fd..000000000 --- a/gtsam/slam/PlanarSLAMValues.h +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file PlanarSLAMValues.h - * @brief Convenience for MATLAB wrapper, which does not allow for identically named methods - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Values PlanarSLAMValues; - -} - diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 228ced08c..5776e989c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -71,10 +71,10 @@ namespace gtsam { // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - boost::shared_ptr pose(int key) { - Pose2 pose = (*this)[PoseKey(key)]; - return boost::shared_ptr(new Pose2(pose)); - } + Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + + /// get a point + Point2 point(int key) const { return (*this)[PointKey(key)]; } /// insert a pose void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } @@ -131,14 +131,11 @@ namespace gtsam { void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); - /// Optimize_ for MATLAB - boost::shared_ptr optimize_(const Values& initialEstimate) { + /// Optimize + Values optimize(const Values& initialEstimate) { typedef NonlinearOptimizer Optimizer; - boost::shared_ptr result( - new Values( - *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA))); - return result; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); } }; diff --git a/wrap/Makefile.am b/wrap/Makefile.am index d572b88e4..7d0c980e1 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -58,64 +58,75 @@ interfacePath = $(top_srcdir) moduleName = gtsam toolboxpath = ../toolbox nameSpace = "gtsam" + +# Set flags to pass to mex mexFlags = -mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" if ENABLE_UNSAFE_WRAP - mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" else - mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" endif -# Find the extension for mex binaries - FIXME: this should be done with mexext with matlab -mexextension = mexa64 +# Find the extension for mex binaries +# this should be done with mexext with matlab +mexextension = if LINUX if IS_64BIT - mexextension += mexa64 +mexextension += mexa64 else - mexextension += mexglx +mexextension += mexglx endif else # Linux if DARWIN - mexextension += mexmaci64 +mexextension += mexmaci64 else - mexextension += mex_bin +mexextension += mex_bin endif endif # Linux -all: +all: generate_toolbox + +generate_toolbox: $(top_srcdir)/gtsam.h ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} -wrap-install-bin: all - install -d ${wrap} && \ +source_mode = -m 644 + +wrap-install-matlab-toolbox: + install -d ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.m ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.cpp ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/Makefile ${toolbox}/gtsam + cp -ru ../toolbox/@* ${toolbox}/gtsam + +wrap-install-bin: + install -d ${wrap} install ./wrap ${wrap} -wrap-install-matlab-tests: all - install -d ${toolbox}/gtsam/tests && \ - cp -rf ../../tests/matlab/*.m ${toolbox}/gtsam/tests +wrap-install-matlab-tests: + install -d ${toolbox}/gtsam/tests + install ${source_mode} -c ../../tests/matlab/*.m ${toolbox}/gtsam/tests + +wrap-install-matlab-examples: + install -d ${toolbox}/gtsam/examples + install ${source_mode} -c ../../examples/matlab/*.m ${toolbox}/gtsam/examples + +wrap_install_targets = +wrap_install_targets += wrap-install-matlab-toolbox -# install the headers and matlab toolbox if ENABLE_INSTALL_WRAP +wrap_install_targets += wrap-install-bin +endif + if ENABLE_INSTALL_MATLAB_TESTS -install-exec-hook: wrap-install-bin wrap-install-matlab-tests - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam -else -install-exec-hook: wrap-install-bin - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam/tests -endif -else -if ENABLE_INSTALL_MATLAB_TESTS -install-exec-hook: wrap-install-matlab-tests - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam -else -install-exec-hook: all - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam/tests +wrap_install_targets += wrap-install-matlab-tests endif + +if ENABLE_INSTALL_MATLAB_EXAMPLES +wrap_install_targets += wrap-install-matlab-examples endif +install-exec-hook: ${wrap_install_targets} + # clean local toolbox dir clean: @test -z "wrap" || rm -f wrap From 4e5a80aa56673705edf72373ddecf92aa8aa710b Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 9 Dec 2011 16:36:50 +0000 Subject: [PATCH 59/72] Removed extraneous headers that were previously needed for wrap, added start of simulated2D and simulated2DOriented domains to gtsam.h, more wrap documentation --- gtsam.h | 101 +++++++++++------- gtsam/slam/Makefile.am | 20 +--- gtsam/slam/Simulated2DMeasurement.h | 28 ----- gtsam/slam/Simulated2DOdometry.h | 28 ----- gtsam/slam/Simulated2DOrientedOdometry.h | 28 ----- gtsam/slam/Simulated2DOrientedPosePrior.h | 29 ----- gtsam/slam/Simulated2DOrientedValues.h | 59 ---------- gtsam/slam/Simulated2DPointPrior.h | 29 ----- gtsam/slam/Simulated2DPosePrior.h | 29 ----- gtsam/slam/Simulated2DValues.h | 58 ---------- gtsam/slam/simulated2D.h | 16 ++- gtsam/slam/simulated2DOriented.h | 30 +++++- .../slam/{Simulated3D.cpp => simulated3D.cpp} | 2 +- gtsam/slam/{Simulated3D.h => simulated3D.h} | 0 gtsam/slam/tests/testSimulated3D.cpp | 2 +- wrap/Makefile.am | 6 +- 16 files changed, 110 insertions(+), 355 deletions(-) delete mode 100644 gtsam/slam/Simulated2DMeasurement.h delete mode 100644 gtsam/slam/Simulated2DOdometry.h delete mode 100644 gtsam/slam/Simulated2DOrientedOdometry.h delete mode 100644 gtsam/slam/Simulated2DOrientedPosePrior.h delete mode 100644 gtsam/slam/Simulated2DOrientedValues.h delete mode 100644 gtsam/slam/Simulated2DPointPrior.h delete mode 100644 gtsam/slam/Simulated2DPosePrior.h delete mode 100644 gtsam/slam/Simulated2DValues.h rename gtsam/slam/{Simulated3D.cpp => simulated3D.cpp} (96%) rename gtsam/slam/{Simulated3D.h => simulated3D.h} (100%) diff --git a/gtsam.h b/gtsam.h index 4f9dd5e46..819d56c5b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -13,6 +13,9 @@ * - void * - Any class with which be copied with boost::make_shared() * - boost::shared_ptr of any object type + * Limitations on methods + * - Parsing does not support overloading + * - There can only be one method with a given name * Arguments to functions any of * - Eigen types: Matrix, Vector * - Eigen types and classes as an optionally const reference @@ -31,6 +34,9 @@ * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" * Methods must start with a lowercase letter * Static methods must start with a letter (upper or lowercase) and use the "static" keyword + * Includes in C++ wrappers + * - By default, the include will be <[classname].h> + * - To override, add a full include statement inside the class definition */ /** @@ -181,6 +187,12 @@ class SharedDiagonal { Vector sample() const; }; +class SharedNoiseModel { +#include + SharedNoiseModel(const SharedDiagonal& model); + SharedNoiseModel(const SharedGaussian& model); +}; + class VectorValues { VectorValues(); VectorValues(int nVars, int varDim); @@ -245,6 +257,14 @@ class GaussianFactorGraph { Matrix sparseJacobian_() const; }; +class GaussianSequentialSolver { + GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); + GaussianBayesNet* eliminate() const; + VectorValues* optimize() const; + GaussianFactor* marginalFactor(int j) const; + Matrix marginalCovariance(int j) const; +}; + class KalmanFilter { KalmanFilter(Vector x, const SharedDiagonal& model); void print(string s) const; @@ -263,11 +283,6 @@ class Ordering { void push_back(string key); }; -class SharedNoiseModel { - SharedNoiseModel(); - // FIXME: this needs actual constructors -}; - // Planar SLAM example domain namespace planarSLAM { @@ -312,14 +327,51 @@ class Odometry { }///\namespace planarSLAM -class GaussianSequentialSolver { - GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); - GaussianBayesNet* eliminate() const; - VectorValues* optimize() const; - GaussianFactor* marginalFactor(int j) const; - Matrix marginalCovariance(int j) const; +// Simulated2D Example Domain +namespace simulated2D { + +class Values { +#include + Values(); + void insertPose(int i, const Point2& p); + void insertPoint(int j, const Point2& p); + int nrPoses() const; + int nrPoints() const; + Point2 pose(int i); + Point2 point(int j); }; +class Graph { +#include + Graph(); +}; + +// TODO: add factors, etc. + +}///\namespace simulated2D + +// Simulated2DOriented Example Domain +namespace simulated2DOriented { + +class Values { +#include + Values(); + void insertPose(int i, const Pose2& p); + void insertPoint(int j, const Point2& p); + int nrPoses() const; + int nrPoints() const; + Pose2 pose(int i); + Point2 point(int j); +}; + +class Graph { +#include + Graph(); +}; + +// TODO: add factors, etc. + +}///\namespace simulated2DOriented //// These are considered to be broken and will be added back as they start working //// It's assumed that there have been interface changes that might break this @@ -339,28 +391,6 @@ class GaussianSequentialSolver { // void push_back(GaussianFactor* factor); //}; // -//class Simulated2DValues { -// Simulated2DValues(); -// void print(string s) const; -// void insertPose(int i, const Point2& p); -// void insertPoint(int j, const Point2& p); -// int nrPoses() const; -// int nrPoints() const; -// Point2* pose(int i); -// Point2* point(int j); -//}; -// -//class Simulated2DOrientedValues { -// Simulated2DOrientedValues(); -// void print(string s) const; -// void insertPose(int i, const Pose2& p); -// void insertPoint(int j, const Point2& p); -// int nrPoses() const; -// int nrPoints() const; -// Pose2* pose(int i); -// Point2* point(int j); -//}; -// //class Simulated2DPosePrior { // Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); // void print(string s) const; @@ -397,10 +427,6 @@ class GaussianSequentialSolver { // double error(const Simulated2DValues& c) const; //}; // -//// These are currently broken -//// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class -//// We also have to solve the shared pointer mess to avoid duplicate methods -// //class GaussianFactor { // GaussianFactor(string key1, // Matrix A1, @@ -435,7 +461,6 @@ class GaussianSequentialSolver { // VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; //}; // -// //class Pose2Values{ // Pose2Values(); // Pose2 get(string key) const; diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index d55d40666..fa51a9512 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -15,24 +15,12 @@ headers += simulated2DConstraints.h sources += simulated2D.cpp smallExample.cpp check_PROGRAMS += tests/testSimulated2D -# MATLAB Wrap headers for simulated2D -headers += Simulated2DMeasurement.h -headers += Simulated2DOdometry.h -headers += Simulated2DPointPrior.h -headers += Simulated2DPosePrior.h -headers += Simulated2DValues.h - # simulated2DOriented example sources += simulated2DOriented.cpp check_PROGRAMS += tests/testSimulated2DOriented -# MATLAB Wrap headers for simulated2DOriented -headers += Simulated2DOrientedOdometry.h -headers += Simulated2DOrientedPosePrior.h -headers += Simulated2DOrientedValues.h - # simulated3D example -sources += Simulated3D.cpp +sources += simulated3D.cpp check_PROGRAMS += tests/testSimulated3D # Generic SLAM headers @@ -50,12 +38,6 @@ check_PROGRAMS += tests/testPose2SLAM sources += planarSLAM.cpp check_PROGRAMS += tests/testPlanarSLAM -# MATLAB Wrap headers for planarSLAM -# headers += Landmark2.h -# headers += PlanarSLAMGraph.h -# headers += PlanarSLAMValues.h -# headers += PlanarSLAMOdometry.h - # 3D Pose constraints sources += pose3SLAM.cpp check_PROGRAMS += tests/testPose3SLAM diff --git a/gtsam/slam/Simulated2DMeasurement.h b/gtsam/slam/Simulated2DMeasurement.h deleted file mode 100644 index 0ab661086..000000000 --- a/gtsam/slam/Simulated2DMeasurement.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DMeasurement.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2D::Measurement Simulated2DMeasurement; - -} - diff --git a/gtsam/slam/Simulated2DOdometry.h b/gtsam/slam/Simulated2DOdometry.h deleted file mode 100644 index 5bd3afc14..000000000 --- a/gtsam/slam/Simulated2DOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2D::Odometry Simulated2DOdometry; - -} - diff --git a/gtsam/slam/Simulated2DOrientedOdometry.h b/gtsam/slam/Simulated2DOrientedOdometry.h deleted file mode 100644 index 1a659c081..000000000 --- a/gtsam/slam/Simulated2DOrientedOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DOrientedOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Kai Ni - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry; - -} - diff --git a/gtsam/slam/Simulated2DOrientedPosePrior.h b/gtsam/slam/Simulated2DOrientedPosePrior.h deleted file mode 100644 index 54a83f1be..000000000 --- a/gtsam/slam/Simulated2DOrientedPosePrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DOrientedPosePrior.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Kai Ni - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a pose Point2 with key 'x1' etc... */ - typedef simulated2DOriented::GenericPosePrior Simulated2DOrientedPosePrior; - -} - diff --git a/gtsam/slam/Simulated2DOrientedValues.h b/gtsam/slam/Simulated2DOrientedValues.h deleted file mode 100644 index b1fe098f8..000000000 --- a/gtsam/slam/Simulated2DOrientedValues.h +++ /dev/null @@ -1,59 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DValues.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - class Simulated2DOrientedValues: public simulated2DOriented::Values { - public: - typedef boost::shared_ptr sharedPoint; - typedef boost::shared_ptr sharedPose; - - Simulated2DOrientedValues() { - } - - void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) { - insert(i, p); - } - - void insertPoint(const simulated2DOriented::PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { - return this->first_.size(); - } - - int nrPoints() const { - return this->second_.size(); - } - - sharedPose pose(const simulated2DOriented::PoseKey& i) { - return sharedPose(new Pose2((*this)[i])); - } - - sharedPoint point(const simulated2DOriented::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); - } - - }; - -} // namespace gtsam - diff --git a/gtsam/slam/Simulated2DPointPrior.h b/gtsam/slam/Simulated2DPointPrior.h deleted file mode 100644 index 8fc896d92..000000000 --- a/gtsam/slam/Simulated2DPointPrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DPointPrior.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a landmark Point2 with key 'l1' etc... */ - typedef simulated2D::GenericPrior Simulated2DPointPrior; - -} - diff --git a/gtsam/slam/Simulated2DPosePrior.h b/gtsam/slam/Simulated2DPosePrior.h deleted file mode 100644 index 1c85e4d5b..000000000 --- a/gtsam/slam/Simulated2DPosePrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DPosePrior.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a pose Point2 with key 'x1' etc... */ - typedef simulated2D::GenericPrior Simulated2DPosePrior; - -} - diff --git a/gtsam/slam/Simulated2DValues.h b/gtsam/slam/Simulated2DValues.h deleted file mode 100644 index c4d1d1534..000000000 --- a/gtsam/slam/Simulated2DValues.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Simulated2DValues.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - class Simulated2DValues: public simulated2D::Values { - public: - typedef boost::shared_ptr sharedPoint; - - Simulated2DValues() { - } - - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); - } - - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { - return this->first_.size(); - } - - int nrPoints() const { - return this->second_.size(); - } - - sharedPoint pose(const simulated2D::PoseKey& i) { - return sharedPoint(new Point2((*this)[i])); - } - - sharedPoint point(const simulated2D::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); - } - - }; - -} // namespace gtsam - diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 3b63c9595..cae60bb80 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -21,6 +21,7 @@ #include #include #include +#include // \namespace @@ -72,13 +73,13 @@ namespace gtsam { } /// Return pose i - sharedPoint pose(const simulated2D::PoseKey& i) { - return sharedPoint(new Point2((*this)[i])); + Point2 pose(const simulated2D::PoseKey& i) const { + return (*this)[i]; } /// Return point j - sharedPoint point(const simulated2D::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); + Point2 point(const simulated2D::PointKey& j) const { + return (*this)[j]; } }; @@ -232,5 +233,12 @@ namespace gtsam { typedef GenericOdometry Odometry; typedef GenericMeasurement Measurement; + // Specialization of a graph for this example domain + // TODO: add functions to add factor types + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + }; + } // namespace simulated2D } // namespace gtsam diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 3710f29ad..b9cf150ef 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -21,6 +21,7 @@ #include #include #include +#include // \namespace @@ -33,7 +34,27 @@ namespace gtsam { typedef TypedSymbol PoseKey; typedef Values PoseValues; typedef Values PointValues; - typedef TupleValues2 Values; + + /// Specialized Values structure with syntactic sugar for + /// compatibility with matlab + class Values: public TupleValues2 { + public: + Values() {} + + void insertPose(const PoseKey& i, const Pose2& p) { + insert(i, p); + } + + void insertPoint(const PointKey& j, const Point2& p) { + insert(j, p); + } + + int nrPoses() const { return this->first_.size(); } + int nrPoints() const { return this->second_.size(); } + + Pose2 pose(const PoseKey& i) const { return (*this)[i]; } + Point2 point(const PointKey& j) const { return (*this)[j]; } + }; //TODO:: point prior is not implemented right now @@ -100,5 +121,12 @@ namespace gtsam { typedef GenericOdometry Odometry; + /// Graph specialization for syntactic sugar use with matlab + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + // TODO: add functions to add factors + }; + } // namespace simulated2DOriented } // namespace gtsam diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/simulated3D.cpp similarity index 96% rename from gtsam/slam/Simulated3D.cpp rename to gtsam/slam/simulated3D.cpp index 76e65a8cf..e2f6211a5 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/simulated3D.cpp @@ -15,7 +15,7 @@ * @author Alex Cunningham **/ -#include +#include namespace gtsam { diff --git a/gtsam/slam/Simulated3D.h b/gtsam/slam/simulated3D.h similarity index 100% rename from gtsam/slam/Simulated3D.h rename to gtsam/slam/simulated3D.h diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index de2c534bb..30f41e2fd 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 7d0c980e1..3dd54942a 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -91,16 +91,16 @@ generate_toolbox: $(top_srcdir)/gtsam.h source_mode = -m 644 -wrap-install-matlab-toolbox: +wrap-install-matlab-toolbox: generate_toolbox install -d ${toolbox}/gtsam install ${source_mode} -c ../toolbox/*.m ${toolbox}/gtsam install ${source_mode} -c ../toolbox/*.cpp ${toolbox}/gtsam install ${source_mode} -c ../toolbox/Makefile ${toolbox}/gtsam cp -ru ../toolbox/@* ${toolbox}/gtsam -wrap-install-bin: +wrap-install-bin: wrap install -d ${wrap} - install ./wrap ${wrap} + install -c ./wrap ${wrap} wrap-install-matlab-tests: install -d ${toolbox}/gtsam/tests From 06dbc2b650b851a224a330f49d4cd26f06811e32 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 9 Dec 2011 20:29:47 +0000 Subject: [PATCH 60/72] Changed namespace mechanism in wrap to "using namespace gtsam;" inside gtsam.h --- gtsam.h | 3 ++ wrap/Class.cpp | 12 ++++---- wrap/Class.h | 6 ++-- wrap/Constructor.cpp | 14 +++------ wrap/Constructor.h | 3 +- wrap/Makefile.am | 3 +- wrap/Method.cpp | 12 ++------ wrap/Method.h | 3 +- wrap/Module.cpp | 12 ++++---- wrap/Module.h | 2 +- wrap/StaticMethod.cpp | 13 +++------ wrap/StaticMethod.h | 3 +- wrap/tests/expected/@Point2/dim.cpp | 3 +- .../expected/@Point2/vectorConfusion.cpp | 11 +++++++ wrap/tests/expected/@Point2/vectorConfusion.m | 4 +++ wrap/tests/expected/@Point2/x.cpp | 3 +- wrap/tests/expected/@Point2/y.cpp | 3 +- wrap/tests/expected/@Point3/norm.cpp | 3 +- .../expected/@Test/arg_EigenConstRef.cpp | 3 +- wrap/tests/expected/@Test/arg_EigenConstRef.m | 4 +++ .../tests/expected/@Test/create_MixedPtrs.cpp | 3 +- wrap/tests/expected/@Test/create_ptrs.cpp | 3 +- wrap/tests/expected/@Test/print.cpp | 3 +- .../tests/expected/@Test/return_Point2Ptr.cpp | 3 +- wrap/tests/expected/@Test/return_Test.cpp | 3 +- wrap/tests/expected/@Test/return_TestPtr.cpp | 3 +- wrap/tests/expected/@Test/return_bool.cpp | 3 +- wrap/tests/expected/@Test/return_double.cpp | 3 +- wrap/tests/expected/@Test/return_field.cpp | 3 +- wrap/tests/expected/@Test/return_int.cpp | 3 +- wrap/tests/expected/@Test/return_matrix1.cpp | 3 +- wrap/tests/expected/@Test/return_matrix2.cpp | 3 +- wrap/tests/expected/@Test/return_pair.cpp | 3 +- wrap/tests/expected/@Test/return_ptrs.cpp | 3 +- wrap/tests/expected/@Test/return_size_t.cpp | 3 +- wrap/tests/expected/@Test/return_string.cpp | 3 +- wrap/tests/expected/@Test/return_vector1.cpp | 3 +- wrap/tests/expected/@Test/return_vector2.cpp | 3 +- .../expected/Point3_StaticFunctionRet.cpp | 8 +++-- wrap/tests/expected/Point3_staticFunction.cpp | 3 +- wrap/tests/expected/new_Point2_.cpp | 3 +- wrap/tests/expected/new_Point2_dd.cpp | 3 +- wrap/tests/expected/new_Point3_ddd.cpp | 3 +- wrap/tests/expected/new_Test_.cpp | 3 +- wrap/tests/expected/new_Test_dM.cpp | 3 +- wrap/tests/geometry.h | 4 +++ wrap/tests/testWrap.cpp | 10 +++++-- wrap/utilities.cpp | 29 +++++++++++++++---- wrap/utilities.h | 13 +++++++++ wrap/wrap.cpp | 6 ++-- 50 files changed, 170 insertions(+), 92 deletions(-) create mode 100644 wrap/tests/expected/@Point2/vectorConfusion.cpp create mode 100644 wrap/tests/expected/@Point2/vectorConfusion.m create mode 100644 wrap/tests/expected/@Test/arg_EigenConstRef.m diff --git a/gtsam.h b/gtsam.h index 819d56c5b..071ababc8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -47,6 +47,9 @@ * - TODO: Handle Rot3M conversions to quaternions */ +// Everything is in the gtsam namespace, so we avoid copying everything in +using namespace gtsam; + class Point2 { Point2(); Point2(double x, double y); diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 76554e608..82245bd26 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -57,28 +57,28 @@ void Class::matlab_proxy(const string& classFile) { } /* ************************************************************************* */ -void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) { +void Class::matlab_constructors(const string& toolboxPath, const vector& using_namespaces) { BOOST_FOREACH(Constructor c, constructors) { c.matlab_mfile (toolboxPath, qualifiedName()); - c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), nameSpace, includes); + c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes); } } /* ************************************************************************* */ -void Class::matlab_methods(const string& classPath, const string& nameSpace) { +void Class::matlab_methods(const string& classPath, const vector& using_namespaces) { string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(Method m, methods) { m.matlab_mfile (classPath); - m.matlab_wrapper(classPath, name, cppName, matlabName, nameSpace, includes); + m.matlab_wrapper(classPath, name, cppName, matlabName, using_namespaces, includes); } } /* ************************************************************************* */ -void Class::matlab_static_methods(const string& toolboxPath, const string& nameSpace) { +void Class::matlab_static_methods(const string& toolboxPath, const vector& using_namespaces) { string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(StaticMethod& m, static_methods) { m.matlab_mfile (toolboxPath, qualifiedName()); - m.matlab_wrapper(toolboxPath, name, matlabName, cppName, nameSpace, includes); + m.matlab_wrapper(toolboxPath, name, matlabName, cppName, using_namespaces, includes); } } diff --git a/wrap/Class.h b/wrap/Class.h index db9c14c8c..ac23102b9 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -42,11 +42,11 @@ struct Class { // And finally MATLAB code is emitted, methods below called by Module::matlab_code void matlab_proxy(const std::string& classFile); ///< emit proxy class void matlab_constructors(const std::string& toolboxPath, - const std::string& nameSpace); ///< emit constructor wrappers + const std::vector& using_namespaces); ///< emit constructor wrappers void matlab_methods(const std::string& classPath, - const std::string& nameSpace); ///< emit method wrappers + const std::vector& using_namespaces); ///< emit method wrappers void matlab_static_methods(const std::string& classPath, - const std::string& nameSpace); ///< emit static method wrappers + const std::vector& using_namespaces); ///< emit static method wrappers void matlab_make_fragment(std::ofstream& ofs, const std::string& toolboxPath, const std::string& mexFlags); ///< emit make fragment for global make script diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 4bc233ca2..04a873547 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -71,9 +71,8 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifie void Constructor::matlab_wrapper(const string& toolboxPath, const string& cppClassName, const string& matlabClassName, - const string& nameSpace, const vector& includes) + const vector& using_namespaces, const vector& includes) { - string matlabName = matlab_wrapper_name(matlabClassName); // open destination wrapperFile @@ -84,14 +83,9 @@ void Constructor::matlab_wrapper(const string& toolboxPath, // generate code emit_header_comment(ofs, "//"); - ofs << "#include " << endl; - if (includes.empty()) // add a default include - ofs << "#include <" << name << ".h>" << endl; - else { - BOOST_FOREACH(const string& s, includes) - ofs << "#include <" << s << ">" << endl; - } - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateIncludes(ofs, name, includes); + generateUsingNamespace(ofs, using_namespaces); + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; ofs << "{" << endl; ofs << " checkArguments(\"" << matlabName << "\",nargout,nargin," << args.size() << ");" << endl; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 08906dbbd..5409ed1f6 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -55,7 +55,8 @@ struct Constructor { void matlab_wrapper(const std::string& toolboxPath, const std::string& cppClassName, const std::string& matlabClassName, - const std::string& nameSpace, const std::vector& includes); + const std::vector& using_namespaces, + const std::vector& includes); }; } // \namespace wrap diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 3dd54942a..8b3e4f493 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -57,7 +57,6 @@ LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a interfacePath = $(top_srcdir) moduleName = gtsam toolboxpath = ../toolbox -nameSpace = "gtsam" # Set flags to pass to mex mexFlags = @@ -87,7 +86,7 @@ endif # Linux all: generate_toolbox generate_toolbox: $(top_srcdir)/gtsam.h - ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} + ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${mexFlags} source_mode = -m 644 diff --git a/wrap/Method.cpp b/wrap/Method.cpp index c5f01abcf..b9f10a9d4 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -52,7 +52,7 @@ void Method::matlab_wrapper(const string& classPath, const string& className, const string& cppClassName, const string& matlabClassName, - const string& nameSpace, const std::vector& includes) + const vector& using_namespaces, const std::vector& includes) { // open destination wrapperFile string wrapperFile = classPath + "/" + name + ".cpp"; @@ -64,14 +64,8 @@ void Method::matlab_wrapper(const string& classPath, // header wrap::emit_header_comment(ofs, "//"); - ofs << "#include \n"; - if (includes.empty()) // add a default include - ofs << "#include <" << className << ".h>" << endl; - else { - BOOST_FOREACH(const string& s, includes) - ofs << "#include <" << s << ">" << endl; - } - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateIncludes(ofs, className, includes); + generateUsingNamespace(ofs, using_namespaces); // call ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; diff --git a/wrap/Method.h b/wrap/Method.h index 95dc83924..57bd1c043 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -46,7 +46,8 @@ struct Method { void matlab_wrapper(const std::string& classPath, const std::string& className, const std::string& cppClassName, - const std::string& matlabClassname,const std::string& nameSpace, + const std::string& matlabClassname, + const std::vector& using_namespaces, const std::vector& includes); ///< cpp wrapper }; diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 9c0feff69..2fd8c9747 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -184,7 +184,10 @@ Module::Module(const string& interfacePath, str_p("}///\\namespace") >> !namespace_name_p // end namespace, avoid confusion with classes [pop_a(namespaces)]; - Rule module_content_p = comments_p | class_p | namespace_def_p ; + Rule using_namespace_p = str_p("using") >> str_p("namespace") + >> namespace_name_p[push_back_a(using_namespaces)] >> ch_p(';'); + + Rule module_content_p = comments_p | using_namespace_p | class_p | namespace_def_p ; Rule module_p = *module_content_p >> !end_p; @@ -238,7 +241,6 @@ void verifyArguments(const vector& validArgs, const vector& vt) { /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, - const string& nameSpace, const string& mexExt, const string& mexFlags) { @@ -306,9 +308,9 @@ void Module::matlab_code(const string& toolboxPath, verifyArguments(validArgs, cls.methods); // create constructor and method wrappers - cls.matlab_constructors(toolboxPath,nameSpace); - cls.matlab_static_methods(toolboxPath,nameSpace); - cls.matlab_methods(classPath,nameSpace); + cls.matlab_constructors(toolboxPath,using_namespaces); + cls.matlab_static_methods(toolboxPath,using_namespaces); + cls.matlab_methods(classPath,using_namespaces); // add lines to make m-file ofs << "%% " << cls.qualifiedName() << endl; diff --git a/wrap/Module.h b/wrap/Module.h index 6ae9202cb..3c294f380 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -31,6 +31,7 @@ struct Module { std::string name; ///< module name std::vector classes; ///< list of classes bool verbose; ///< verbose flag + std::vector using_namespaces; ///< all default namespaces /// constructor that parses interface file Module(const std::string& interfacePath, @@ -39,7 +40,6 @@ struct Module { /// MATLAB code generation: void matlab_code(const std::string& path, - const std::string& nameSpace, const std::string& mexExt, const std::string& mexFlags); }; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 17f3e2a60..02a2aaf76 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -50,7 +50,8 @@ void StaticMethod::matlab_mfile(const string& toolboxPath, const string& classNa /* ************************************************************************* */ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, - const string& matlabClassName, const string& cppClassName, const string& nameSpace, + const string& matlabClassName, const string& cppClassName, + const vector& using_namespaces, const std::vector& includes) { // open destination wrapperFile @@ -64,14 +65,8 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& class // header wrap::emit_header_comment(ofs, "//"); - ofs << "#include \n"; - if (includes.empty()) // add a default include - ofs << "#include <" << className << ".h>" << endl; - else { - BOOST_FOREACH(const string& s, includes) - ofs << "#include <" << s << ">" << endl; - } - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateIncludes(ofs, className, includes); + generateUsingNamespace(ofs, using_namespaces); // call ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 4038a398e..429e24183 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -47,7 +47,8 @@ struct StaticMethod { void matlab_mfile(const std::string& toolboxPath, const std::string& className); ///< m-file void matlab_wrapper(const std::string& toolboxPath, const std::string& className, const std::string& matlabClassName, - const std::string& cppClassName, const std::string& nameSpace, + const std::string& cppClassName, + const std::vector& using_namespaces, const std::vector& includes); ///< cpp wrapper }; diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index 1d6486356..e845db94a 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/vectorConfusion.cpp b/wrap/tests/expected/@Point2/vectorConfusion.cpp new file mode 100644 index 000000000..cdb06b2df --- /dev/null +++ b/wrap/tests/expected/@Point2/vectorConfusion.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("vectorConfusion",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + VectorNotEigen result = self->vectorConfusion(); + out[0] = wrap_shared_ptr(make_shared< VectorNotEigen >(result),"VectorNotEigen"); +} diff --git a/wrap/tests/expected/@Point2/vectorConfusion.m b/wrap/tests/expected/@Point2/vectorConfusion.m new file mode 100644 index 000000000..d141e8085 --- /dev/null +++ b/wrap/tests/expected/@Point2/vectorConfusion.m @@ -0,0 +1,4 @@ +function result = vectorConfusion(obj) +% usage: obj.vectorConfusion() + error('need to compile vectorConfusion.cpp'); +end diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp index 4c2ef7b33..d245748d0 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index 525238ea9..6342d6238 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 4c494a5f8..ad36e32b8 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("norm",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp index b2b779c9f..e44b74f1b 100644 --- a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("arg_EigenConstRef",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.m b/wrap/tests/expected/@Test/arg_EigenConstRef.m new file mode 100644 index 000000000..e353faa29 --- /dev/null +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.m @@ -0,0 +1,4 @@ +function result = arg_EigenConstRef(obj,value) +% usage: obj.arg_EigenConstRef(value) + error('need to compile arg_EigenConstRef.cpp'); +end diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp index 19a0eb740..a2c237c05 100644 --- a/wrap/tests/expected/@Test/create_MixedPtrs.cpp +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_MixedPtrs",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index 26e901123..33722bd14 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index 90e1b0965..e92a58b10 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp index 400a0f9df..99cf67f0b 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.cpp +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp index c7a91cbe3..19256f9ac 100644 --- a/wrap/tests/expected/@Test/return_Test.cpp +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index 2f2fd7ef4..39ed01f15 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp index 6d57ad149..016bf2934 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp index adf1847b3..c087c194b 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp index f4de80c8a..3c2ea29b0 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp index 736b9c981..c9bada69e 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp index 9dfc56332..acd39c9f5 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp index c55b3c56c..50e2ee462 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp index a629cd757..a10e79109 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp index bcd24536d..6c2ab46a6 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp index f0a05cdcc..af1524ec7 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp index 28dab1870..71a86e63b 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp index 38b8f1b0b..df8779989 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp index c629d2eb2..ac87ab83a 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.cpp b/wrap/tests/expected/Point3_StaticFunctionRet.cpp index 1877b7e99..657f90bb1 100644 --- a/wrap/tests/expected/Point3_StaticFunctionRet.cpp +++ b/wrap/tests/expected/Point3_StaticFunctionRet.cpp @@ -1,9 +1,11 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("Point3_StaticFunctionRet",nargout,nargin,0); - Point3 result = Point3::StaticFunctionRet(); + checkArguments("Point3_StaticFunctionRet",nargout,nargin,1); + double z = unwrap< double >(in[0]); + Point3 result = Point3::StaticFunctionRet(z); out[0] = wrap_shared_ptr(make_shared< Point3 >(result),"Point3"); } diff --git a/wrap/tests/expected/Point3_staticFunction.cpp b/wrap/tests/expected/Point3_staticFunction.cpp index 869810709..e0519e20e 100644 --- a/wrap/tests/expected/Point3_staticFunction.cpp +++ b/wrap/tests/expected/Point3_staticFunction.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("Point3_staticFunction",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index 34ef89203..5cfcad238 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point2_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Point2_dd.cpp b/wrap/tests/expected/new_Point2_dd.cpp index 61d4d1535..6ab5721a1 100644 --- a/wrap/tests/expected/new_Point2_dd.cpp +++ b/wrap/tests/expected/new_Point2_dd.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point2_dd",nargout,nargin,2); diff --git a/wrap/tests/expected/new_Point3_ddd.cpp b/wrap/tests/expected/new_Point3_ddd.cpp index f9bed0d45..285ed9ca6 100644 --- a/wrap/tests/expected/new_Point3_ddd.cpp +++ b/wrap/tests/expected/new_Point3_ddd.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-06 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point3_ddd",nargout,nargin,3); diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp index d3cf0d658..9c9aaab88 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Test_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Test_dM.cpp b/wrap/tests/expected/new_Test_dM.cpp index 94c473061..4179921da 100644 --- a/wrap/tests/expected/new_Test_dM.cpp +++ b/wrap/tests/expected/new_Test_dM.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Dec-08 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Test_dM",nargout,nargin,2); diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index f4adb8eb6..d77d6ee77 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -1,5 +1,9 @@ // comments! +// set the default namespace +// location of namespace isn't significant +using namespace geometry; + class Point2 { Point2(); Point2(double x, double y); diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index d94834aed..d4277533a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -56,7 +56,7 @@ TEST( wrap, check_exception ) { string path = topdir + "/wrap/tests"; Module module(path.c_str(), "testWrap1",enable_verbose); - CHECK_EXCEPTION(module.matlab_code("actual", "", "mexa64", "-O5"), DependencyMissing); + CHECK_EXCEPTION(module.matlab_code("actual", "mexa64", "-O5"), DependencyMissing); } /* ************************************************************************* */ @@ -65,6 +65,10 @@ TEST( wrap, parse ) { Module module(header_path.c_str(), "geometry",enable_verbose); EXPECT_LONGS_EQUAL(3, module.classes.size()); + // check using declarations + EXPECT_LONGS_EQUAL(1, module.using_namespaces.size()); + EXPECT(assert_equal("geometry", module.using_namespaces.front())); + // check first class, Point2 { Class cls = module.classes.at(0); @@ -173,7 +177,7 @@ TEST( wrap, matlab_code_namespaces ) { // emit MATLAB code string exp_path = path + "/tests/expected_namespaces/"; string act_path = "actual_namespaces/"; - module.matlab_code("actual_namespaces", "", "mexa64", "-O5"); + module.matlab_code("actual_namespaces", "mexa64", "-O5"); EXPECT(files_equal(exp_path + "make_testNamespaces.m", act_path + "make_testNamespaces.m")); EXPECT(files_equal(exp_path + "Makefile" , act_path + "Makefile" )); @@ -192,7 +196,7 @@ TEST( wrap, matlab_code ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual", "", "mexa64", "-O5"); + module.matlab_code("actual", "mexa64", "-O5"); EXPECT(files_equal(path + "/tests/expected/@Point2/Point2.m" , "actual/@Point2/Point2.m" )); EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" )); diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 2dc93b49d..6877b29c2 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -15,8 +15,8 @@ **/ #include -#include +#include #include #include "utilities.h" @@ -42,12 +42,12 @@ string file_contents(const string& filename, bool skipheader) { } /* ************************************************************************* */ -bool assert_equal(const std::string& expected, const std::string& actual) { +bool assert_equal(const string& expected, const string& actual) { if (expected == actual) return true; printf("Not equal:\n"); - std::cout << "expected: [" << expected << "]\n"; - std::cout << "actual: [" << actual << "]" << std::endl; + cout << "expected: [" << expected << "]\n"; + cout << "actual: [" << actual << "]" << endl; return false; } @@ -82,13 +82,32 @@ void emit_header_comment(ofstream& ofs, const string& delimiter) { } /* ************************************************************************* */ -std::string maybe_shared_ptr(bool add, const std::string& type) { +string maybe_shared_ptr(bool add, const string& type) { string str = add? "shared_ptr<" : ""; str += type; if (add) str += ">"; return str; } +/* ************************************************************************* */ +void generateUsingNamespace(ofstream& ofs, const vector& using_namespaces) { + if (using_namespaces.empty()) return; + BOOST_FOREACH(const string& s, using_namespaces) + ofs << "using namespace " << s << ";" << endl; +} + +/* ************************************************************************* */ +void generateIncludes(std::ofstream& ofs, const std::string& class_name, + const std::vector& includes) { + ofs << "#include " << endl; + if (includes.empty()) // add a default include + ofs << "#include <" << class_name << ".h>" << endl; + else { + BOOST_FOREACH(const string& s, includes) + ofs << "#include <" << s << ">" << endl; + } +} + /* ************************************************************************* */ } // \namespace wrap diff --git a/wrap/utilities.h b/wrap/utilities.h index f43b9d9fb..f31dd7fd3 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -16,7 +16,9 @@ #pragma once +#include #include +#include #include namespace wrap { @@ -85,4 +87,15 @@ void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); // auxiliary function to wrap an argument into a shared_ptr template std::string maybe_shared_ptr(bool add, const std::string& type); +/** + * Creates the "using namespace [name];" declarations + */ +void generateUsingNamespace(std::ofstream& ofs, const std::vector& using_namespaces); + +/** + * Creates the #include statements + */ +void generateIncludes(std::ofstream& ofs, const std::string& class_name, + const std::vector& includes); + } // \namespace wrap diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 4fdd2d3fa..e2aaa6aed 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -35,7 +35,6 @@ void generate_matlab_toolbox(const string& mexExt, const string& interfacePath, const string& moduleName, const string& toolboxPath, - const string& nameSpace, const string& mexFlags) { // Parse interface file into class object @@ -43,7 +42,7 @@ void generate_matlab_toolbox(const string& mexExt, wrap::Module module(interfacePath, moduleName, true); // Then emit MATLAB code - module.matlab_code(toolboxPath,nameSpace,mexExt,mexFlags); + module.matlab_code(toolboxPath,mexExt,mexFlags); } /** @@ -58,10 +57,9 @@ int main(int argc, const char* argv[]) { cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; - cerr << " nameSpace : namespace to use, pass empty string if none" << endl; cerr << " [mexFlags] : extra flags for the mex command" << endl; } else - generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argv[5],argc==6 ? " " : argv[6]); + generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argc==5 ? " " : argv[5]); return 0; } From 216348622d422a305323f6f1ec4550a3c2d6ee72 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 9 Dec 2011 20:33:03 +0000 Subject: [PATCH 61/72] Updated myconfigure.matlab to install examples/tests/binary --- myconfigure.matlab | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/myconfigure.matlab b/myconfigure.matlab index 33addb665..a94644b77 100755 --- a/myconfigure.matlab +++ b/myconfigure.matlab @@ -1 +1,12 @@ -../configure --prefix=$HOME --with-toolbox=$HOME/toolbox --enable-build-toolbox CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install +../configure --prefix=$HOME \ + --with-toolbox=$HOME/toolbox \ + --enable-build-toolbox \ + --enable-install-matlab-tests \ + --enable-install-matlab-examples \ + --enable-install-wrap \ + --with-wrap=$HOME/bin \ + CFLAGS="-fno-inline -g -Wall" \ + CXXFLAGS="-fno-inline -g -Wall" \ + LDFLAGS="-fno-inline -g -Wall" \ + --disable-static \ + --disable-fast-install From 6a0da1519ae565ef580244ee2721659ea35b6a11 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 11 Dec 2011 21:09:07 +0000 Subject: [PATCH 62/72] Cleanup in wrap --- .cproject | 3 +++ wrap/Argument.cpp | 14 ++++++-------- wrap/Argument.h | 12 ++++++------ wrap/Class.cpp | 15 +++++++-------- wrap/Class.h | 12 ++++++------ wrap/Constructor.cpp | 13 ++++++------- wrap/Constructor.h | 8 ++++---- wrap/Method.cpp | 7 +++---- wrap/Method.h | 4 ++-- wrap/Module.cpp | 12 +++++------- wrap/Module.h | 2 +- wrap/ReturnValue.cpp | 8 ++++---- wrap/ReturnValue.h | 8 ++++---- wrap/StaticMethod.cpp | 7 +++---- wrap/StaticMethod.h | 4 ++-- wrap/utilities.cpp | 2 +- wrap/utilities.h | 2 +- 17 files changed, 64 insertions(+), 69 deletions(-) diff --git a/.cproject b/.cproject index 05b53a07a..35f7946a4 100644 --- a/.cproject +++ b/.cproject @@ -36,6 +36,9 @@ diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index e01da3db3..1610f98aa 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -25,9 +25,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Argument::matlab_unwrap(ofstream& ofs, - const string& matlabName) -{ +void Argument::matlab_unwrap(ofstream& ofs, const string& matlabName) const { ofs << " "; string cppType = qualifiedType("::"); @@ -53,14 +51,14 @@ void Argument::matlab_unwrap(ofstream& ofs, } /* ************************************************************************* */ -string Argument::qualifiedType(const string& delim) { +string Argument::qualifiedType(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; return result + type; } /* ************************************************************************* */ -string ArgumentList::types() { +string ArgumentList::types() const { string str; bool first=true; BOOST_FOREACH(Argument arg, *this) { @@ -70,7 +68,7 @@ string ArgumentList::types() { } /* ************************************************************************* */ -string ArgumentList::signature() { +string ArgumentList::signature() const { string str; BOOST_FOREACH(Argument arg, *this) str += arg.type[0]; @@ -78,7 +76,7 @@ string ArgumentList::signature() { } /* ************************************************************************* */ -string ArgumentList::names() { +string ArgumentList::names() const { string str; bool first=true; BOOST_FOREACH(Argument arg, *this) { @@ -88,7 +86,7 @@ string ArgumentList::names() { } /* ************************************************************************* */ -void ArgumentList::matlab_unwrap(ofstream& ofs, int start) { +void ArgumentList::matlab_unwrap(ofstream& ofs, int start) const { int index = start; BOOST_FOREACH(Argument arg, *this) { stringstream buf; diff --git a/wrap/Argument.h b/wrap/Argument.h index 07c11a5cc..cb5b1b8c8 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -32,18 +32,18 @@ struct Argument { is_const(false), is_ref(false), is_ptr(false) { } - std::string qualifiedType(const std::string& delim = ""); // adds namespaces to type + std::string qualifiedType(const std::string& delim = "") const; // adds namespaces to type /// MATLAB code generation, MATLAB to C++ - void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName); + void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName) const; }; /// Argument list struct ArgumentList: public std::vector { std::vector args; // why does it contain this? - std::string types(); - std::string signature(); - std::string names(); + std::string types() const; + std::string signature() const; + std::string names() const; // MATLAB code generation: @@ -52,7 +52,7 @@ struct ArgumentList: public std::vector { * @param ofs output stream * @param start initial index for input array, set to 1 for method */ - void matlab_unwrap(std::ofstream& ofs, int start = 0); // MATLAB to C++ + void matlab_unwrap(std::ofstream& ofs, int start = 0) const; // MATLAB to C++ }; } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 82245bd26..47e01f00b 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -27,7 +27,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Class::matlab_proxy(const string& classFile) { +void Class::matlab_proxy(const string& classFile) const { // open destination classFile ofstream ofs(classFile.c_str()); if(!ofs) throw CantOpenFile(classFile); @@ -57,7 +57,7 @@ void Class::matlab_proxy(const string& classFile) { } /* ************************************************************************* */ -void Class::matlab_constructors(const string& toolboxPath, const vector& using_namespaces) { +void Class::matlab_constructors(const string& toolboxPath, const vector& using_namespaces) const { BOOST_FOREACH(Constructor c, constructors) { c.matlab_mfile (toolboxPath, qualifiedName()); c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes); @@ -65,7 +65,7 @@ void Class::matlab_constructors(const string& toolboxPath, const vector& } /* ************************************************************************* */ -void Class::matlab_methods(const string& classPath, const vector& using_namespaces) { +void Class::matlab_methods(const string& classPath, const vector& using_namespaces) const { string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(Method m, methods) { m.matlab_mfile (classPath); @@ -74,9 +74,9 @@ void Class::matlab_methods(const string& classPath, const vector& using_ } /* ************************************************************************* */ -void Class::matlab_static_methods(const string& toolboxPath, const vector& using_namespaces) { +void Class::matlab_static_methods(const string& toolboxPath, const vector& using_namespaces) const { string matlabName = qualifiedName(), cppName = qualifiedName("::"); - BOOST_FOREACH(StaticMethod& m, static_methods) { + BOOST_FOREACH(const StaticMethod& m, static_methods) { m.matlab_mfile (toolboxPath, qualifiedName()); m.matlab_wrapper(toolboxPath, name, matlabName, cppName, using_namespaces, includes); } @@ -85,8 +85,7 @@ void Class::matlab_static_methods(const string& toolboxPath, const vector& using_namespaces); ///< emit constructor wrappers + const std::vector& using_namespaces) const; ///< emit constructor wrappers void matlab_methods(const std::string& classPath, - const std::vector& using_namespaces); ///< emit method wrappers + const std::vector& using_namespaces) const; ///< emit method wrappers void matlab_static_methods(const std::string& classPath, - const std::vector& using_namespaces); ///< emit static method wrappers + const std::vector& using_namespaces) const; ///< emit static method wrappers void matlab_make_fragment(std::ofstream& ofs, const std::string& toolboxPath, - const std::string& mexFlags); ///< emit make fragment for global make script - void makefile_fragment(std::ofstream& ofs); ///< emit makefile fragment + const std::string& mexFlags) const; ///< emit make fragment for global make script + void makefile_fragment(std::ofstream& ofs) const; ///< emit makefile fragment std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter }; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 04a873547..a822f722f 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -26,13 +26,13 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -string Constructor::matlab_wrapper_name(const string& className) { +string Constructor::matlab_wrapper_name(const string& className) const { string str = "new_" + className + "_" + args.signature(); return str; } /* ************************************************************************* */ -void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) { +void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) const { ofs << " if nargin == " << args.size() << ", obj.self = " << matlab_wrapper_name(className) << "("; bool first = true; @@ -45,7 +45,7 @@ void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) } /* ************************************************************************* */ -void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) { +void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) const { string matlabName = matlab_wrapper_name(qualifiedMatlabName); @@ -56,7 +56,7 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifie if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "%"); + generateHeaderComment(ofs, "%"); ofs << "function result = " << matlabName << "(obj"; if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; @@ -71,8 +71,7 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifie void Constructor::matlab_wrapper(const string& toolboxPath, const string& cppClassName, const string& matlabClassName, - const vector& using_namespaces, const vector& includes) -{ + const vector& using_namespaces, const vector& includes) const { string matlabName = matlab_wrapper_name(matlabClassName); // open destination wrapperFile @@ -82,7 +81,7 @@ void Constructor::matlab_wrapper(const string& toolboxPath, if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "//"); + generateHeaderComment(ofs, "//"); generateIncludes(ofs, name, includes); generateUsingNamespace(ofs, using_namespaces); diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 5409ed1f6..b9a44e2cc 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -42,21 +42,21 @@ struct Constructor { // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m /// wrapper name - std::string matlab_wrapper_name(const std::string& className); + std::string matlab_wrapper_name(const std::string& className) const; /// proxy class fragment - void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className); + void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className) const; /// m-file void matlab_mfile(const std::string& toolboxPath, - const std::string& qualifiedMatlabName); + const std::string& qualifiedMatlabName) const; /// cpp wrapper void matlab_wrapper(const std::string& toolboxPath, const std::string& cppClassName, const std::string& matlabClassName, const std::vector& using_namespaces, - const std::vector& includes); + const std::vector& includes) const; }; } // \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index b9f10a9d4..0d9ba98e2 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Method::matlab_mfile(const string& classPath) { +void Method::matlab_mfile(const string& classPath) const { // open destination m-file string wrapperFile = classPath + "/" + name + ".m"; @@ -52,8 +52,7 @@ void Method::matlab_wrapper(const string& classPath, const string& className, const string& cppClassName, const string& matlabClassName, - const vector& using_namespaces, const std::vector& includes) -{ + const vector& using_namespaces, const std::vector& includes) const { // open destination wrapperFile string wrapperFile = classPath + "/" + name + ".cpp"; ofstream ofs(wrapperFile.c_str()); @@ -63,7 +62,7 @@ void Method::matlab_wrapper(const string& classPath, // generate code // header - wrap::emit_header_comment(ofs, "//"); + generateHeaderComment(ofs, "//"); generateIncludes(ofs, className, includes); generateUsingNamespace(ofs, using_namespaces); diff --git a/wrap/Method.h b/wrap/Method.h index 57bd1c043..bcfac82e3 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -42,13 +42,13 @@ struct Method { // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 - void matlab_mfile(const std::string& classPath); ///< m-file + void matlab_mfile(const std::string& classPath) const; ///< m-file void matlab_wrapper(const std::string& classPath, const std::string& className, const std::string& cppClassName, const std::string& matlabClassname, const std::vector& using_namespaces, - const std::vector& includes); ///< cpp wrapper + const std::vector& includes) const; ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 2fd8c9747..120b89f10 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -232,7 +232,7 @@ void verifyArguments(const vector& validArgs, const vector& vt) { BOOST_FOREACH(const T& t, vt) { BOOST_FOREACH(Argument arg, t.args) { string fullType = arg.qualifiedType("::"); - if(std::find(validArgs.begin(), validArgs.end(), fullType) + if(find(validArgs.begin(), validArgs.end(), fullType) == validArgs.end()) throw DependencyMissing(fullType, t.name); } @@ -241,9 +241,7 @@ void verifyArguments(const vector& validArgs, const vector& vt) { /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, - const string& mexExt, - const string& mexFlags) -{ + const string& mexExt, const string& mexFlags) const { string installCmd = "install -d " + toolboxPath; system(installCmd.c_str()); @@ -258,7 +256,7 @@ void Module::matlab_code(const string& toolboxPath, if(!make_ofs) throw CantOpenFile(makeFile); if (verbose) cerr << "generating " << matlabMakeFile << endl; - emit_header_comment(ofs,"%"); + generateHeaderComment(ofs,"%"); ofs << "echo on" << endl << endl; ofs << "toolboxpath = mfilename('fullpath');" << endl; ofs << "delims = find(toolboxpath == '/');" << endl; @@ -267,13 +265,13 @@ void Module::matlab_code(const string& toolboxPath, ofs << "addpath(toolboxpath);" << endl << endl; if (verbose) cerr << "generating " << makeFile << endl; - emit_header_comment(make_ofs,"#"); + generateHeaderComment(make_ofs,"#"); make_ofs << "\nMEX = mex\n"; make_ofs << "MEXENDING = " << mexExt << "\n"; make_ofs << "mex_flags = " << mexFlags << "\n\n"; //Dependency check list - std::vector validArgs; + vector validArgs; validArgs.push_back("string"); validArgs.push_back("int"); validArgs.push_back("bool"); diff --git a/wrap/Module.h b/wrap/Module.h index 3c294f380..ec057b644 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -41,7 +41,7 @@ struct Module { /// MATLAB code generation: void matlab_code(const std::string& path, const std::string& mexExt, - const std::string& mexFlags); + const std::string& mexFlags) const; }; } // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index c10c173d7..38d85d074 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -14,7 +14,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -string ReturnValue::return_type(bool add_ptr, pairing p) { +string ReturnValue::return_type(bool add_ptr, pairing p) const { if (p==pair && isPair) { string str = "pair< " + maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::")) + ", " + @@ -30,21 +30,21 @@ string ReturnValue::matlab_returnType() const { } /* ************************************************************************* */ -string ReturnValue::qualifiedType1(const string& delim) { +string ReturnValue::qualifiedType1(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim; return result + type1; } /* ************************************************************************* */ -string ReturnValue::qualifiedType2(const string& delim) { +string ReturnValue::qualifiedType2(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim; return result + type2; } /* ************************************************************************* */ -void ReturnValue::wrap_result(std::ostream& ofs) { +void ReturnValue::wrap_result(ostream& ofs) const { string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(); string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(); diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index ba606ad9c..eb2406d80 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -39,14 +39,14 @@ struct ReturnValue { arg1, arg2, pair } pairing; - std::string return_type(bool add_ptr, pairing p); + std::string return_type(bool add_ptr, pairing p) const; - std::string qualifiedType1(const std::string& delim = ""); - std::string qualifiedType2(const std::string& delim = ""); + std::string qualifiedType1(const std::string& delim = "") const; + std::string qualifiedType2(const std::string& delim = "") const; std::string matlab_returnType() const; - void wrap_result(std::ostream& ofs); + void wrap_result(std::ostream& ofs) const; }; diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 02a2aaf76..ff4d31592 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) { +void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) const { // open destination m-file string full_name = className + "_" + name; @@ -52,8 +52,7 @@ void StaticMethod::matlab_mfile(const string& toolboxPath, const string& classNa void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, const string& matlabClassName, const string& cppClassName, const vector& using_namespaces, - const std::vector& includes) -{ + const vector& includes) const { // open destination wrapperFile string full_name = matlabClassName + "_" + name; string wrapperFile = toolboxPath + "/" + full_name + ".cpp"; @@ -64,7 +63,7 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& class // generate code // header - wrap::emit_header_comment(ofs, "//"); + generateHeaderComment(ofs, "//"); generateIncludes(ofs, className, includes); generateUsingNamespace(ofs, using_namespaces); diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 429e24183..f9cc14af7 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -44,12 +44,12 @@ struct StaticMethod { // NOTE: static functions are not inside the class, and // are created with [ClassName]_[FunctionName]() format - void matlab_mfile(const std::string& toolboxPath, const std::string& className); ///< m-file + void matlab_mfile(const std::string& toolboxPath, const std::string& className) const; ///< m-file void matlab_wrapper(const std::string& toolboxPath, const std::string& className, const std::string& matlabClassName, const std::string& cppClassName, const std::vector& using_namespaces, - const std::vector& includes); ///< cpp wrapper + const std::vector& includes) const; ///< cpp wrapper }; } // \namespace wrap diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 6877b29c2..ebf1ad020 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -76,7 +76,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) } /* ************************************************************************* */ -void emit_header_comment(ofstream& ofs, const string& delimiter) { +void generateHeaderComment(ofstream& ofs, const string& delimiter) { date today = day_clock::local_day(); ofs << delimiter << " automatically generated by wrap on " << today << endl; } diff --git a/wrap/utilities.h b/wrap/utilities.h index f31dd7fd3..3837c2b95 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -82,7 +82,7 @@ bool assert_equal(const std::string& expected, const std::string& actual); /** * emit a header at the top of generated files */ -void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); +void generateHeaderComment(std::ofstream& ofs, const std::string& delimiter); // auxiliary function to wrap an argument into a shared_ptr template std::string maybe_shared_ptr(bool add, const std::string& type); From 3c8a7a29f9a49b78b0fa097649a4ca193aa8802e Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 12 Dec 2011 03:57:48 +0000 Subject: [PATCH 63/72] updated visual SLAM examples to follow correct coordinate system conventions. The ISAM example now also uses the same input files as the general SFM example. --- examples/Data/calib.txt | 2 +- examples/Data/measurementsISAM.txt | 12 ------- examples/Data/posesISAM.txt | 12 ------- examples/Data/ttpy10.pose | 8 ++--- examples/Data/ttpy100.pose | 8 ++--- examples/Data/ttpy20.pose | 8 ++--- examples/Data/ttpy30.pose | 8 ++--- examples/Data/ttpy40.pose | 8 ++--- examples/Data/ttpy50.pose | 8 ++--- examples/Data/ttpy60.pose | 8 ++--- examples/Data/ttpy70.pose | 8 ++--- examples/Data/ttpy80.pose | 8 ++--- examples/Data/ttpy90.pose | 8 ++--- examples/vSLAMexample/vISAMexample.cpp | 24 +++++++------ examples/vSLAMexample/vSLAMutils.cpp | 49 +++++--------------------- examples/vSLAMexample/vSLAMutils.h | 4 +-- 16 files changed, 64 insertions(+), 119 deletions(-) delete mode 100644 examples/Data/measurementsISAM.txt delete mode 100644 examples/Data/posesISAM.txt diff --git a/examples/Data/calib.txt b/examples/Data/calib.txt index 8eeb7e656..cb8b77384 100644 --- a/examples/Data/calib.txt +++ b/examples/Data/calib.txt @@ -1 +1 @@ -800 600 -1119.61507797 1119.61507797 399.5 299.5 +800 600 1119.61507797 1119.61507797 399.5 299.5 diff --git a/examples/Data/measurementsISAM.txt b/examples/Data/measurementsISAM.txt deleted file mode 100644 index 7325c7dd9..000000000 --- a/examples/Data/measurementsISAM.txt +++ /dev/null @@ -1,12 +0,0 @@ -10 -ttpy10.feat -ttpy20.feat -ttpy30.feat -ttpy40.feat -ttpy50.feat -ttpy60.feat -ttpy70.feat -ttpy80.feat -ttpy90.feat -ttpy100.feat - diff --git a/examples/Data/posesISAM.txt b/examples/Data/posesISAM.txt deleted file mode 100644 index ebe39c6f2..000000000 --- a/examples/Data/posesISAM.txt +++ /dev/null @@ -1,12 +0,0 @@ -10 -ttpy10.pose -ttpy20.pose -ttpy30.pose -ttpy40.pose -ttpy50.pose -ttpy60.pose -ttpy70.pose -ttpy80.pose -ttpy90.pose -ttpy100.pose - diff --git a/examples/Data/ttpy10.pose b/examples/Data/ttpy10.pose index ee0f6231a..b9226c723 100644 --- a/examples/Data/ttpy10.pose +++ b/examples/Data/ttpy10.pose @@ -1,4 +1,4 @@ -0.939693 0.34202 0 0 --0.241845 0.664463 -0.707107 0 --0.241845 0.664463 0.707107 0 -34.202 -93.9693 100 1 +0.93969 0.24185 -0.24185 34.202 +0.34202 -0.66446 0.66446 -93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy100.pose b/examples/Data/ttpy100.pose index 4225e3d4e..cbd21233f 100644 --- a/examples/Data/ttpy100.pose +++ b/examples/Data/ttpy100.pose @@ -1,4 +1,4 @@ --0.939693 -0.34202 0 0 -0.241845 -0.664463 -0.707107 0 -0.241845 -0.664463 0.707107 0 --34.202 93.9693 100 1 +-0.93969 -0.24185 0.24185 -34.202 +-0.34202 0.66446 -0.66446 93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy20.pose b/examples/Data/ttpy20.pose index f8b252855..bba2606d0 100644 --- a/examples/Data/ttpy20.pose +++ b/examples/Data/ttpy20.pose @@ -1,4 +1,4 @@ -0.766044 0.642788 0 0 --0.454519 0.541675 -0.707107 0 --0.454519 0.541675 0.707107 0 -64.2788 -76.6044 100 1 +0.76604 0.45452 -0.45452 64.279 +0.64279 -0.54168 0.54168 -76.604 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy30.pose b/examples/Data/ttpy30.pose index f7ad9f674..737e391f2 100644 --- a/examples/Data/ttpy30.pose +++ b/examples/Data/ttpy30.pose @@ -1,4 +1,4 @@ -0.5 0.866025 0 0 --0.612372 0.353553 -0.707107 0 --0.612372 0.353553 0.707107 0 -86.6025 -50 100 1 +0.5 0.61237 -0.61237 86.603 +0.86603 -0.35355 0.35355 -50 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy40.pose b/examples/Data/ttpy40.pose index e9c9cd251..4e241c130 100644 --- a/examples/Data/ttpy40.pose +++ b/examples/Data/ttpy40.pose @@ -1,4 +1,4 @@ -0.173648 0.984808 0 0 --0.696364 0.122788 -0.707107 0 --0.696364 0.122788 0.707107 0 -98.4808 -17.3648 100 1 +0.17365 0.69636 -0.69636 98.481 +0.98481 -0.12279 0.12279 -17.365 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy50.pose b/examples/Data/ttpy50.pose index 9c84b54f3..5a57b10ef 100644 --- a/examples/Data/ttpy50.pose +++ b/examples/Data/ttpy50.pose @@ -1,4 +1,4 @@ --0.173648 0.984808 0 0 --0.696364 -0.122788 -0.707107 0 --0.696364 -0.122788 0.707107 0 -98.4808 17.3648 100 1 +-0.17365 0.69636 -0.69636 98.481 +0.98481 0.12279 -0.12279 17.365 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy60.pose b/examples/Data/ttpy60.pose index 9c0a8c942..4e5455a0d 100644 --- a/examples/Data/ttpy60.pose +++ b/examples/Data/ttpy60.pose @@ -1,4 +1,4 @@ --0.5 0.866025 0 0 --0.612372 -0.353553 -0.707107 0 --0.612372 -0.353553 0.707107 0 -86.6025 50 100 1 +-0.5 0.61237 -0.61237 86.603 +0.86603 0.35355 -0.35355 50 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy70.pose b/examples/Data/ttpy70.pose index 0c2254de6..608e9ff7c 100644 --- a/examples/Data/ttpy70.pose +++ b/examples/Data/ttpy70.pose @@ -1,4 +1,4 @@ --0.766044 0.642788 0 0 --0.454519 -0.541675 -0.707107 0 --0.454519 -0.541675 0.707107 0 -64.2788 76.6044 100 1 +-0.76604 0.45452 -0.45452 64.279 +0.64279 0.54168 -0.54168 76.604 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy80.pose b/examples/Data/ttpy80.pose index 9d6491304..8a6ae4cd1 100644 --- a/examples/Data/ttpy80.pose +++ b/examples/Data/ttpy80.pose @@ -1,4 +1,4 @@ --0.939693 0.34202 0 0 --0.241845 -0.664463 -0.707107 0 --0.241845 -0.664463 0.707107 0 -34.202 93.9693 100 1 +-0.93969 0.24185 -0.24185 34.202 +0.34202 0.66446 -0.66446 93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy90.pose b/examples/Data/ttpy90.pose index b51c1a58f..863f71fea 100644 --- a/examples/Data/ttpy90.pose +++ b/examples/Data/ttpy90.pose @@ -1,4 +1,4 @@ --1 0 0 0 -0 -0.707107 -0.707107 0 -0 -0.707107 0.707107 0 -0 100 100 1 +-1 -0 0 0 +0 0.70711 -0.70711 100 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index ed3a68ba8..bd35e3636 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace boost; // Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h @@ -40,8 +41,8 @@ using namespace boost; /* ************************************************************************* */ #define CALIB_FILE "calib.txt" #define LANDMARKS_FILE "landmarks.txt" -#define POSES_FILE "posesISAM.txt" -#define MEASUREMENTS_FILE "measurementsISAM.txt" +#define POSES_FILE "poses.txt" +#define MEASUREMENTS_FILE "measurements.txt" // Base data folder string g_dataFolder; @@ -49,8 +50,8 @@ string g_dataFolder; // Store groundtruth values, read from files shared_ptrK g_calib; map g_landmarks; // map: -std::vector g_poses; // prior camera poses at each frame -std::vector > g_measurements; // feature sets detected at each frame +map g_poses; // map: +std::map > g_measurements; // feature sets detected at each frame // Noise models SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); @@ -69,7 +70,7 @@ void readAllDataISAM() { g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE); // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes. - g_poses = readPosesISAM(g_dataFolder, POSES_FILE); + g_poses = readPoses(g_dataFolder, POSES_FILE); // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark. g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE); @@ -80,7 +81,7 @@ void readAllDataISAM() { * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, - int pose_id, Pose3& pose, std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { + int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements newFactors = shared_ptr (new Graph()); @@ -121,16 +122,19 @@ int main(int argc, char* argv[]) { g_dataFolder = string(argv[1]) + "/"; readAllDataISAM(); - // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates + // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); - // At each frame i with new camera pose and new set of measurements associated with it, + // At each frame (poseId) with new camera pose and set of associated measurements, // create a graph of new factors and update ISAM - for (size_t i = 0; i < g_measurements.size(); i++) { + typedef std::map > FeatureMap; + BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { + const int poseId = features.first; shared_ptr newFactors; shared_ptr initialValues; - createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib); + createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], + features.second, measurementSigma, g_calib); isam.update(*newFactors, *initialValues); visualSLAM::Values currentEstimate = isam.estimate(); diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp index ae9042f2a..1c3912f2d 100644 --- a/examples/vSLAMexample/vSLAMutils.cpp +++ b/examples/vSLAMexample/vSLAMutils.cpp @@ -46,10 +46,6 @@ std::map readLandMarks(const std::string& landmarkFile) { } /* ************************************************************************* */ -/** - * Read pose from file, output by Panda3D. - * Warning: row major!!! - */ gtsam::Pose3 readPose(const char* Fn) { ifstream poseFile(Fn); if (!poseFile) { @@ -62,18 +58,7 @@ gtsam::Pose3 readPose(const char* Fn) { poseFile >> v[i]; poseFile.close(); - // Because panda3d's camera is z-up, y-view, - // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule - //... similar to OpenGL's camera - for (int i = 0; i<3; i++) { - float t = v[4+i]; - v[4+i] = v[8+i]; - v[8+i] = -t; - } - - ::Vector vec = Vector_(16, v); - - Matrix T = Matrix_(4,4, vec); // column order !!! + Matrix T = Matrix_(4,4, v); // row order !!! Pose3 pose(T); return pose; @@ -166,28 +151,7 @@ std::vector readAllMeasurements(const std::string& baseFolder, const } /* ************************************************************************* */ -std::vector readPosesISAM(const std::string& baseFolder, const std::string& posesFn) { - ifstream posesFile((baseFolder+posesFn).c_str()); - if (!posesFile) { - cout << "Cannot read all pose ISAM file: " << posesFn << endl; - exit(0); - } - int numPoses; - posesFile >> numPoses; - vector poses; - for (int i = 0; i> poseFileName; - - Pose3 pose = readPose((baseFolder+poseFileName).c_str()); - poses.push_back(pose); - } - - return poses; -} - -/* ************************************************************************* */ -std::vector > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) { +std::map > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) { ifstream measurementsFile((baseFolder+measurementsFn).c_str()); if (!measurementsFile) { cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl; @@ -196,13 +160,16 @@ std::vector > readAllMeasurementsISAM(const std::string& int numPoses; measurementsFile >> numPoses; - std::vector > allFeatures; + std::map > allFeatures; for (int i = 0; i> poseId; + string featureFileName; measurementsFile >> featureFileName; - vector features = readFeatures(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM - allFeatures.push_back(features); + vector features = readFeatures(poseId, (baseFolder+featureFileName).c_str()); + allFeatures[poseId] = features; } return allFeatures; diff --git a/examples/vSLAMexample/vSLAMutils.h b/examples/vSLAMexample/vSLAMutils.h index f1483cd70..52d492224 100644 --- a/examples/vSLAMexample/vSLAMutils.h +++ b/examples/vSLAMexample/vSLAMutils.h @@ -29,11 +29,9 @@ std::map readLandMarks(const std::string& landmarkFile); gtsam::Pose3 readPose(const char* poseFn); std::map readPoses(const std::string& baseFolder, const std::string& posesFN); -std::vector readPosesISAM(const std::string& baseFolder, const std::string& posesFN); - gtsam::shared_ptrK readCalibData(const std::string& calibFn); std::vector readFeatureFile(const char* filename); std::vector readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn); -std::vector< std::vector > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); +std::map > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); From 7ae564ed712a162de15357d19c6e9de0b140a9ad Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 12 Dec 2011 23:10:24 +0000 Subject: [PATCH 64/72] Upgrade Eigen to 3.0.4 --- gtsam/3rdparty/Eigen/CMakeLists.txt | 10 ++- gtsam/3rdparty/Eigen/Eigen/Core | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h | 4 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h | 4 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h | 4 - .../Eigen/Eigen/src/Core/PlainObjectBase.h | 23 +++++- .../Eigen/Eigen/src/Core/arch/NEON/Complex.h | 4 +- .../Eigen/src/Core/arch/NEON/PacketMath.h | 56 ++++++++----- .../Core/products/GeneralBlockPanelKernel.h | 4 +- .../Eigen/Eigen/src/Core/util/Macros.h | 35 ++++---- .../Eigen/Eigen/src/Core/util/Memory.h | 54 +++++++++---- .../Eigen/Eigen/src/Core/util/XprHelper.h | 3 +- .../src/Eigenvalues/SelfAdjointEigenSolver.h | 16 ++-- .../Eigen/Eigen/src/Geometry/Quaternion.h | 21 +++-- gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h | 10 +-- .../Eigen/bench/btl/generic_bench/btl.hh | 2 +- gtsam/3rdparty/Eigen/doc/CMakeLists.txt | 5 ++ gtsam/3rdparty/Eigen/doc/Overview.dox | 2 +- .../TutorialLinAlgSelfAdjointEigenSolver.cpp | 3 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 2 +- gtsam/3rdparty/Eigen/test/geo_quaternion.cpp | 5 +- gtsam/3rdparty/Eigen/test/main.h | 2 +- gtsam/3rdparty/Eigen/test/sizeoverflow.cpp | 81 +++++++++++++++++++ .../Eigen/unsupported/Eigen/MPRealSupport | 3 +- .../Eigen/unsupported/doc/Overview.dox | 11 ++- 26 files changed, 265 insertions(+), 103 deletions(-) create mode 100644 gtsam/3rdparty/Eigen/test/sizeoverflow.cpp diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index c66a14e0e..4a6a849e9 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -283,11 +283,17 @@ if(EIGEN_BUILD_PKGCONFIG) STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}") message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib") FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search}) - message(STATUS "found ${pkg_config_libdir}/pkgconfig" ) + if(pkg_config_libdir) + SET(pkg_config_install_dir ${pkg_config_libdir}) + message(STATUS "found ${pkg_config_libdir}/pkgconfig" ) + else(pkg_config_libdir) + SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share) + message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" ) + endif(pkg_config_libdir) configure_file(eigen3.pc.in eigen3.pc) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc - DESTINATION ${pkg_config_libdir}/pkgconfig + DESTINATION ${pkg_config_install_dir}/pkgconfig ) endif(EIGEN_BUILD_PKGCONFIG) diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index 6e855427c..a5025e37e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -167,7 +167,7 @@ #include #endif -#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_NO_EXCEPTIONS) +#if defined(_CPPUNWIND) || defined(__EXCEPTIONS) #define EIGEN_EXCEPTIONS #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index a3a2167ad..a11fb1b53 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -68,10 +68,8 @@ class Array friend struct internal::conservative_resize_like_impl; using Base::m_storage; + public: - enum { NeedsToAlign = (!(Options&DontAlign)) - && SizeAtCompileTime!=Dynamic && ((static_cast(sizeof(Scalar))*SizeAtCompileTime)%16)==0 }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) using Base::base; using Base::coeff; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 2b251bc2c..fe3e449bf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -94,7 +94,7 @@ struct traits MaskPacketAccessBit = (InnerSize == Dynamic || (InnerSize % packet_traits::size) == 0) && (InnerStrideAtCompileTime == 1) ? PacketAccessBit : 0, - MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && ((OuterStrideAtCompileTime % packet_traits::size) == 0)) ? AlignedBit : 0, + MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * sizeof(Scalar)) % 16) == 0)) ? AlignedBit : 0, FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0, FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index c23bcbfdc..9426e2d24 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -170,8 +170,8 @@ template class MapBase EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(internal::traits::Flags&PacketAccessBit, internal::inner_stride_at_compile_time::ret==1), PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); - eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % (sizeof(Scalar)*internal::packet_traits::size)) == 0) - && "data is not aligned"); + eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % 16) == 0) + && "data is not aligned"); } PointerType m_data; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index 44de22cb4..982c9256a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -153,10 +153,6 @@ class Matrix typedef typename Base::PlainObject PlainObject; - enum { NeedsToAlign = (!(Options&DontAlign)) - && SizeAtCompileTime!=Dynamic && ((static_cast(sizeof(Scalar))*SizeAtCompileTime)%16)==0 }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) - using Base::base; using Base::coeffRef; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index c70db9247..612254e9d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -34,6 +34,19 @@ namespace internal { +template +EIGEN_ALWAYS_INLINE void check_rows_cols_for_overflow(Index rows, Index cols) +{ + // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242 + // we assume Index is signed + Index max_index = (size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed + bool error = (rows < 0 || cols < 0) ? true + : (rows == 0 || cols == 0) ? false + : (rows > max_index / cols); + if (error) + throw_std_bad_alloc(); +} + template (Derived::IsVectorAtCompileTime)> struct conservative_resize_like_impl; template struct matrix_swap_impl; @@ -84,14 +97,12 @@ class PlainObjectBase : public internal::dense_xpr_base::type template struct StridedConstMapType { typedef Eigen::Map type; }; template struct StridedAlignedMapType { typedef Eigen::Map type; }; template struct StridedConstAlignedMapType { typedef Eigen::Map type; }; - protected: DenseStorage m_storage; public: - enum { NeedsToAlign = (!(Options&DontAlign)) - && SizeAtCompileTime!=Dynamic && ((static_cast(sizeof(Scalar))*SizeAtCompileTime)%16)==0 }; + enum { NeedsToAlign = SizeAtCompileTime != Dynamic && (internal::traits::Flags & AlignedBit) != 0 }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) Base& base() { return *static_cast(this); } @@ -200,11 +211,13 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resize(Index rows, Index cols) { #ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO + internal::check_rows_cols_for_overflow(rows, cols); Index size = rows*cols; bool size_changed = size != this->size(); m_storage.resize(size, rows, cols); if(size_changed) EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED #else + internal::check_rows_cols_for_overflow(rows, cols); m_storage.resize(rows*cols, rows, cols); #endif } @@ -273,6 +286,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { const OtherDerived& other = _other.derived(); + internal::check_rows_cols_for_overflow(other.rows(), other.cols()); const Index othersize = other.rows()*other.cols(); if(RowsAtCompileTime == 1) { @@ -417,6 +431,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { _check_template_params(); + internal::check_rows_cols_for_overflow(other.derived().rows(), other.derived().cols()); Base::operator=(other.derived()); } @@ -581,6 +596,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type { eigen_assert(rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); + internal::check_rows_cols_for_overflow(rows, cols); m_storage.resize(rows*cols,rows,cols); EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED } @@ -638,6 +654,7 @@ struct internal::conservative_resize_like_impl if ( ( Derived::IsRowMajor && _this.cols() == cols) || // row-major and we change only the number of rows (!Derived::IsRowMajor && _this.rows() == rows) ) // column-major and we change only the number of columns { + internal::check_rows_cols_for_overflow(rows, cols); _this.derived().m_storage.conservativeResize(rows*cols,rows,cols); } else diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h index 8e55548c9..212887184 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h @@ -27,8 +27,8 @@ namespace internal { -static uint32x4_t p4ui_CONJ_XOR = { 0x00000000, 0x80000000, 0x00000000, 0x80000000 }; -static uint32x2_t p2ui_CONJ_XOR = { 0x00000000, 0x80000000 }; +static uint32x4_t p4ui_CONJ_XOR = EIGEN_INIT_NEON_PACKET4(0x00000000, 0x80000000, 0x00000000, 0x80000000); +static uint32x2_t p2ui_CONJ_XOR = EIGEN_INIT_NEON_PACKET2(0x00000000, 0x80000000); //---------- float ---------- struct Packet2cf diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h index 478ef8038..6c7cd1590 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h @@ -52,6 +52,16 @@ typedef uint32x4_t Packet4ui; #define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \ const Packet4i p4i_##NAME = pset1(X) +#if defined(__llvm__) && !defined(__clang__) + //Special treatment for Apple's llvm-gcc, its NEON packet types are unions + #define EIGEN_INIT_NEON_PACKET2(X, Y) {{X, Y}} + #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {{X, Y, Z, W}} +#else + //Default initializer for packets + #define EIGEN_INIT_NEON_PACKET2(X, Y) {X, Y} + #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W} +#endif + #ifndef __pld #define __pld(x) asm volatile ( " pld [%[addr]]\n" :: [addr] "r" (x) : "cc" ); #endif @@ -84,7 +94,7 @@ template<> struct packet_traits : default_packet_traits }; }; -#if EIGEN_GNUC_AT_MOST(4,4) +#if EIGEN_GNUC_AT_MOST(4,4) && !defined(__llvm__) // workaround gcc 4.2, 4.3 and 4.4 compilatin issue EIGEN_STRONG_INLINE float32x4_t vld1q_f32(const float* x) { return ::vld1q_f32((const float32_t*)x); } EIGEN_STRONG_INLINE float32x2_t vld1_f32 (const float* x) { return ::vld1_f32 ((const float32_t*)x); } @@ -100,12 +110,12 @@ template<> EIGEN_STRONG_INLINE Packet4i pset1(const int& from) { template<> EIGEN_STRONG_INLINE Packet4f plset(const float& a) { - Packet4f countdown = { 0, 1, 2, 3 }; + Packet4f countdown = EIGEN_INIT_NEON_PACKET4(0, 1, 2, 3); return vaddq_f32(pset1(a), countdown); } template<> EIGEN_STRONG_INLINE Packet4i plset(const int& a) { - Packet4i countdown = { 0, 1, 2, 3 }; + Packet4i countdown = EIGEN_INIT_NEON_PACKET4(0, 1, 2, 3); return vaddq_s32(pset1(a), countdown); } @@ -395,25 +405,29 @@ template<> EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) return s[0]; } -template -struct palign_impl -{ - EIGEN_STRONG_INLINE static void run(Packet4f& first, const Packet4f& second) - { - if (Offset!=0) - first = vextq_f32(first, second, Offset); - } -}; +// this PALIGN_NEON business is to work around a bug in LLVM Clang 3.0 causing incorrect compilation errors, +// see bug 347 and this LLVM bug: http://llvm.org/bugs/show_bug.cgi?id=11074 +#define PALIGN_NEON(Offset,Type,Command) \ +template<>\ +struct palign_impl\ +{\ + EIGEN_STRONG_INLINE static void run(Type& first, const Type& second)\ + {\ + if (Offset!=0)\ + first = Command(first, second, Offset);\ + }\ +};\ -template -struct palign_impl -{ - EIGEN_STRONG_INLINE static void run(Packet4i& first, const Packet4i& second) - { - if (Offset!=0) - first = vextq_s32(first, second, Offset); - } -}; +PALIGN_NEON(0,Packet4f,vextq_f32) +PALIGN_NEON(1,Packet4f,vextq_f32) +PALIGN_NEON(2,Packet4f,vextq_f32) +PALIGN_NEON(3,Packet4f,vextq_f32) +PALIGN_NEON(0,Packet4i,vextq_s32) +PALIGN_NEON(1,Packet4i,vextq_s32) +PALIGN_NEON(2,Packet4i,vextq_s32) +PALIGN_NEON(3,Packet4i,vextq_s32) + +#undef PALIGN_NEON } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index 6f3f27170..c601f4771 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -118,14 +118,14 @@ inline void computeProductBlockingSizes(std::ptrdiff_t& k, std::ptrdiff_t& m, st // FIXME (a bit overkill maybe ?) template struct gebp_madd_selector { - EIGEN_STRONG_INLINE EIGEN_ALWAYS_INLINE_ATTRIB static void run(const CJ& cj, A& a, B& b, C& c, T& /*t*/) + EIGEN_ALWAYS_INLINE static void run(const CJ& cj, A& a, B& b, C& c, T& /*t*/) { c = cj.pmadd(a,b,c); } }; template struct gebp_madd_selector { - EIGEN_STRONG_INLINE EIGEN_ALWAYS_INLINE_ATTRIB static void run(const CJ& cj, T& a, T& b, T& c, T& t) + EIGEN_ALWAYS_INLINE static void run(const CJ& cj, T& a, T& b, T& c, T& t) { t = b; t = cj.pmul(a,t); c = padd(c,t); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 2a647cfda..e6c5ef425 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -28,7 +28,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 0 -#define EIGEN_MINOR_VERSION 3 +#define EIGEN_MINOR_VERSION 4 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -45,7 +45,7 @@ #define EIGEN_GNUC_AT_MOST(x,y) 0 #endif -#if EIGEN_GNUC_AT_MOST(4,3) +#if EIGEN_GNUC_AT_MOST(4,3) && !defined(__clang__) // see bug 89 #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0 #else @@ -130,31 +130,34 @@ #define EIGEN_MAKESTRING2(a) #a #define EIGEN_MAKESTRING(a) EIGEN_MAKESTRING2(a) -// EIGEN_ALWAYS_INLINE_ATTRIB should be use in the declaration of function -// which should be inlined even in debug mode. -// FIXME with the always_inline attribute, -// gcc 3.4.x reports the following compilation error: -// Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval Eigen::MatrixBase::eval() const' -// : function body not available -#if EIGEN_GNUC_AT_LEAST(4,0) -#define EIGEN_ALWAYS_INLINE_ATTRIB __attribute__((always_inline)) -#else -#define EIGEN_ALWAYS_INLINE_ATTRIB -#endif - #if EIGEN_GNUC_AT_LEAST(4,1) && !defined(__clang__) && !defined(__INTEL_COMPILER) #define EIGEN_FLATTEN_ATTRIB __attribute__((flatten)) #else #define EIGEN_FLATTEN_ATTRIB #endif -// EIGEN_FORCE_INLINE means "inline as much as possible" +// EIGEN_STRONG_INLINE is a stronger version of the inline, using __forceinline on MSVC, +// but it still doesn't use GCC's always_inline. This is useful in (common) situations where MSVC needs forceinline +// but GCC is still doing fine with just inline. #if (defined _MSC_VER) || (defined __INTEL_COMPILER) #define EIGEN_STRONG_INLINE __forceinline #else #define EIGEN_STRONG_INLINE inline #endif +// EIGEN_ALWAYS_INLINE is the stronget, it has the effect of making the function inline and adding every possible +// attribute to maximize inlining. This should only be used when really necessary: in particular, +// it uses __attribute__((always_inline)) on GCC, which most of the time is useless and can severely harm compile times. +// FIXME with the always_inline attribute, +// gcc 3.4.x reports the following compilation error: +// Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval Eigen::MatrixBase::eval() const' +// : function body not available +#if EIGEN_GNUC_AT_LEAST(4,0) +#define EIGEN_ALWAYS_INLINE __attribute__((always_inline)) inline +#else +#define EIGEN_ALWAYS_INLINE EIGEN_STRONG_INLINE +#endif + #if (defined __GNUC__) #define EIGEN_DONT_INLINE __attribute__((noinline)) #elif (defined _MSC_VER) @@ -249,7 +252,7 @@ #define EIGEN_UNUSED_VARIABLE(var) (void)var; #if (defined __GNUC__) -#define EIGEN_ASM_COMMENT(X) asm("#"X) +#define EIGEN_ASM_COMMENT(X) asm("#" X) #else #define EIGEN_ASM_COMMENT(X) #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index a580b95ad..023716dc9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -82,6 +82,16 @@ namespace internal { +inline void throw_std_bad_alloc() +{ + #ifdef EIGEN_EXCEPTIONS + throw std::bad_alloc(); + #else + std::size_t huge = -1; + new int[huge]; + #endif +} + /***************************************************************************** *** Implementation of handmade aligned functions *** *****************************************************************************/ @@ -192,7 +202,7 @@ inline void check_that_malloc_is_allowed() #endif /** \internal Allocates \a size bytes. The returned pointer is guaranteed to have 16 bytes alignment. - * On allocation error, the returned pointer is null, and if exceptions are enabled then a std::bad_alloc is thrown. + * On allocation error, the returned pointer is null, and std::bad_alloc is thrown. */ inline void* aligned_malloc(size_t size) { @@ -213,10 +223,9 @@ inline void* aligned_malloc(size_t size) result = handmade_aligned_malloc(size); #endif - #ifdef EIGEN_EXCEPTIONS - if(result == 0) - throw std::bad_alloc(); - #endif + if(!result && size) + throw_std_bad_alloc(); + return result; } @@ -241,7 +250,7 @@ inline void aligned_free(void *ptr) /** * \internal * \brief Reallocates an aligned block of memory. -* \throws std::bad_alloc if EIGEN_EXCEPTIONS are defined. +* \throws std::bad_alloc on allocation failure **/ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) { @@ -269,10 +278,9 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) result = handmade_aligned_realloc(ptr,new_size,old_size); #endif -#ifdef EIGEN_EXCEPTIONS - if (result==0 && new_size!=0) - throw std::bad_alloc(); -#endif + if (!result && new_size) + throw_std_bad_alloc(); + return result; } @@ -281,7 +289,7 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) *****************************************************************************/ /** \internal Allocates \a size bytes. If Align is true, then the returned ptr is 16-byte-aligned. - * On allocation error, the returned pointer is null, and if exceptions are enabled then a std::bad_alloc is thrown. + * On allocation error, the returned pointer is null, and a std::bad_alloc is thrown. */ template inline void* conditional_aligned_malloc(size_t size) { @@ -293,9 +301,8 @@ template<> inline void* conditional_aligned_malloc(size_t size) check_that_malloc_is_allowed(); void *result = std::malloc(size); - #ifdef EIGEN_EXCEPTIONS - if(!result) throw std::bad_alloc(); - #endif + if(!result && size) + throw_std_bad_alloc(); return result; } @@ -347,18 +354,27 @@ template inline void destruct_elements_of_array(T *ptr, size_t size) *** Implementation of aligned new/delete-like functions *** *****************************************************************************/ +template +EIGEN_ALWAYS_INLINE void check_size_for_overflow(size_t size) +{ + if(size > size_t(-1) / sizeof(T)) + throw_std_bad_alloc(); +} + /** \internal Allocates \a size objects of type T. The returned pointer is guaranteed to have 16 bytes alignment. - * On allocation error, the returned pointer is undefined, but if exceptions are enabled then a std::bad_alloc is thrown. + * On allocation error, the returned pointer is undefined, but a std::bad_alloc is thrown. * The default constructor of T is called. */ template inline T* aligned_new(size_t size) { + check_size_for_overflow(size); T *result = reinterpret_cast(aligned_malloc(sizeof(T)*size)); return construct_elements_of_array(result, size); } template inline T* conditional_aligned_new(size_t size) { + check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); return construct_elements_of_array(result, size); } @@ -383,6 +399,8 @@ template inline void conditional_aligned_delete(T *ptr, template inline T* conditional_aligned_realloc_new(T* pts, size_t new_size, size_t old_size) { + check_size_for_overflow(new_size); + check_size_for_overflow(old_size); if(new_size < old_size) destruct_elements_of_array(pts+new_size, old_size-new_size); T *result = reinterpret_cast(conditional_aligned_realloc(reinterpret_cast(pts), sizeof(T)*new_size, sizeof(T)*old_size)); @@ -394,6 +412,7 @@ template inline T* conditional_aligned_realloc_new(T* pt template inline T* conditional_aligned_new_auto(size_t size) { + check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); if(NumTraits::RequireInitialization) construct_elements_of_array(result, size); @@ -402,6 +421,8 @@ template inline T* conditional_aligned_new_auto(size_t s template inline T* conditional_aligned_realloc_new_auto(T* pts, size_t new_size, size_t old_size) { + check_size_for_overflow(new_size); + check_size_for_overflow(old_size); if(NumTraits::RequireInitialization && (new_size < old_size)) destruct_elements_of_array(pts+new_size, old_size-new_size); T *result = reinterpret_cast(conditional_aligned_realloc(reinterpret_cast(pts), sizeof(T)*new_size, sizeof(T)*old_size)); @@ -536,6 +557,7 @@ template class aligned_stack_memory_handler #endif #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \ + Eigen::internal::check_size_for_overflow(SIZE); \ TYPE* NAME = (BUFFER)!=0 ? (BUFFER) \ : reinterpret_cast( \ (sizeof(TYPE)*SIZE<=EIGEN_STACK_ALLOCATION_LIMIT) ? EIGEN_ALIGNED_ALLOCA(sizeof(TYPE)*SIZE) \ @@ -545,6 +567,7 @@ template class aligned_stack_memory_handler #else #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \ + Eigen::internal::check_size_for_overflow(SIZE); \ TYPE* NAME = (BUFFER)!=0 ? BUFFER : reinterpret_cast(Eigen::internal::aligned_malloc(sizeof(TYPE)*SIZE)); \ Eigen::internal::aligned_stack_memory_handler EIGEN_CAT(NAME,_stack_memory_destructor)((BUFFER)==0 ? NAME : 0,SIZE,true) @@ -669,6 +692,7 @@ public: pointer allocate( size_type num, const void* hint = 0 ) { EIGEN_UNUSED_VARIABLE(hint); + internal::check_size_for_overflow(num); return static_cast( internal::aligned_malloc( num * sizeof(T) ) ); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 9047c5f83..fcd3b093f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -125,10 +125,9 @@ class compute_matrix_flags aligned_bit = ( ((Options&DontAlign)==0) - && packet_traits::Vectorizable && ( #if EIGEN_ALIGN_STATICALLY - ((!is_dynamic_size_storage) && (((MaxCols*MaxRows) % packet_traits::size) == 0)) + ((!is_dynamic_size_storage) && (((MaxCols*MaxRows*sizeof(Scalar)) % 16) == 0)) #else 0 #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 965dda88b..c8945a848 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -220,6 +220,7 @@ template class SelfAdjointEigenSolver const MatrixType& eigenvectors() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); return m_eivec; } @@ -242,6 +243,7 @@ template class SelfAdjointEigenSolver const RealVectorType& eigenvalues() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); return m_eivalues; } @@ -266,6 +268,7 @@ template class SelfAdjointEigenSolver MatrixType operatorSqrt() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint(); } @@ -291,6 +294,7 @@ template class SelfAdjointEigenSolver MatrixType operatorInverseSqrt() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint(); } @@ -307,7 +311,8 @@ template class SelfAdjointEigenSolver /** \brief Maximum number of iterations. * - * Maximum number of iterations allowed for an eigenvalue to converge. + * The algorithm terminates if it does not converge within m_maxIterations * n iterations, where n + * denotes the size of the matrix. This value is currently set to 30 (copied from LAPACK). */ static const int m_maxIterations = 30; @@ -407,7 +412,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver Index end = n-1; Index start = 0; - Index iter = 0; // number of iterations we are working on one element + Index iter = 0; // total number of iterations while (end>0) { @@ -418,15 +423,14 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver // find the largest unreduced block while (end>0 && m_subdiag[end-1]==0) { - iter = 0; end--; } if (end<=0) break; - // if we spent too many iterations on the current element, we give up + // if we spent too many iterations, we give up iter++; - if(iter > m_maxIterations) break; + if(iter > m_maxIterations * n) break; start = end - 1; while (start>0 && m_subdiag[start-1]!=0) @@ -435,7 +439,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n); } - if (iter <= m_maxIterations) + if (iter <= m_maxIterations * n) m_info = Success; else m_info = NoConvergence; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 2662d60fe..b2f127b6a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -182,10 +182,9 @@ public: template inline typename internal::cast_return_type >::type cast() const { - return typename internal::cast_return_type >::type( - coeffs().template cast()); + return typename internal::cast_return_type >::type(derived()); } - + #ifdef EIGEN_QUATERNIONBASE_PLUGIN # include EIGEN_QUATERNIONBASE_PLUGIN #endif @@ -225,22 +224,25 @@ struct traits > typedef _Scalar Scalar; typedef Matrix<_Scalar,4,1,_Options> Coefficients; enum{ - IsAligned = bool(EIGEN_ALIGN) && ((int(_Options)&Aligned)==Aligned), + IsAligned = internal::traits::Flags & AlignedBit, Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit }; }; } template -class Quaternion : public QuaternionBase >{ +class Quaternion : public QuaternionBase > +{ typedef QuaternionBase > Base; + enum { IsAligned = internal::traits::IsAligned }; + public: typedef _Scalar Scalar; EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) using Base::operator*=; - typedef typename internal::traits >::Coefficients Coefficients; + typedef typename internal::traits::Coefficients Coefficients; typedef typename Base::AngleAxisType AngleAxisType; /** Default constructor leaving the quaternion uninitialized. */ @@ -271,9 +273,16 @@ public: template explicit inline Quaternion(const MatrixBase& other) { *this = other; } + /** Explicit copy constructor with scalar conversion */ + template + explicit inline Quaternion(const Quaternion& other) + { m_coeffs = other.coeffs().template cast(); } + inline Coefficients& coeffs() { return m_coeffs;} inline const Coefficients& coeffs() const { return m_coeffs;} + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) + protected: Coefficients m_coeffs; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h index 633fb23fd..46ae7d651 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h @@ -443,7 +443,6 @@ FullPivLU& FullPivLU::compute(const MatrixType& matrix) m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case) m_maxpivot = RealScalar(0); - RealScalar cutoff(0); for(Index k = 0; k < size; ++k) { @@ -458,14 +457,7 @@ FullPivLU& FullPivLU::compute(const MatrixType& matrix) row_of_biggest_in_corner += k; // correct the values! since they were computed in the corner, col_of_biggest_in_corner += k; // need to add k to them. - // when k==0, biggest_in_corner is the biggest coeff absolute value in the original matrix - if(k == 0) cutoff = biggest_in_corner * NumTraits::epsilon(); - - // if the pivot (hence the corner) is "zero", terminate to avoid generating nan/inf values. - // Notice that using an exact comparison (biggest_in_corner==0) here, as Golub-van Loan do in - // their pseudo-code, results in numerical instability! The cutoff here has been validated - // by running the unit test 'lu' with many repetitions. - if(biggest_in_corner < cutoff) + if(biggest_in_corner==RealScalar(0)) { // before exiting, make sure to initialize the still uninitialized transpositions // in a sane state without destroying what we already have. diff --git a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh index 17cd397a1..f1a88ff74 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh +++ b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh @@ -39,7 +39,7 @@ #endif #if (defined __GNUC__) -#define BTL_ASM_COMMENT(X) asm("#"X) +#define BTL_ASM_COMMENT(X) asm("#" X) #else #define BTL_ASM_COMMENT(X) #endif diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 43f31b91e..50ce7ee0c 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -64,9 +64,14 @@ add_custom_target( add_dependencies(doc-eigen-prerequisites all_snippets all_examples) add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_examples) + add_custom_target(doc ALL COMMAND doxygen Doxyfile-unsupported COMMAND doxygen COMMAND doxygen Doxyfile-unsupported # run doxygen twice to get proper eigen <=> unsupported cross references + COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc + COMMAND ${CMAKE_COMMAND} -E tar cvfz eigen-doc/eigen-doc.tgz eigen-doc/*.html eigen-doc/*.map eigen-doc/*.png eigen-doc/*.css eigen-doc/*.js eigen-doc/*.txt eigen-doc/unsupported + COMMAND ${CMAKE_COMMAND} -E rename eigen-doc html WORKING_DIRECTORY ${Eigen_BINARY_DIR}/doc) + add_dependencies(doc doc-eigen-prerequisites doc-unsupported-prerequisites) diff --git a/gtsam/3rdparty/Eigen/doc/Overview.dox b/gtsam/3rdparty/Eigen/doc/Overview.dox index 04bc075f0..598a96b4d 100644 --- a/gtsam/3rdparty/Eigen/doc/Overview.dox +++ b/gtsam/3rdparty/Eigen/doc/Overview.dox @@ -8,7 +8,7 @@ o /** \mainpage Eigen | \ref QuickRefPage "Short reference" -This is the API documentation for Eigen3. +This is the API documentation for Eigen3. You can download it as a tgz archive for offline reading. Eigen2 users: here is a \ref Eigen2ToEigen3 guide to help porting your application. diff --git a/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp b/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp index e98444347..8d1d1ed65 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp +++ b/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp @@ -10,8 +10,9 @@ int main() A << 1, 2, 2, 3; cout << "Here is the matrix A:\n" << A << endl; SelfAdjointEigenSolver eigensolver(A); + if (eigensolver.info() != Success) abort(); cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl; - cout << "Here's a matrix whose columns are eigenvectors of A " + cout << "Here's a matrix whose columns are eigenvectors of A \n" << "corresponding to these eigenvalues:\n" << eigensolver.eigenvectors() << endl; } diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index adaecb5f8..fab7de0d4 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -121,7 +121,7 @@ ei_add_test(nullary) ei_add_test(nesting_ops "${CMAKE_CXX_FLAGS_DEBUG}") ei_add_test(zerosized) ei_add_test(dontalign) - +ei_add_test(sizeoverflow) ei_add_test(prec_inverse_4x4) string(TOLOWER "${CMAKE_CXX_COMPILER}" cmake_cxx_compiler_tolower) diff --git a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp index e03245654..1e7b2cba0 100644 --- a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp @@ -120,6 +120,10 @@ template void quaternion(void) VERIFY_IS_APPROX(q1f.template cast(),q1); Quaternion q1d = q1.template cast(); VERIFY_IS_APPROX(q1d.template cast(),q1); + + // test bug 369 - improper alignment. + Quaternionx *q = new Quaternionx; + delete q; } template void mapQuaternion(void){ @@ -191,7 +195,6 @@ template void check_const_correctness(const PlainObjec VERIFY( !(Map::Flags & LvalueBit) ); } - void test_geo_quaternion() { for(int i = 0; i < g_repeat; i++) { diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 4ddd11e6b..4510c1905 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -118,7 +118,7 @@ namespace Eigen } \ else if (Eigen::internal::push_assert) \ { \ - eigen_assert_list.push_back(std::string(EI_PP_MAKE_STRING(__FILE__)" ("EI_PP_MAKE_STRING(__LINE__)") : "#a) ); \ + eigen_assert_list.push_back(std::string(EI_PP_MAKE_STRING(__FILE__) " (" EI_PP_MAKE_STRING(__LINE__) ") : " #a) ); \ } #define VERIFY_RAISES_ASSERT(a) \ diff --git a/gtsam/3rdparty/Eigen/test/sizeoverflow.cpp b/gtsam/3rdparty/Eigen/test/sizeoverflow.cpp new file mode 100644 index 000000000..78ededbea --- /dev/null +++ b/gtsam/3rdparty/Eigen/test/sizeoverflow.cpp @@ -0,0 +1,81 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see . + +#include "main.h" + +#define VERIFY_THROWS_BADALLOC(a) { \ + bool threw = false; \ + try { \ + a; \ + } \ + catch (std::bad_alloc&) { threw = true; } \ + VERIFY(threw && "should have thrown bad_alloc: " #a); \ + } + +typedef DenseIndex Index; + +template +void triggerMatrixBadAlloc(Index rows, Index cols) +{ + VERIFY_THROWS_BADALLOC( MatrixType m(rows, cols) ); + VERIFY_THROWS_BADALLOC( MatrixType m; m.resize(rows, cols) ); + VERIFY_THROWS_BADALLOC( MatrixType m; m.conservativeResize(rows, cols) ); +} + +template +void triggerVectorBadAlloc(Index size) +{ + VERIFY_THROWS_BADALLOC( VectorType v(size) ); + VERIFY_THROWS_BADALLOC( VectorType v; v.resize(size) ); + VERIFY_THROWS_BADALLOC( VectorType v; v.conservativeResize(size) ); +} + +void test_sizeoverflow() +{ + // there are 2 levels of overflow checking. first in PlainObjectBase.h we check for overflow in rows*cols computations. + // this is tested in tests of the form times_itself_gives_0 * times_itself_gives_0 + // Then in Memory.h we check for overflow in size * sizeof(T) computations. + // this is tested in tests of the form times_4_gives_0 * sizeof(float) + + size_t times_itself_gives_0 = size_t(1) << (8 * sizeof(Index) / 2); + VERIFY(times_itself_gives_0 * times_itself_gives_0 == 0); + + size_t times_4_gives_0 = size_t(1) << (8 * sizeof(Index) - 2); + VERIFY(times_4_gives_0 * 4 == 0); + + size_t times_8_gives_0 = size_t(1) << (8 * sizeof(Index) - 3); + VERIFY(times_8_gives_0 * 8 == 0); + + triggerMatrixBadAlloc(times_itself_gives_0, times_itself_gives_0); + triggerMatrixBadAlloc(times_itself_gives_0 / 4, times_itself_gives_0); + triggerMatrixBadAlloc(times_4_gives_0, 1); + + triggerMatrixBadAlloc(times_itself_gives_0, times_itself_gives_0); + triggerMatrixBadAlloc(times_itself_gives_0 / 8, times_itself_gives_0); + triggerMatrixBadAlloc(times_8_gives_0, 1); + + triggerVectorBadAlloc(times_4_gives_0); + + triggerVectorBadAlloc(times_8_gives_0); +} diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport index 10fa23b35..8f2396353 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport @@ -35,7 +35,8 @@ namespace Eigen { - /** \defgroup MPRealSupport_Module MPFRC++ Support module + /** \ingroup Unsupported_modules + * \defgroup MPRealSupport_Module MPFRC++ Support module * * \code * #include diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox b/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox index c09f06857..458b507b5 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox +++ b/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox @@ -1,13 +1,22 @@ namespace Eigen { -o /** \mainpage Eigen's unsupported modules +/** \mainpage Eigen's unsupported modules This is the API documentation for Eigen's unsupported modules. These modules are contributions from various users. They are provided "as is", without any support. +Click on the \e Modules tab at the top of this page to get a list of all unsupported modules. + Don't miss the official Eigen documentation. + +\defgroup Unsupported_modules Unsupported modules + +The unsupported modules are contributions from various users. They are +provided "as is", without any support. Nevertheless, some of them are +subject to be included in Eigen in the future. + */ } From df0751ac56a4a95c1ce973b979ef1c152da4b54d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Dec 2011 02:24:18 +0000 Subject: [PATCH 65/72] Started adding cmake support --- CMakeLists.txt | 40 ++++++++++++++++++++++++++++++++++++++++ gtsam/CMakeLists.txt | 18 ++++++++++++++++++ 2 files changed, 58 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 gtsam/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..d98b500b5 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,40 @@ +project(GTSAM CXX C) +set (GTSAM_VERSION_MAJOR 0) +set (GTSAM_VERSION_MINOR 9) +set (GTSAM_VERSION_PATCH 3) +cmake_minimum_required(VERSION 2.6) + +# guard against in-source builds +if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) + message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") +endif() + +# Turn off function inlining when debugging +set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall") +# No optimization in relwithdebinfo +set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall") +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall") +# Eigen no debug in release mode +set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") + +# Configurable Options + +# General build settings +include_directories(gtsam CppUnitLite) + +# Build GTSAM library +add_subdirectory(gtsam) + +# Build CppUnitLite +add_subdirectory(CppUnitLite) + +# Build Tests +add_subdirectory(tests) + +# Build wrap +add_subdirectory(wrap) + +# Build examples +add_subdirectory(examples) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt new file mode 100644 index 000000000..1e40dbb68 --- /dev/null +++ b/gtsam/CMakeLists.txt @@ -0,0 +1,18 @@ +# build 3rd party libraries +add_subdirectory(3rdparty) + +# Build full gtsam library as a single library +# and also build tests +set (gtsam_subdirs + base + geometry + inference + linear + nonlinear + slam) + +foreach(subdir ${gtsam_subdirs}) + file(GLOB sub_gtsam_srcs ${subdir} "*.cpp") + list(APPEND gtsam_srcs sub_gtsam_srcs) +endforeach(subdir) + From 099ab88b5db492dcee46c02243f69b79274b05db Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Dec 2011 02:24:19 +0000 Subject: [PATCH 66/72] Added more cmake functionality --- .cproject | 31 ++++++++++++++++++++++++++++ CMakeLists.txt | 11 +++++++--- CppUnitLite/CMakeLists.txt | 9 ++++++++ gtsam/CMakeLists.txt | 42 +++++++++++++++++++++++++++++++++----- 4 files changed, 85 insertions(+), 8 deletions(-) create mode 100644 CppUnitLite/CMakeLists.txt diff --git a/.cproject b/.cproject index 35f7946a4..0e31dd254 100644 --- a/.cproject +++ b/.cproject @@ -1617,6 +1617,37 @@ true true + + cmake + .. + true + false + true + + + make + -j2 VERBOSE=1 + all + true + false + true + + + make + -j5 VERBOSE=1 + all + true + false + true + + + make + -j2 + clean + true + true + true + make -j2 diff --git a/CMakeLists.txt b/CMakeLists.txt index d98b500b5..e3a754810 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,11 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") # Configurable Options + +# Pull in tests +ENABLE_TESTING() +INCLUDE(Dart) + # General build settings include_directories(gtsam CppUnitLite) @@ -31,10 +36,10 @@ add_subdirectory(gtsam) add_subdirectory(CppUnitLite) # Build Tests -add_subdirectory(tests) +#add_subdirectory(tests) # Build wrap -add_subdirectory(wrap) +#add_subdirectory(wrap) # Build examples -add_subdirectory(examples) \ No newline at end of file +#add_subdirectory(examples) \ No newline at end of file diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt new file mode 100644 index 000000000..6fba22eac --- /dev/null +++ b/CppUnitLite/CMakeLists.txt @@ -0,0 +1,9 @@ +# Build/install CppUnitLite + +FILE(GLOB cppunitlite_headers "*.h") +FILE(GLOB cppunitlite_src "*.cpp") + +ADD_LIBRARY(CppUnitLite STATIC ${cppunitlite_src}) + +install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite) +install(TARGETS CppUnitLite ARCHIVE DESTINATION lib) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1e40dbb68..a47b0c858 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -1,6 +1,3 @@ -# build 3rd party libraries -add_subdirectory(3rdparty) - # Build full gtsam library as a single library # and also build tests set (gtsam_subdirs @@ -11,8 +8,43 @@ set (gtsam_subdirs nonlinear slam) +# Include CCOLAMD source directly into gtsam +include_directories( + 3rdparty/UFconfig + 3rdparty/CCOLAMD/Include + ${CMAKE_SOURCE_DIR}) + +set (ccolamd_srcs + 3rdparty/CCOLAMD/Source/ccolamd.c + 3rdparty/CCOLAMD/Source/ccolamd_global.c + 3rdparty/UFconfig/UFconfig.c) + +set(gtsam_srcs ${ccolamd_srcs}) + +add_library(ccolamd STATIC ${ccolamd_srcs}) + +# Get all sources and headers from each foreach(subdir ${gtsam_subdirs}) - file(GLOB sub_gtsam_srcs ${subdir} "*.cpp") - list(APPEND gtsam_srcs sub_gtsam_srcs) + message(STATUS "Building ${subdir}") + file(GLOB sub_gtsam_srcs "${subdir}/*.cpp") + list(APPEND gtsam_srcs ${sub_gtsam_srcs}) + + # Make a convenience library + add_library(${subdir} STATIC ${sub_gtsam_srcs}) + list(APPEND inner_libs ${subdir}) + + # Build tests + file(GLOB tests_srcs "${subdir}/tests/test*.cpp") + foreach(test_src ${tests_srcs}) + get_filename_component(test_base ${test_src} NAME_WE) + set( test_bin ${EXECUTABLE_OUTPUT_PATH}/${subdir}/${test_base} ) + add_executable(${test_bin} ${test_src}) + #add_test(${subdir}/${test_base} ${subdir}/${test_base}) + #target_link_libraries(${subdir}/${test_base} ${inner_libs} CppUnitLite ccolamd) + #add_custom_target(${test_base}.run ${EXECUTABLE_OUTPUT_PATH}/${test_base} ${ARGN}) + endforeach(test_src) endforeach(subdir) +# build a single shared library +add_library(gtsam SHARED ${gtsam_srcs}) + From 61d05813ffebf115863eef2e959f45885cf1a935 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Dec 2011 02:24:21 +0000 Subject: [PATCH 67/72] Library now builds and all tests run with cmake --- .cproject | 24 +++++++++++++++++++++ CMakeLists.txt | 17 +++++++++++---- examples/CMakeLists.txt | 14 ++++++++++++ examples/vSLAMexample/CMakeLists.txt | 17 +++++++++++++++ gtsam/CMakeLists.txt | 32 +++++++++++++++++++++------- tests/CMakeLists.txt | 31 +++++++++++++++++++++++++++ wrap/CMakeLists.txt | 23 ++++++++++++++++++++ 7 files changed, 146 insertions(+), 12 deletions(-) create mode 100644 examples/CMakeLists.txt create mode 100644 examples/vSLAMexample/CMakeLists.txt create mode 100644 tests/CMakeLists.txt create mode 100644 wrap/CMakeLists.txt diff --git a/.cproject b/.cproject index 0e31dd254..6fc7d8f1f 100644 --- a/.cproject +++ b/.cproject @@ -1648,6 +1648,30 @@ true true + + make + -j2 + test + true + true + true + + + make + -j2 + testSimulated2D.run + true + true + true + + + make + -j2 + wrap_testWrap.run + true + true + true + make -j2 diff --git a/CMakeLists.txt b/CMakeLists.txt index e3a754810..3450ceccb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,8 +26,17 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") ENABLE_TESTING() INCLUDE(Dart) +# Find boost +find_package(Boost 1.40 REQUIRED) + # General build settings -include_directories(gtsam CppUnitLite) +include_directories( + gtsam/3rdparty/UFconfig + gtsam/3rdparty/CCOLAMD/Include + ${CMAKE_SOURCE_DIR} + CppUnitLite + ${BOOST_INCLUDE_DIR}) +link_directories(${Boost_LIBRARY_DIRS}) # Build GTSAM library add_subdirectory(gtsam) @@ -36,10 +45,10 @@ add_subdirectory(gtsam) add_subdirectory(CppUnitLite) # Build Tests -#add_subdirectory(tests) +add_subdirectory(tests) # Build wrap -#add_subdirectory(wrap) +add_subdirectory(wrap) # Build examples -#add_subdirectory(examples) \ No newline at end of file +add_subdirectory(examples) \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 000000000..c2c4113f6 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,14 @@ +# Build example executables +FILE(GLOB example_srcs "*.cpp") +foreach(example_src ${example_srcs} ) + get_filename_component(example_base ${example_src} NAME_WE) + set( example_bin ${example_base} ) + add_executable(${example_bin} ${example_src}) + target_link_libraries(${example_bin} gtsam) +endforeach(example_src) + +add_subdirectory(vSLAMexample) + + + + diff --git a/examples/vSLAMexample/CMakeLists.txt b/examples/vSLAMexample/CMakeLists.txt new file mode 100644 index 000000000..0297159af --- /dev/null +++ b/examples/vSLAMexample/CMakeLists.txt @@ -0,0 +1,17 @@ +# Build vSLAMexample + +set ( srcs + Feature2D.cpp + vSLAMutils.cpp +) +add_library(vSLAMexample ${srcs}) + +add_executable(vISAMexample vISAMexample.cpp) +target_link_libraries(vISAMexample vSLAMexample gtsam) + +add_executable(vSFMexample vSFMexample.cpp) +target_link_libraries(vSFMexample vSLAMexample gtsam) + + + + diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index a47b0c858..efd0ad18f 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -9,10 +9,10 @@ set (gtsam_subdirs slam) # Include CCOLAMD source directly into gtsam -include_directories( - 3rdparty/UFconfig - 3rdparty/CCOLAMD/Include - ${CMAKE_SOURCE_DIR}) +#include_directories( +# 3rdparty/UFconfig +# 3rdparty/CCOLAMD/Include +# ${CMAKE_SOURCE_DIR}) set (ccolamd_srcs 3rdparty/CCOLAMD/Source/ccolamd.c @@ -22,6 +22,7 @@ set (ccolamd_srcs set(gtsam_srcs ${ccolamd_srcs}) add_library(ccolamd STATIC ${ccolamd_srcs}) +list(APPEND inner_libs ccolamd) # Get all sources and headers from each foreach(subdir ${gtsam_subdirs}) @@ -37,12 +38,27 @@ foreach(subdir ${gtsam_subdirs}) file(GLOB tests_srcs "${subdir}/tests/test*.cpp") foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) - set( test_bin ${EXECUTABLE_OUTPUT_PATH}/${subdir}/${test_base} ) + # Trying to put the executables in the right place + #set( test_bin ${EXECUTABLE_OUTPUT_PATH}${subdir}/${test_base} ) + set( test_bin ${subdir}_${test_base} ) add_executable(${test_bin} ${test_src}) - #add_test(${subdir}/${test_base} ${subdir}/${test_base}) - #target_link_libraries(${subdir}/${test_base} ${inner_libs} CppUnitLite ccolamd) - #add_custom_target(${test_base}.run ${EXECUTABLE_OUTPUT_PATH}/${test_base} ${ARGN}) + add_test(${subdir}/${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + + #Linking: would like to link against subset of convenience libraries + #target_link_libraries(${test_bin} CppUnitLite ${inner_libs}) + target_link_libraries(${test_bin} CppUnitLite gtsam) + add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) endforeach(test_src) + + # Build timing scripts + file(GLOB time_srcs "${subdir}/tests/time*.cpp") + foreach(time_src ${time_srcs}) + get_filename_component(time_base ${time_src} NAME_WE) + set( time_bin ${time_base} ) + add_executable(${time_bin} ${time_src}) + target_link_libraries(${time_bin} CppUnitLite gtsam) + add_custom_target(${time_base}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN}) + endforeach(time_src) endforeach(subdir) # build a single shared library diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 000000000..cda7b1aa3 --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,31 @@ +include_directories( + 3rdparty/UFconfig + 3rdparty/CCOLAMD/Include + ${CMAKE_SOURCE_DIR}) + +find_package(Boost COMPONENTS serialization REQUIRED) + +# Build tests +file(GLOB tests_srcs "test*.cpp") +foreach(test_src ${tests_srcs}) + get_filename_component(test_base ${test_src} NAME_WE) + # Trying to put the executables in the right place + set( test_bin ${test_base} ) + add_executable(${test_bin} ${test_src}) + add_test(${test_bin} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + + target_link_libraries(${test_bin} CppUnitLite gtsam ${Boost_SERIALIZATION_LIBRARY}) + add_custom_target(${test_base}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) +endforeach(test_src) + +# Build timing scripts +file(GLOB time_srcs "time*.cpp") +foreach(time_src ${time_srcs}) + get_filename_component(time_base ${time_src} NAME_WE) + # Trying to put the executables in the right place + set( time_bin ${time_base} ) + add_executable(${time_bin} ${time_src}) + + target_link_libraries(${time_bin} CppUnitLite gtsam) + add_custom_target(${time_base}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN}) +endforeach(time_src) \ No newline at end of file diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt new file mode 100644 index 000000000..b357481bf --- /dev/null +++ b/wrap/CMakeLists.txt @@ -0,0 +1,23 @@ +# Build/install Wrap + +# Build the executable itself +FILE(GLOB wrap_srcs "*.cpp") +LIST(REMOVE_ITEM wrap_srcs wrap.cpp) +add_library(wrapLib STATIC ${wrap_srcs}) +add_executable(wrap wrap.cpp) +target_link_libraries(wrap wrapLib) + +# Build tests +FILE(GLOB wrap_test_srcs "tests/test*.cpp") +add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") +foreach(test_src ${wrap_test_srcs} ) + get_filename_component(test_base ${test_src} NAME_WE) + set( test_bin wrap_${test_base} ) + add_executable(${test_bin} ${test_src}) + add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + target_link_libraries(${test_bin} CppUnitLite gtsam wrapLib) + add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) +endforeach(test_src) + + + From 5dd461c5b1badcd3bdb6fd5159304bdfa73a8e6d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Dec 2011 02:24:23 +0000 Subject: [PATCH 68/72] Added make check equivalent --- .cproject | 293 +++++++++++++++++++++++-------------------- CMakeLists.txt | 12 +- gtsam/CMakeLists.txt | 21 ++-- tests/CMakeLists.txt | 10 +- wrap/CMakeLists.txt | 3 +- 5 files changed, 180 insertions(+), 159 deletions(-) diff --git a/.cproject b/.cproject index 6fc7d8f1f..a456f1ab6 100644 --- a/.cproject +++ b/.cproject @@ -378,6 +378,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -404,7 +412,6 @@ make - tests/testBayesTree.run true false @@ -412,7 +419,6 @@ make - testBinaryBayesNet.run true false @@ -460,7 +466,6 @@ make - testSymbolicBayesNet.run true false @@ -468,7 +473,6 @@ make - tests/testSymbolicFactor.run true false @@ -476,7 +480,6 @@ make - testSymbolicFactorGraph.run true false @@ -492,20 +495,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -532,6 +526,7 @@ make + testGraph.run true false @@ -603,6 +598,7 @@ make + testInference.run true false @@ -610,6 +606,7 @@ make + testGaussianFactor.run true false @@ -617,6 +614,7 @@ make + testJunctionTree.run true false @@ -624,6 +622,7 @@ make + testSymbolicBayesNet.run true false @@ -631,6 +630,7 @@ make + testSymbolicFactorGraph.run true false @@ -700,22 +700,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -732,6 +716,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -756,15 +756,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -772,14 +764,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -820,7 +804,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -828,6 +820,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1150,7 +1150,6 @@ make - testErrors.run true false @@ -1558,6 +1557,7 @@ make + testSimulated2DOriented.run true false @@ -1597,6 +1597,7 @@ make + testSimulated2D.run true false @@ -1604,6 +1605,7 @@ make + testSimulated3D.run true false @@ -1672,6 +1674,22 @@ true true + + make + -j2 + check + true + true + true + + + make + -j5 + check + true + false + true + make -j2 @@ -1906,6 +1924,7 @@ make + tests/testGaussianISAM2 true false @@ -1927,46 +1946,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -2063,6 +2042,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + make -j2 @@ -2119,54 +2186,6 @@ false true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - diff --git a/CMakeLists.txt b/CMakeLists.txt index 3450ceccb..408e0c6bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,10 @@ project(GTSAM CXX C) +cmake_minimum_required(VERSION 2.6) + +# Set the version number for the libarary set (GTSAM_VERSION_MAJOR 0) set (GTSAM_VERSION_MINOR 9) set (GTSAM_VERSION_PATCH 3) -cmake_minimum_required(VERSION 2.6) # guard against in-source builds if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) @@ -23,8 +25,12 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") # Pull in tests -ENABLE_TESTING() -INCLUDE(Dart) +enable_testing() +include(Dart) +include(CTest) + +# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) +add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) # Find boost find_package(Boost 1.40 REQUIRED) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index efd0ad18f..0a027dfda 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -7,12 +7,6 @@ set (gtsam_subdirs linear nonlinear slam) - -# Include CCOLAMD source directly into gtsam -#include_directories( -# 3rdparty/UFconfig -# 3rdparty/CCOLAMD/Include -# ${CMAKE_SOURCE_DIR}) set (ccolamd_srcs 3rdparty/CCOLAMD/Source/ccolamd.c @@ -21,8 +15,9 @@ set (ccolamd_srcs set(gtsam_srcs ${ccolamd_srcs}) -add_library(ccolamd STATIC ${ccolamd_srcs}) -list(APPEND inner_libs ccolamd) +# ccolamd convenience library +#add_library(ccolamd STATIC ${ccolamd_srcs}) +#list(APPEND inner_libs ccolamd) # Get all sources and headers from each foreach(subdir ${gtsam_subdirs}) @@ -31,8 +26,8 @@ foreach(subdir ${gtsam_subdirs}) list(APPEND gtsam_srcs ${sub_gtsam_srcs}) # Make a convenience library - add_library(${subdir} STATIC ${sub_gtsam_srcs}) - list(APPEND inner_libs ${subdir}) + # add_library(${subdir} STATIC ${sub_gtsam_srcs}) + # list(APPEND inner_libs ${subdir}) # Build tests file(GLOB tests_srcs "${subdir}/tests/test*.cpp") @@ -41,7 +36,8 @@ foreach(subdir ${gtsam_subdirs}) # Trying to put the executables in the right place #set( test_bin ${EXECUTABLE_OUTPUT_PATH}${subdir}/${test_base} ) set( test_bin ${subdir}_${test_base} ) - add_executable(${test_bin} ${test_src}) + add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) + add_dependencies(check ${test_bin}) add_test(${subdir}/${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) #Linking: would like to link against subset of convenience libraries @@ -55,7 +51,8 @@ foreach(subdir ${gtsam_subdirs}) foreach(time_src ${time_srcs}) get_filename_component(time_base ${time_src} NAME_WE) set( time_bin ${time_base} ) - add_executable(${time_bin} ${time_src}) + add_executable(${time_bin} EXCLUDE_FROM_ALL ${time_src}) + add_dependencies(check ${time_bin}) target_link_libraries(${time_bin} CppUnitLite gtsam) add_custom_target(${time_base}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN}) endforeach(time_src) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index cda7b1aa3..96f101f5c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -9,11 +9,10 @@ find_package(Boost COMPONENTS serialization REQUIRED) file(GLOB tests_srcs "test*.cpp") foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) - # Trying to put the executables in the right place set( test_bin ${test_base} ) - add_executable(${test_bin} ${test_src}) + add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) + add_dependencies(check ${test_bin}) add_test(${test_bin} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) - target_link_libraries(${test_bin} CppUnitLite gtsam ${Boost_SERIALIZATION_LIBRARY}) add_custom_target(${test_base}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) endforeach(test_src) @@ -22,10 +21,9 @@ endforeach(test_src) file(GLOB time_srcs "time*.cpp") foreach(time_src ${time_srcs}) get_filename_component(time_base ${time_src} NAME_WE) - # Trying to put the executables in the right place set( time_bin ${time_base} ) - add_executable(${time_bin} ${time_src}) - + add_executable(${time_bin} EXCLUDE_FROM_ALL ${time_src}) + add_dependencies(check ${time_bin}) target_link_libraries(${time_bin} CppUnitLite gtsam) add_custom_target(${time_base}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN}) endforeach(time_src) \ No newline at end of file diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index b357481bf..c28a1e006 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -13,8 +13,9 @@ add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") foreach(test_src ${wrap_test_srcs} ) get_filename_component(test_base ${test_src} NAME_WE) set( test_bin wrap_${test_base} ) - add_executable(${test_bin} ${test_src}) + add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + add_dependencies(check ${test_bin}) target_link_libraries(${test_bin} CppUnitLite gtsam wrapLib) add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) endforeach(test_src) From 3afc03cf0491bbad74b121676875175e5959fa52 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Dec 2011 02:24:25 +0000 Subject: [PATCH 69/72] Moved installation of 3rdparty headers into separate cmake file, install some wrap components --- .cproject | 8 ++++++++ CMakeLists.txt | 12 ++++++++++++ gtsam/3rdparty/CMakeLists.txt | 15 +++++++++++++++ gtsam/CMakeLists.txt | 22 +++++++++------------- wrap/CMakeLists.txt | 11 ++++++----- 5 files changed, 50 insertions(+), 18 deletions(-) create mode 100644 gtsam/3rdparty/CMakeLists.txt diff --git a/.cproject b/.cproject index a456f1ab6..64b368108 100644 --- a/.cproject +++ b/.cproject @@ -1690,6 +1690,14 @@ false true + + make + -j2 + install + true + true + true + make -j2 diff --git a/CMakeLists.txt b/CMakeLists.txt index 408e0c6bd..094fcd59a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,18 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() +# guard against bad build-type strings +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Debug") +endif() + +string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) +if( NOT cmake_build_type_tolower STREQUAL "debug" + AND NOT cmake_build_type_tolower STREQUAL "release" + AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") + message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, RelWithDebInfo (case-insensitive).") +endif() + # Turn off function inlining when debugging set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall") diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt new file mode 100644 index 000000000..3b2d4d178 --- /dev/null +++ b/gtsam/3rdparty/CMakeLists.txt @@ -0,0 +1,15 @@ +# install CCOLAMD headers +install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/CCOLAMD) +install(FILES UFconfig/UFconfig.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/UFconfig) + +# install Eigen - only the headers +install(DIRECTORY Eigen/Eigen + DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen + FILES_MATCHING PATTERN "*.h") +file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") +foreach(eigen_dir ${eigen_dir_headers_all}) + get_filename_component(filename ${eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src"))) + install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/Eigen) + endif() +endforeach(eigen_dir) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 0a027dfda..5b795b11f 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -12,12 +12,12 @@ set (ccolamd_srcs 3rdparty/CCOLAMD/Source/ccolamd.c 3rdparty/CCOLAMD/Source/ccolamd_global.c 3rdparty/UFconfig/UFconfig.c) - + +# install headers from 3rdparty libraries +add_subdirectory(3rdparty) + +# Accumulate gtsam_srcs set(gtsam_srcs ${ccolamd_srcs}) - -# ccolamd convenience library -#add_library(ccolamd STATIC ${ccolamd_srcs}) -#list(APPEND inner_libs ccolamd) # Get all sources and headers from each foreach(subdir ${gtsam_subdirs}) @@ -25,23 +25,18 @@ foreach(subdir ${gtsam_subdirs}) file(GLOB sub_gtsam_srcs "${subdir}/*.cpp") list(APPEND gtsam_srcs ${sub_gtsam_srcs}) - # Make a convenience library - # add_library(${subdir} STATIC ${sub_gtsam_srcs}) - # list(APPEND inner_libs ${subdir}) + # install headers + file(GLOB sub_gtsam_headers "${subdir}/*.h") + install(FILES ${sub_gtsam_headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/${subdir}) # Build tests file(GLOB tests_srcs "${subdir}/tests/test*.cpp") foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) - # Trying to put the executables in the right place - #set( test_bin ${EXECUTABLE_OUTPUT_PATH}${subdir}/${test_base} ) set( test_bin ${subdir}_${test_base} ) add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) add_dependencies(check ${test_bin}) add_test(${subdir}/${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) - - #Linking: would like to link against subset of convenience libraries - #target_link_libraries(${test_bin} CppUnitLite ${inner_libs}) target_link_libraries(${test_bin} CppUnitLite gtsam) add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) endforeach(test_src) @@ -60,4 +55,5 @@ endforeach(subdir) # build a single shared library add_library(gtsam SHARED ${gtsam_srcs}) +install(TARGETS gtsam LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index c28a1e006..b6bf8b4f9 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -1,14 +1,15 @@ # Build/install Wrap # Build the executable itself -FILE(GLOB wrap_srcs "*.cpp") -LIST(REMOVE_ITEM wrap_srcs wrap.cpp) +file(GLOB wrap_srcs "*.cpp") +list(REMOVE_ITEM wrap_srcs wrap.cpp) add_library(wrapLib STATIC ${wrap_srcs}) add_executable(wrap wrap.cpp) target_link_libraries(wrap wrapLib) +install(TARGETS wrap DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) # Build tests -FILE(GLOB wrap_test_srcs "tests/test*.cpp") +file(GLOB wrap_test_srcs "tests/test*.cpp") add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") foreach(test_src ${wrap_test_srcs} ) get_filename_component(test_base ${test_src} NAME_WE) @@ -20,5 +21,5 @@ foreach(test_src ${wrap_test_srcs} ) add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) endforeach(test_src) - - +# Install matlab header +install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/wrap) From 051e30648d95bdfc7737a8b9df331a00493a3dd5 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Wed, 14 Dec 2011 02:30:29 +0000 Subject: [PATCH 70/72] Added ignore .svn to CMakeLists.txt in 3rdparty when installing --- gtsam/3rdparty/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 3b2d4d178..ece7bace8 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -9,7 +9,7 @@ install(DIRECTORY Eigen/Eigen file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") foreach(eigen_dir ${eigen_dir_headers_all}) get_filename_component(filename ${eigen_dir} NAME) - if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src"))) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/Eigen) endif() -endforeach(eigen_dir) \ No newline at end of file +endforeach(eigen_dir) From 5a75c9e963094e84c3b5ab07b6562931d0eb9fff Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Wed, 14 Dec 2011 21:10:56 +0000 Subject: [PATCH 71/72] Fixed bug with Argument.cpp due to different constructor argument starting with the name letter --- wrap/Argument.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 1610f98aa..329f0a643 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "Argument.h" @@ -70,8 +71,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string str; + BOOST_FOREACH(Argument arg, *this) - str += arg.type[0]; + { + BOOST_FOREACH(char ch, arg.type) + if(isupper(ch)) + str += ch; + + if(str.length() == 0) + str += arg.type[0]; + } + return str; } From 33c15dd4238372ef6a909c798c8d18ea7ebdf852 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 15 Dec 2011 04:15:05 +0000 Subject: [PATCH 72/72] Cheirality check for Projection Factor with Calibrated Camera. --- gtsam/geometry/CalibratedCamera.cpp | 2 ++ gtsam/geometry/CalibratedCamera.h | 5 +++++ gtsam/slam/ProjectionFactor.h | 15 ++++++++++++--- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index f0c70408e..a45da9789 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -61,6 +61,8 @@ Point2 CalibratedCamera::project(const Point3& point, const Rot3& R = pose.rotation(); const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); Point3 q = pose.transform_to(point); + if(q.z() <= 0) + throw CheiralityException(); if (D_intrinsic_pose || D_intrinsic_point) { double X = q.x(), Y = q.y(), Z = q.z(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 880bb9316..4538b3367 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -23,6 +23,11 @@ namespace gtsam { + class CheiralityException: public std::runtime_error { + public: + CheiralityException() : std::runtime_error("Cheirality Exception") {} + }; + /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index ee5649f78..6940e367f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -82,9 +82,18 @@ namespace gtsam { /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { - SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); - return reprojectionError.vector(); + try { + SimpleCamera camera(*K_, pose); + Point2 reprojectionError(camera.project(point, H1, H2) - z_); + return reprojectionError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2,6); + if (H2) *H2 = zeros(2,3); + cout << e.what() << ": Landmark "<< this->key2_.index() << + " moved behind camera " << this->key1_.index() << endl; + return zero(2); + } } /** return the measurement */