diff --git a/.cproject b/.cproject
index 075558734..b31515394 100644
--- a/.cproject
+++ b/.cproject
@@ -308,6 +308,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -334,7 +342,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -342,7 +349,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -390,7 +396,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -398,7 +403,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -406,7 +410,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -422,20 +425,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j5
@@ -524,22 +518,6 @@
false
true
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
make
-j2
@@ -556,6 +534,22 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
@@ -580,26 +574,26 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ testValues.run
true
true
true
-
+
make
- -j2
- check
+ -j5
+ testOrdering.run
true
true
true
-
+
make
- -j2
- clean
+ -j5
+ testKey.run
true
true
true
@@ -684,26 +678,26 @@
true
true
-
+
make
- -j5
- testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- testOrdering.run
+ -j2
+ check
true
true
true
-
+
make
- -j5
- testKey.run
+ -j2
+ clean
true
true
true
@@ -958,7 +952,6 @@
make
-
testGraph.run
true
false
@@ -966,7 +959,6 @@
make
-
testJunctionTree.run
true
false
@@ -974,7 +966,6 @@
make
-
testSymbolicBayesNetB.run
true
false
@@ -1110,7 +1101,6 @@
make
-
testErrors.run
true
false
@@ -1574,6 +1564,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1613,6 +1604,7 @@
make
+
testSimulated2D.run
true
false
@@ -1620,6 +1612,7 @@
make
+
testSimulated3D.run
true
false
@@ -1835,6 +1828,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -1856,6 +1850,102 @@
true
true
+
+ make
+ -j2
+ testRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
+ true
+ true
+ true
+
make
-j2
@@ -2057,7 +2147,6 @@
cpack
-
-G DEB
true
false
@@ -2065,7 +2154,6 @@
cpack
-
-G RPM
true
false
@@ -2073,7 +2161,6 @@
cpack
-
-G TGZ
true
false
@@ -2081,7 +2168,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2207,98 +2293,34 @@
true
true
-
+
make
- -j2
- testRot3.run
+ -j5
+ testSpirit.run
true
true
true
-
+
make
- -j2
- testRot2.run
+ -j5
+ testWrap.run
true
true
true
-
+
make
- -j2
- testPose3.run
+ -j5
+ check.wrap
true
true
true
-
+
make
- -j2
- timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
+ -j5
+ wrap
true
true
true
@@ -2342,46 +2364,7 @@
false
true
-
- make
- -j5
- testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- testWrap.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
+
diff --git a/gtsam.h b/gtsam.h
index 46ea37241..73f73389c 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -702,17 +702,17 @@ class VariableIndex {
#include
namespace noiseModel {
-class Base {
+virtual class Base {
};
-class Gaussian : gtsam::noiseModel::Base {
+virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
// Matrix R() const; // FIXME: cannot parse!!!
void print(string s) const;
};
-class Diagonal : gtsam::noiseModel::Gaussian {
+virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
@@ -720,14 +720,14 @@ class Diagonal : gtsam::noiseModel::Gaussian {
void print(string s) const;
};
-class Isotropic : gtsam::noiseModel::Gaussian {
+virtual class Isotropic : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
void print(string s) const;
};
-class Unit : gtsam::noiseModel::Gaussian {
+virtual class Unit : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Unit* Create(size_t dim);
void print(string s) const;
};
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index a9bd195ba..8c470f1aa 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -86,9 +86,6 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
params_ = rhs.params_;
doglegDelta_ = rhs.doglegDelta_;
-#ifndef NDEBUG
- lastRelinVariables_ = rhs.lastRelinVariables_;
-#endif
lastAffectedVariableCount = rhs.lastAffectedVariableCount;
lastAffectedFactorCount = rhs.lastAffectedFactorCount;
lastAffectedCliqueCount = rhs.lastAffectedCliqueCount;
@@ -661,15 +658,8 @@ ISAM2Result ISAM2::update(
toc(7,"expmap");
result.variablesRelinearized = markedKeys.size();
-
-#ifndef NDEBUG
- lastRelinVariables_ = markedRelinMask;
-#endif
} else {
result.variablesRelinearized = 0;
-#ifndef NDEBUG
- lastRelinVariables_ = vector(ordering_.nVars(), false);
-#endif
}
tic(8,"linearize new");
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 34fc7b46f..0d540a729 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -403,11 +403,6 @@ protected:
/** The inverse ordering, only used for creating ISAM2Result::DetailedResults */
boost::optional inverseOrdering_;
-private:
-#ifndef NDEBUG
- std::vector lastRelinVariables_;
-#endif
-
public:
typedef ISAM2 This; ///< This class
diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp
index 5b04b92c6..2133c0ee8 100644
--- a/gtsam/slam/visualSLAM.cpp
+++ b/gtsam/slam/visualSLAM.cpp
@@ -31,7 +31,7 @@ namespace visualSLAM {
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries");
- for(size_t k=0;k
+
+#include
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
+ Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
+ rekeyFactor(invOrdering);
+}
+
+/* ************************************************************************* */
+void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
+ BOOST_FOREACH(Index& idx, factor_->keys()) {
+ Key fullKey = invOrdering.find(idx)->second;
+ idx = fullKey;
+ keys_.push_back(fullKey);
+ }
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+ const JacobianFactor& factor, const Ordering& ordering)
+: factor_(factor.clone()) {
+ rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+ const HessianFactor& factor, const Ordering& ordering)
+: factor_(factor.clone()) {
+ rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+ const GaussianFactor::shared_ptr& factor, const Ordering& ordering)
+: factor_(factor->clone()) {
+ rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+ const GaussianFactor::shared_ptr& factor)
+: factor_(factor->clone())
+{
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
+ const Ordering::InvertedMap& inverted_ordering)
+: factor_(factor.clone()) {
+ rekeyFactor(inverted_ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
+ const Ordering::InvertedMap& inverted_ordering)
+: factor_(factor.clone()) {
+ rekeyFactor(inverted_ordering);
+}
+
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(
+ const GaussianFactor::shared_ptr& factor,
+ const Ordering::InvertedMap& ordering)
+: factor_(factor->clone()) {
+ rekeyFactor(ordering);
+}
+
+/* ************************************************************************* */
+void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+ Base::print(s+"LinearContainerFactor", keyFormatter);
+ if (factor_)
+ factor_->print(" Stored Factor", keyFormatter);
+}
+
+/* ************************************************************************* */
+bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const {
+ const LinearContainerFactor* jcf = dynamic_cast(&f);
+ return jcf && factor_->equals(*jcf->factor_, tol) && NonlinearFactor::equals(f);
+}
+
+/* ************************************************************************* */
+double LinearContainerFactor::error(const Values& c) const {
+ // VectorValues vecvalues;
+ // // FIXME: add values correctly here
+ // return factor_.error(vecvalues);
+ return 0; // FIXME: placeholder
+}
+
+/* ************************************************************************* */
+size_t LinearContainerFactor::dim() const {
+ if (isJacobian())
+ return toJacobian()->get_model()->dim();
+ else
+ return 1; // Hessians don't have true dimension
+}
+
+/* ************************************************************************* */
+boost::shared_ptr
+LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const {
+ // clone factor
+ boost::shared_ptr result = factor_->clone();
+
+ // rekey
+ BOOST_FOREACH(Index& key, result->keys())
+ key = ordering[key];
+
+ return result;
+}
+
+/* ************************************************************************* */
+bool LinearContainerFactor::isJacobian() const {
+ return boost::shared_dynamic_cast(factor_);
+}
+
+/* ************************************************************************* */
+JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const {
+ return boost::shared_dynamic_cast(factor_);
+}
+
+/* ************************************************************************* */
+HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
+ return boost::shared_dynamic_cast(factor_);
+}
+
+/* ************************************************************************* */
+GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& ordering) const {
+ GaussianFactor::shared_ptr result = factor_->negate();
+ BOOST_FOREACH(Key& key, result->keys())
+ key = ordering[key];
+ return result;
+}
+
+/* ************************************************************************* */
+} // \namespace gtsam
+
+
+
diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h
new file mode 100644
index 000000000..23705f96e
--- /dev/null
+++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h
@@ -0,0 +1,113 @@
+/**
+ * @file LinearContainerFactor.h
+ *
+ * @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph
+ *
+ * @date Jul 6, 2012
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+/**
+ * Dummy version of a generic linear factor to be injected into a nonlinear factor graph
+ */
+class LinearContainerFactor : public NonlinearFactor {
+protected:
+
+ GaussianFactor::shared_ptr factor_;
+
+public:
+
+ /** Primary constructor: store a linear factor and decode the ordering */
+ LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering);
+
+ /** Primary constructor: store a linear factor and decode the ordering */
+ LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering);
+
+ /** Constructor from shared_ptr */
+ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
+
+ /** Constructor from re-keyed factor: all indices assumed replaced with Key */
+ LinearContainerFactor(const GaussianFactor::shared_ptr& factor);
+
+ /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
+ LinearContainerFactor(const JacobianFactor& factor,
+ const Ordering::InvertedMap& inverted_ordering);
+
+ /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
+ LinearContainerFactor(const HessianFactor& factor,
+ const Ordering::InvertedMap& inverted_ordering);
+
+ /** Constructor from shared_ptr with inverted ordering*/
+ LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
+ const Ordering::InvertedMap& ordering);
+
+ // Access
+
+ const GaussianFactor::shared_ptr& factor() const { return factor_; }
+
+ // Testable
+
+ /** print */
+ void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+ /** Check if two factors are equal */
+ bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+
+ // NonlinearFactor
+
+ /**
+ * Calculate the error of the factor: uses the underlying linear factor to compute ordering
+ */
+ double error(const Values& c) const;
+
+ /** get the dimension of the factor: rows of linear factor */
+ size_t dim() const;
+
+ /** linearize to a GaussianFactor: values has no effect, just clones/rekeys underlying factor */
+ boost::shared_ptr
+ linearize(const Values& c, const Ordering& ordering) const;
+
+ /**
+ * Creates an anti-factor directly and performs rekeying due to ordering
+ */
+ GaussianFactor::shared_ptr negate(const Ordering& ordering) const;
+
+ /**
+ * Creates a shared_ptr clone of the factor - needs to be specialized to allow
+ * for subclasses
+ *
+ * Clones the underlying linear factor
+ */
+ NonlinearFactor::shared_ptr clone() const {
+ return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_));
+ }
+
+ // casting syntactic sugar
+
+ /**
+ * Simple check whether this is a Jacobian or Hessian factor
+ */
+ bool isJacobian() const;
+
+ /** Casts to JacobianFactor */
+ JacobianFactor::shared_ptr toJacobian() const;
+
+ /** Casts to HessianFactor */
+ HessianFactor::shared_ptr toHessian() const;
+
+protected:
+ void rekeyFactor(const Ordering& ordering);
+ void rekeyFactor(const Ordering::InvertedMap& invOrdering);
+
+}; // \class LinearContainerFactor
+
+} // \namespace gtsam
+
+
diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp
new file mode 100644
index 000000000..e85b880e8
--- /dev/null
+++ b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp
@@ -0,0 +1,113 @@
+/**
+ * @file testLinearContainerFactor.cpp
+ *
+ * @date Jul 6, 2012
+ * @author Alex Cunningham
+ */
+
+#include
+
+#include
+
+#include
+
+using namespace gtsam;
+
+const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
+const double tol = 1e-5;
+
+gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
+
+Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
+Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
+
+/* ************************************************************************* */
+TEST( testLinearContainerFactor, generic_jacobian_factor ) {
+
+ Ordering initOrdering; initOrdering += x1, x2, l1, l2;
+
+ JacobianFactor expLinFactor1(
+ initOrdering[l1],
+ Matrix_(2,2,
+ 2.74222, -0.0067457,
+ 0.0, 2.63624),
+ initOrdering[l2],
+ Matrix_(2,2,
+ -0.0455167, -0.0443573,
+ -0.0222154, -0.102489),
+ Vector_(2, 0.0277052,
+ -0.0533393),
+ diag_model2);
+
+ LinearContainerFactor actFactor1(expLinFactor1, initOrdering);
+ Values values;
+ values.insert(l1, landmark1);
+ values.insert(l2, landmark2);
+ values.insert(x1, poseA1);
+ values.insert(x2, poseA2);
+
+ // Check reconstruction from same ordering
+ GaussianFactor::shared_ptr actLinearizationA = actFactor1.linearize(values, initOrdering);
+ EXPECT(assert_equal(*expLinFactor1.clone(), *actLinearizationA, tol));
+
+ // Check reconstruction from new ordering
+ Ordering newOrdering; newOrdering += x1, l1, x2, l2;
+ GaussianFactor::shared_ptr actLinearizationB = actFactor1.linearize(values, newOrdering);
+
+ JacobianFactor expLinFactor2(
+ newOrdering[l1],
+ Matrix_(2,2,
+ 2.74222, -0.0067457,
+ 0.0, 2.63624),
+ newOrdering[l2],
+ Matrix_(2,2,
+ -0.0455167, -0.0443573,
+ -0.0222154, -0.102489),
+ Vector_(2, 0.0277052,
+ -0.0533393),
+ diag_model2);
+
+ EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
+}
+
+/* ************************************************************************* */
+TEST( testLinearContainerFactor, generic_hessian_factor ) {
+ Matrix G11 = Matrix_(1,1, 1.0);
+ Matrix G12 = Matrix_(1,2, 2.0, 4.0);
+ Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
+
+ Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
+ Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
+
+ Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
+
+ Vector g1 = Vector_(1, -7.0);
+ Vector g2 = Vector_(2, -8.0, -9.0);
+ Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
+
+ double f = 10.0;
+
+ Ordering initOrdering; initOrdering += x1, x2, l1, l2;
+ HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
+ G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
+
+ Values values;
+ values.insert(l1, landmark1);
+ values.insert(l2, landmark2);
+ values.insert(x1, poseA1);
+ values.insert(x2, poseA2);
+
+ LinearContainerFactor actFactor(initFactor, initOrdering);
+ GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering);
+ EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
+
+ Ordering newOrdering; newOrdering += l1, x1, x2, l2;
+ HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
+ G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
+ GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
+ EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index f63d2add5..9fdd6512f 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -431,7 +431,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons
bool totalGradOk = assert_equal(expectedGradient, actualGradient);
EXPECT(totalGradOk);
- return nodeGradientsOk && expectedGradOk && totalGradOk;
+ return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual;
}
/* ************************************************************************* */
diff --git a/wrap/Class.cpp b/wrap/Class.cpp
index 803017e42..dd28a830a 100644
--- a/wrap/Class.cpp
+++ b/wrap/Class.cpp
@@ -18,7 +18,9 @@
#include
#include
#include
-#include
+//#include // on Linux GCC: fails with error regarding needing C++0x std flags
+//#include // same failure as above
+#include // works on Linux GCC
#include
#include
diff --git a/wrap/Class.h b/wrap/Class.h
index 5703f8353..78d779a1f 100644
--- a/wrap/Class.h
+++ b/wrap/Class.h
@@ -34,7 +34,7 @@ struct Class {
typedef std::map StaticMethods;
/// Constructor creates an empty class
- Class(bool verbose=true) : verbose_(verbose), isVirtual(false) {}
+ Class(bool verbose=true) : isVirtual(false), verbose_(verbose) {}
// Then the instance variables are set directly by the Module constructor
std::string name; ///< Class name
diff --git a/wrap/Method.h b/wrap/Method.h
index d9b1f09f1..936b3812d 100644
--- a/wrap/Method.h
+++ b/wrap/Method.h
@@ -30,7 +30,7 @@ struct Method {
/// Constructor creates empty object
Method(bool verbose = true) :
- verbose_(verbose) {}
+ verbose_(verbose), is_const_(false) {}
// Then the instance variables are set directly by the Module constructor
bool verbose_;
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 006e38d4a..413fc84b8 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -1,350 +1,350 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file Module.ccp
- * @author Frank Dellaert
- * @author Alex Cunningham
- * @author Andrew Melim
- **/
-
-#include "Module.h"
-#include "FileWriter.h"
-#include "utilities.h"
-#include "spirit_actors.h"
-
-//#define BOOST_SPIRIT_DEBUG
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-
-using namespace std;
-using namespace wrap;
-using namespace BOOST_SPIRIT_CLASSIC_NS;
-namespace bl = boost::lambda;
-namespace fs = boost::filesystem;
-
-typedef rule Rule;
-
-/* ************************************************************************* */
-// We parse an interface file into a Module object.
-// The grammar is defined using the boost/spirit combinatorial parser.
-// For example, str_p("const") parses the string "const", and the >>
-// operator creates a sequence parser. The grammar below, composed of rules
-// and with start rule [class_p], doubles as the specs for our interface files.
-/* ************************************************************************* */
-
-Module::Module(const string& interfacePath,
- const string& moduleName, bool 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.
- string methodName, methodName0;
- bool isConst, isConst0 = false;
- ReturnValue retVal0, retVal;
- Argument arg0, arg;
- ArgumentList args0, args;
- vector arg_dup; ///keep track of duplicates
- Constructor constructor0(enable_verbose), constructor(enable_verbose);
- Deconstructor deconstructor0(enable_verbose), deconstructor(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);
- ForwardDeclaration fwDec0, fwDec;
- vector namespaces, /// current namespace tag
- namespace_includes, /// current set of includes
- namespaces_return, /// namespace for current return type
- using_namespace_current; /// All namespaces from "using" declarations
- string include_path = "";
- const string null_str = "";
-
- //----------------------------------------------------------------------------
- // 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
- // lexeme_d turns off white space skipping
- // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
- // ----------------------------------------------------------------------------
-
- Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p);
-
- Rule basisType_p =
- (str_p("string") | "bool" | "size_t" | "int" | "double" | "char" | "unsigned char");
-
- Rule keywords_p =
- (str_p("const") | "static" | "namespace" | basisType_p);
-
- Rule eigenType_p =
- (str_p("Vector") | "Matrix");
-
- Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p);
-
- Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
-
- Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::");
-
- Rule 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 classArg_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_ptr,true)] | ch_p('&')[assign_a(arg.is_ref,true)]);
-
- Rule classParent_p =
- *(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >>
- className_p[push_back_a(cls.qualifiedParent)];
-
- Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
-
- Rule argument_p =
- ((basisType_p[assign_a(arg.type)] | argEigenType_p | eigenRef_p | classArg_p)
- >> name_p[assign_a(arg.name)])
- [push_back_a(args, arg)]
- [assign_a(arg,arg0)];
-
- Rule argumentList_p = !argument_p >> * (',' >> argument_p);
-
- Rule constructor_p =
- (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p)
- [push_back_a(constructor.args_list, args)]
- [assign_a(args,args0)];
- //[assign_a(constructor.args,args)]
- //[assign_a(constructor.name,cls.name)]
- //[push_back_a(cls.constructors, constructor)]
- //[assign_a(constructor,constructor0)];
-
- 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)]) |
- ((*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)]) |
- ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)]
- >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >>
- !ch_p('*') [assign_a(retVal.isPtr2,true)]) |
- (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::EIGEN)]);
-
- Rule pair_p =
- (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>')
- [assign_a(retVal.isPair,true)];
-
- Rule void_p = str_p("void")[assign_a(retVal.type1)];
-
- Rule returnType_p = void_p | returnType1_p | pair_p;
-
- Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')];
-
- Rule method_p =
- (returnType_p >> methodName_p[assign_a(methodName)] >>
- '(' >> argumentList_p >> ')' >>
- !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p)
- [bl::bind(&Method::addOverload,
- bl::var(cls.methods)[bl::var(methodName)],
- verbose,
- bl::var(isConst),
- bl::var(methodName),
- bl::var(args),
- bl::var(retVal))]
- [assign_a(isConst,isConst0)]
- [assign_a(methodName,methodName0)]
- [assign_a(args,args0)]
- [assign_a(retVal,retVal0)];
-
- Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
-
- Rule static_method_p =
- (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(methodName)] >>
- '(' >> argumentList_p >> ')' >> ';' >> *comments_p)
- [bl::bind(&StaticMethod::addOverload,
- bl::var(cls.static_methods)[bl::var(methodName)],
- verbose,
- bl::var(methodName),
- bl::var(args),
- bl::var(retVal))]
- [assign_a(methodName,methodName0)]
- [assign_a(args,args0)]
- [assign_a(retVal,retVal0)];
-
- Rule functions_p = constructor_p | method_p | static_method_p;
-
- Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[assign_a(include_path)] >> ch_p('>');
-
- Rule class_p =
- (!*include_p
- >> !(str_p("virtual")[assign_a(cls.isVirtual, true)])
- >> str_p("class")[push_back_a(cls.includes, include_path)][assign_a(include_path, null_str)]
- >> className_p[assign_a(cls.name)]
- >> ((':' >> classParent_p >> '{') | '{') // By having (parent >> '{' | '{') here instead of (!parent >> '{'), we trigger a parse error on a badly-formed parent spec
- >> *(functions_p | comments_p)
- >> str_p("};"))
- [assign_a(constructor.name, cls.name)]
- [assign_a(cls.constructor, constructor)]
- [assign_a(cls.namespaces, namespaces)]
- [assign_a(cls.using_namespaces, using_namespace_current)]
- [append_a(cls.includes, namespace_includes)]
- [assign_a(deconstructor.name,cls.name)]
- [assign_a(cls.deconstructor, deconstructor)]
- [push_back_a(classes, cls)]
- [assign_a(deconstructor,deconstructor0)]
- [assign_a(constructor, constructor0)]
- [assign_a(cls,cls0)];
-
- Rule namespace_def_p =
- (!*include_p
- >> str_p("namespace")[push_back_a(namespace_includes, include_path)][assign_a(include_path, null_str)]
- >> namespace_name_p[push_back_a(namespaces)]
- >> ch_p('{')
- >> *(class_p | namespace_def_p | comments_p)
- >> str_p("}///\\namespace") // end namespace, avoid confusion with classes
- >> !namespace_name_p)
- [pop_a(namespaces)]
- [pop_a(namespace_includes)];
-
- Rule using_namespace_p =
- str_p("using") >> str_p("namespace")
- >> namespace_name_p[push_back_a(using_namespace_current)] >> ch_p(';');
-
- Rule forward_declaration_p =
- !(str_p("virtual")[assign_a(fwDec.isVirtual, true)])
- >> str_p("class")
- >> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)]
- >> ch_p(';')
- [push_back_a(forward_declarations, fwDec)]
- [assign_a(fwDec, fwDec0)];
-
- Rule module_content_p = comments_p | using_namespace_p | class_p | forward_declaration_p | namespace_def_p ;
-
- Rule module_p = *module_content_p >> !end_p;
-
- //----------------------------------------------------------------------------
- // for debugging, define BOOST_SPIRIT_DEBUG
-# ifdef BOOST_SPIRIT_DEBUG
- BOOST_SPIRIT_DEBUG_NODE(className_p);
- BOOST_SPIRIT_DEBUG_NODE(classPtr_p);
- BOOST_SPIRIT_DEBUG_NODE(classRef_p);
- BOOST_SPIRIT_DEBUG_NODE(basisType_p);
- BOOST_SPIRIT_DEBUG_NODE(name_p);
- BOOST_SPIRIT_DEBUG_NODE(argument_p);
- BOOST_SPIRIT_DEBUG_NODE(argumentList_p);
- BOOST_SPIRIT_DEBUG_NODE(constructor_p);
- BOOST_SPIRIT_DEBUG_NODE(returnType1_p);
- BOOST_SPIRIT_DEBUG_NODE(returnType2_p);
- BOOST_SPIRIT_DEBUG_NODE(pair_p);
- BOOST_SPIRIT_DEBUG_NODE(void_p);
- BOOST_SPIRIT_DEBUG_NODE(returnType_p);
- BOOST_SPIRIT_DEBUG_NODE(methodName_p);
- BOOST_SPIRIT_DEBUG_NODE(method_p);
- BOOST_SPIRIT_DEBUG_NODE(class_p);
- BOOST_SPIRIT_DEBUG_NODE(namespace_def_p);
- BOOST_SPIRIT_DEBUG_NODE(module_p);
-# endif
- //----------------------------------------------------------------------------
-
- // read interface file
- string interfaceFile = interfacePath + "/" + moduleName + ".h";
- string contents = file_contents(interfaceFile);
-
- // and parse contents
- parse_info info = parse(contents.c_str(), module_p, space_p);
- if(!info.full) {
- printf("parsing stopped at \n%.20s\n",info.stop);
- throw ParseFailed(info.length);
- }
-}
-
-/* ************************************************************************* */
-template
-void verifyArguments(const vector& validArgs, const map& vt) {
- typedef typename map::value_type Name_Method;
- BOOST_FOREACH(const Name_Method& name_method, vt) {
- const T& t = name_method.second;
- BOOST_FOREACH(const ArgumentList& argList, t.argLists) {
- BOOST_FOREACH(Argument arg, argList) {
- string fullType = arg.qualifiedType("::");
- if(find(validArgs.begin(), validArgs.end(), fullType)
- == validArgs.end())
- throw DependencyMissing(fullType, t.name);
- }
- }
- }
-}
-
-/* ************************************************************************* */
-template
-void verifyReturnTypes(const vector& validtypes, const map& vt) {
- typedef typename map::value_type Name_Method;
- BOOST_FOREACH(const Name_Method& name_method, vt) {
- const T& t = name_method.second;
- BOOST_FOREACH(const ReturnValue& retval, t.returnVals) {
- if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end())
- throw DependencyMissing(retval.qualifiedType1("::"), t.name);
- if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end())
- throw DependencyMissing(retval.qualifiedType2("::"), t.name);
- }
- }
-}
-
-/* ************************************************************************* */
-void Module::matlab_code(const string& toolboxPath, const string& headerPath) const {
-
- fs::create_directories(toolboxPath);
-
- // create the unified .cpp switch file
- const string wrapperName = name + "_wrapper";
- string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
- FileWriter wrapperFile(wrapperFileName, verbose, "//");
- vector functionNames; // Function names stored by index for switch
- wrapperFile.oss << "#include \n";
- wrapperFile.oss << "#include