diff --git a/.cproject b/.cproject index 9481bd99c..959372067 100644 --- a/.cproject +++ b/.cproject @@ -322,6 +322,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -348,7 +356,6 @@ make - tests/testBayesTree.run true false @@ -356,7 +363,6 @@ make - testBinaryBayesNet.run true false @@ -404,7 +410,6 @@ make - testSymbolicBayesNet.run true false @@ -412,7 +417,6 @@ make - tests/testSymbolicFactor.run true false @@ -420,7 +424,6 @@ make - testSymbolicFactorGraph.run true false @@ -436,20 +439,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -484,6 +478,7 @@ make + testGraph.run true false @@ -579,6 +574,7 @@ make + testInference.run true false @@ -586,6 +582,7 @@ make + testGaussianBayesNet.run true false @@ -593,6 +590,7 @@ make + testGaussianFactor.run true false @@ -600,6 +598,7 @@ make + testJunctionTree.run true false @@ -607,6 +606,7 @@ make + testSymbolicBayesNet.run true false @@ -614,6 +614,7 @@ make + testSymbolicFactorGraph.run true false @@ -851,6 +852,14 @@ true true + + make + -j2 + testNonlinearFactorGraph.run + true + true + true + make -j2 @@ -949,7 +958,6 @@ make - testErrors.run true false @@ -1309,6 +1317,7 @@ make + testSimulated2DOriented.run true false @@ -1348,6 +1357,7 @@ make + testSimulated2D.run true false @@ -1355,6 +1365,7 @@ make + testSimulated3D.run true false @@ -1410,6 +1421,7 @@ make + tests/testGaussianISAM2 true false @@ -1431,86 +1443,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 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -1607,6 +1539,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 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -1639,14 +1659,6 @@ true true - - make - -j2 - check - true - true - true - @@ -1969,6 +1981,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1995,7 +2015,6 @@ make - tests/testBayesTree.run true false @@ -2003,7 +2022,6 @@ make - testBinaryBayesNet.run true false @@ -2051,7 +2069,6 @@ make - testSymbolicBayesNet.run true false @@ -2059,7 +2076,6 @@ make - tests/testSymbolicFactor.run true false @@ -2067,7 +2083,6 @@ make - testSymbolicFactorGraph.run true false @@ -2083,20 +2098,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2131,6 +2137,7 @@ make + testGraph.run true false @@ -2226,6 +2233,7 @@ make + testInference.run true false @@ -2233,6 +2241,7 @@ make + testGaussianBayesNet.run true false @@ -2240,6 +2249,7 @@ make + testGaussianFactor.run true false @@ -2247,6 +2257,7 @@ make + testJunctionTree.run true false @@ -2254,6 +2265,7 @@ make + testSymbolicBayesNet.run true false @@ -2261,6 +2273,7 @@ make + testSymbolicFactorGraph.run true false @@ -2498,6 +2511,14 @@ true true + + make + -j2 + testNonlinearFactorGraph.run + true + true + true + make -j2 @@ -2596,7 +2617,6 @@ make - testErrors.run true false @@ -2956,6 +2976,7 @@ make + testSimulated2DOriented.run true false @@ -2995,6 +3016,7 @@ make + testSimulated2D.run true false @@ -3002,6 +3024,7 @@ make + testSimulated3D.run true false @@ -3057,6 +3080,7 @@ make + tests/testGaussianISAM2 true false @@ -3078,86 +3102,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 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -3254,6 +3198,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 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -3286,14 +3318,6 @@ true true - - make - -j2 - check - true - true - true - diff --git a/gtsam/inference/FactorBase.h b/gtsam/inference/FactorBase.h index 1c848e2cb..4f8bda92b 100644 --- a/gtsam/inference/FactorBase.h +++ b/gtsam/inference/FactorBase.h @@ -25,6 +25,7 @@ #include #include // for noncopyable #include +#include #include #include #include @@ -99,6 +100,12 @@ public: FactorBase(Key key1, Key key2, Key key3, Key key4) : keys_(4) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } + /** Construct n-way factor */ + FactorBase(std::set keys) { + BOOST_FOREACH(const Key& key, keys) + keys_.push_back(key); + assertInvariants(); } + /** Create a combined joint factor (new style for EliminationTree). */ template static typename DERIVED::shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 836466bb4..acefd93f4 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -59,6 +59,9 @@ public: /** Construct 4-way factor */ IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) {} + /** Construct n-way factor */ + IndexFactor(std::set js) : Base(js) {} + /** Create a combined joint factor (new style for EliminationTree). */ static shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph-inl.h index a0f262fbd..cb0ed0288 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph-inl.h @@ -58,6 +58,17 @@ void NonlinearFactorGraph::print(const std::string& str) const { return total_error; } + /* ************************************************************************* */ + /* ************************************************************************* */ + template + std::set NonlinearFactorGraph::keys() const { + std::set keys; + BOOST_FOREACH(const sharedFactor& factor, this->factors_) + keys.insert(factor->begin(), factor->end()); + return keys; + } + + /* ************************************************************************* */ template Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(const VALUES& config) const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index dad79ca22..bdd27442a 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -49,6 +49,9 @@ namespace gtsam { /** print just calls base class */ void print(const std::string& str = "NonlinearFactorGraph: ") const; + /** return keys in some random order */ + std::set keys() const; + /** unnormalized error */ double error(const VALUES& c) const; @@ -92,6 +95,7 @@ namespace gtsam { boost::shared_ptr linearize(const VALUES& config, const Ordering& ordering) const; + }; } // namespace diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9052681df..0e8eca8eb 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -22,6 +22,7 @@ using namespace std; #include +#include using namespace boost::assign; #include @@ -57,6 +58,18 @@ TEST( Graph, error ) DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); } +/* ************************************************************************* */ +TEST( Graph, keys ) +{ + Graph fg = createNonlinearFactorGraph(); + set actual = fg.keys(); + LONGS_EQUAL(3, actual.size()); + set::const_iterator it = actual.begin(); + CHECK(assert_equal(Symbol('l', 1), *(it++))); + CHECK(assert_equal(Symbol('x', 1), *(it++))); + CHECK(assert_equal(Symbol('x', 2), *(it++))); +} + /* ************************************************************************* */ TEST( Graph, GET_ORDERING) {