diff --git a/.cproject b/.cproject
index 63e07de34..0a8bd560e 100644
--- a/.cproject
+++ b/.cproject
@@ -492,6 +492,14 @@
false
true
+
+make
+
+testSymbolicFactorGraph.run
+true
+false
+true
+
make
diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp
index ccb1b450e..05076cd7e 100644
--- a/cpp/ConstrainedLinearFactorGraph.cpp
+++ b/cpp/ConstrainedLinearFactorGraph.cpp
@@ -107,7 +107,7 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
ConstrainedConditionalGaussian::shared_ptr ccg = constraint->eliminate(key);
// perform a change of variables on the linear factors in the separator
- vector separator = find_factors_and_remove(key);
+ vector separator = findAndRemoveFactors(key);
BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) {
// store the block matrices
map blocks;
diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index 34aa82754..52834f554 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -171,13 +171,13 @@ list FactorGraph::factors(const string& key) const {
/* ************************************************************************* */
template
vector >
-FactorGraph::find_factors_and_remove(const string& key) {
+FactorGraph::findAndRemoveFactors(const string& key) {
vector > found;
Indices::iterator it = indices_.find(key);
if (it == indices_.end())
throw(std::invalid_argument
- ("FactorGraph::find_factors_and_remove invalid key: " + key));
+ ("FactorGraph::findAndRemoveFactors invalid key: " + key));
list *indices_ptr; // pointer to indices list in indices_ map
indices_ptr = &(it->second);
@@ -190,5 +190,18 @@ FactorGraph::find_factors_and_remove(const string& key) {
return found;
}
+/* ************************************************************************* */
+/* find factors and remove them from the factor graph: O(n) */
+/* ************************************************************************* */
+template
+boost::shared_ptr
+FactorGraph::removeAndCombineFactors(const string& key)
+{
+ typedef typename boost::shared_ptr shared_factor;
+ vector found = findAndRemoveFactors(key);
+ shared_factor new_factor(new Factor(found));
+ return new_factor;
+}
+
/* ************************************************************************* */
}
diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h
index e39fe1448..6f8df8014 100644
--- a/cpp/FactorGraph.h
+++ b/cpp/FactorGraph.h
@@ -93,7 +93,14 @@ namespace gtsam {
* from the factor graph
* @param key the key for the given node
*/
- std::vector find_factors_and_remove(const std::string& key);
+ std::vector findAndRemoveFactors(const std::string& key);
+
+ /**
+ * extract and combine all the factors that involve a given node
+ * @param key the key for the given node
+ * @return the combined linear factor
+ */
+ shared_factor removeAndCombineFactors(const std::string& key);
private:
diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp
index 2e4d9147b..57ce004e9 100644
--- a/cpp/LinearFactorGraph.cpp
+++ b/cpp/LinearFactorGraph.cpp
@@ -13,6 +13,7 @@
#include
#include "ChordalBayesNet.h"
+#include "FactorGraph-inl.h"
#include "LinearFactorGraph.h"
using namespace std;
@@ -47,17 +48,6 @@ set LinearFactorGraph::find_separator(const string& key) const
return separator;
}
-/* ************************************************************************* */
-/* find factors and remove them from the factor graph: O(n) */
-/* ************************************************************************* */
-boost::shared_ptr
-LinearFactorGraph::combine_factors(const string& key)
-{
- vector found = find_factors_and_remove(key);
- boost::shared_ptr lf(new LinearFactor(found));
- return lf;
-}
-
/* ************************************************************************* */
/* eliminate one node from the linear factor graph */
/* ************************************************************************* */
@@ -65,7 +55,7 @@ ConditionalGaussian::shared_ptr LinearFactorGraph::eliminate_one(const string& k
{
// combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor
- boost::shared_ptr joint_factor = combine_factors(key);
+ boost::shared_ptr joint_factor = removeAndCombineFactors(key);
// eliminate that joint factor
try {
diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h
index e3e7668c2..a15c64426 100644
--- a/cpp/LinearFactorGraph.h
+++ b/cpp/LinearFactorGraph.h
@@ -67,17 +67,6 @@ namespace gtsam {
*/
std::set find_separator(const std::string& key) const;
- /**
- * extract and combine all the factors that involve a given node
- * NOTE: the combined factor will be depends on a system-dependent
- * ordering of the input set of factors. Do not rely on this order
- * when using the function.
- * @param key the key for the given node
- * @return the combined linear factor
- */
- boost::shared_ptr
- combine_factors(const std::string& key);
-
/**
* eliminate one node yielding a ConditionalGaussian
* Eliminates the factors from the factor graph through find_factors_and_remove
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 3ea06573a..da71d4176 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -81,15 +81,16 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include
timeLinearFactor: LDFLAGS += -L.libs -lgtsam
# graphs
-sources += LinearFactorGraph.cpp
-sources += SymbolicBayesChain.cpp
-sources += ChordalBayesNet.cpp
+sources += SymbolicFactorGraph.cpp LinearFactorGraph.cpp
+sources += SymbolicBayesChain.cpp ChordalBayesNet.cpp
sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp
-check_PROGRAMS += testFactorgraph testLinearFactorGraph testNonlinearFactorGraph
+check_PROGRAMS += testFactorgraph testSymbolicFactorGraph
+check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph
check_PROGRAMS += testChordalBayesNet testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesChain testBayesTree
check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph
testFactorgraph_SOURCES = testFactorgraph.cpp
+testSymbolicFactorGraph_SOURCES = $(example) testSymbolicFactorGraph.cpp
testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
@@ -100,6 +101,7 @@ testConstrainedNonlinearFactorGraph_SOURCES = $(example) testConstrainedNonlinea
testConstrainedLinearFactorGraph_SOURCES = $(example) testConstrainedLinearFactorGraph.cpp
testFactorgraph_LDADD = libgtsam.la
+testSymbolicFactorGraph_LDADD = libgtsam.la
testLinearFactorGraph_LDADD = libgtsam.la
testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_LDADD = libgtsam.la
diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp
new file mode 100644
index 000000000..2e4d18ccb
--- /dev/null
+++ b/cpp/SymbolicFactorGraph.cpp
@@ -0,0 +1,47 @@
+/*
+ * SymbolicFactorGraph.cpp
+ *
+ * Created on: Oct 29, 2009
+ * Author: Frank Dellaert
+ */
+
+#include
+#include
+#include "SymbolicFactorGraph.h"
+
+using namespace std;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+namespace gtsam {
+
+ /* ************************************************************************* */
+ SymbolicFactor::SymbolicFactor(const vector & factors) {
+
+ // store keys in a map to make them unique (set is not portable)
+ map map;
+ BOOST_FOREACH(shared_ptr factor, factors)
+ BOOST_FOREACH(string key, factor->keys())
+ map.insert(make_pair(key,key));
+
+ // create the unique keys
+ string key,val;
+ FOREACH_PAIR(key, val, map)
+ keys_.push_back(key);
+ }
+
+ /* ************************************************************************* */
+ void SymbolicFactor::print(const string& s) const {
+ cout << s << " ";
+ BOOST_FOREACH(string key, keys_) cout << key << " ";
+ cout << endl;
+ }
+
+ /* ************************************************************************* */
+ bool SymbolicFactor::equals(const SymbolicFactor& other, double tol) const {
+ return keys_ == other.keys_;
+ }
+ /* ************************************************************************* */
+
+}
diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h
new file mode 100644
index 000000000..8682f6617
--- /dev/null
+++ b/cpp/SymbolicFactorGraph.h
@@ -0,0 +1,76 @@
+/*
+ * SymbolicFactorGraph.h
+ *
+ * Created on: Oct 29, 2009
+ * Author: Frank Dellaert
+ */
+
+#ifndef SYMBOLICFACTORGRAPH_H_
+#define SYMBOLICFACTORGRAPH_H_
+
+#include
+#include
+#include "FactorGraph.h"
+
+namespace gtsam {
+
+ /** Symbolic Factor */
+ class SymbolicFactor: public Testable {
+ private:
+
+ std::list keys_;
+
+ public:
+
+ typedef boost::shared_ptr shared_ptr;
+
+ /**
+ * Constructor from a list of keys
+ */
+ SymbolicFactor(std::list keys) :
+ keys_(keys) {
+ }
+
+ /**
+ * Constructor that combines a set of factors
+ * @param factors Set of factors to combine
+ */
+ SymbolicFactor(const std::vector & factors);
+
+ /** print */
+ void print(const std::string& s = "SymbolicFactor") const;
+
+ /** check equality */
+ bool equals(const SymbolicFactor& other, double tol = 1e-9) const;
+
+ /**
+ * Find all variables
+ * @return The set of all variable keys
+ */
+ std::list keys() const {
+ return keys_;
+ }
+ };
+
+ /** Symbolic Factor Graph */
+ class SymbolicFactorGraph: public FactorGraph {
+ public:
+
+ SymbolicFactorGraph() {
+ }
+
+ template
+ SymbolicFactorGraph(const FactorGraph& fg) {
+ for (size_t i = 0; i < fg.size(); i++) {
+ boost::shared_ptr f = fg[i];
+ std::list keys = f->keys();
+ SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
+ push_back(factor);
+ }
+ }
+
+ };
+
+}
+
+#endif /* SYMBOLICFACTORGRAPH_H_ */
diff --git a/cpp/testConstrainedNonlinearFactorGraph.cpp b/cpp/testConstrainedNonlinearFactorGraph.cpp
index 22b14968b..feddfc447 100644
--- a/cpp/testConstrainedNonlinearFactorGraph.cpp
+++ b/cpp/testConstrainedNonlinearFactorGraph.cpp
@@ -7,6 +7,7 @@
#include
+#include "FactorGraph-inl.h"
#include "ConstrainedNonlinearFactorGraph.h"
#include "smallExample.h"
diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp
index 67287b146..da5e107bb 100644
--- a/cpp/testLinearFactorGraph.cpp
+++ b/cpp/testLinearFactorGraph.cpp
@@ -63,20 +63,13 @@ TEST( LinearFactorGraph, find_separator )
}
/* ************************************************************************* */
-// Note: This test fails on different systems due to a dependency on the ordering
-// of LinearFactor::shared_ptr in STL sets. Because the ordering is based
-// on pointer values instead of factor values, the ordering is different
-// between separate systems.
TEST( LinearFactorGraph, combine_factors_x1 )
{
// create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
// combine all factors
- LinearFactor::shared_ptr actual = fg.combine_factors("x1");
-
- //FIXME: this expected value must be constructed in the order that
- // the factors are stored in a set - which differs between systems
+ LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
// the expected linear factor
Matrix Al1 = Matrix_(6,2,
@@ -128,10 +121,7 @@ TEST( LinearFactorGraph, combine_factors_x2 )
LinearFactorGraph fg = createLinearFactorGraph();
// combine all factors
- LinearFactor::shared_ptr actual = fg.combine_factors("x2");
-
- // FIXME: change the ordering of the expected to match whatever the real
- // ordering is and constructed the expected with the correct order
+ LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x2");
// the expected linear factor
Matrix Al1 = Matrix_(4,2,
@@ -476,7 +466,7 @@ TEST( LinearFactorGraph, factor_lookup)
}
/* ************************************************************************* */
-TEST( LinearFactorGraph, find_factors_and_remove )
+TEST( LinearFactorGraph, findAndRemoveFactors )
{
// create the graph
LinearFactorGraph fg = createLinearFactorGraph();
@@ -487,7 +477,7 @@ TEST( LinearFactorGraph, find_factors_and_remove )
LinearFactor::shared_ptr f2 = fg[2];
// call the function
- vector factors = fg.find_factors_and_remove("x1");
+ vector factors = fg.findAndRemoveFactors("x1");
// Check the factors
CHECK(f0==factors[0]);
@@ -499,7 +489,7 @@ TEST( LinearFactorGraph, find_factors_and_remove )
}
/* ************************************************************************* */
-TEST( LinearFactorGraph, find_factors_and_remove__twice )
+TEST( LinearFactorGraph, findAndRemoveFactors_twice )
{
// create the graph
LinearFactorGraph fg = createLinearFactorGraph();
@@ -510,14 +500,14 @@ TEST( LinearFactorGraph, find_factors_and_remove__twice )
LinearFactor::shared_ptr f2 = fg[2];
// call the function
- vector factors = fg.find_factors_and_remove("x1");
+ vector factors = fg.findAndRemoveFactors("x1");
// Check the factors
CHECK(f0==factors[0]);
CHECK(f1==factors[1]);
CHECK(f2==factors[2]);
- factors = fg.find_factors_and_remove("x1");
+ factors = fg.findAndRemoveFactors("x1");
CHECK(factors.size() == 0);
// CHECK if the factors are deleted from the factor graph
diff --git a/cpp/testSymbolicBayesChain.cpp b/cpp/testSymbolicBayesChain.cpp
index 334655176..9837c4bb1 100644
--- a/cpp/testSymbolicBayesChain.cpp
+++ b/cpp/testSymbolicBayesChain.cpp
@@ -11,135 +11,9 @@
#include "BayesChain-inl.h"
#include "SymbolicBayesChain-inl.h"
-namespace gtsam {
-
- /** Symbolic Factor */
- class SymbolicFactor: public Testable {
- private:
-
- std::list keys_;
-
- public:
-
- SymbolicFactor(std::list keys) :
- keys_(keys) {
- }
-
- typedef boost::shared_ptr shared_ptr;
-
- /** print */
- void print(const std::string& s = "SymbolicFactor") const {
- cout << s << " ";
- BOOST_FOREACH(string key, keys_) cout << key << " ";
- cout << endl;
- }
-
- /** check equality */
- bool equals(const SymbolicFactor& other, double tol = 1e-9) const {
- return keys_ == other.keys_;
- }
-
- /**
- * Find all variables
- * @return The set of all variable keys
- */
- std::list keys() const {
- return keys_;
- }
- };
-
- /** Symbolic Factor Graph */
- class SymbolicFactorGraph: public FactorGraph {
- public:
-
- SymbolicFactorGraph() {}
-
- template
- SymbolicFactorGraph(const FactorGraph& fg) {
- for (size_t i = 0; i < fg.size(); i++) {
- boost::shared_ptr f = fg[i];
- std::list keys = f->keys();
- SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
- push_back(factor);
- }
- }
-
- };
-
-}
-
using namespace std;
using namespace gtsam;
-/* ************************************************************************* */
-TEST( SymbolicFactorGraph, symbolicFactorGraph )
-{
- // construct expected symbolic graph
- SymbolicFactorGraph expected;
-
- list f1_keys; f1_keys.push_back("x1");
- SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
- expected.push_back(f1);
-
- list f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
- SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
- expected.push_back(f2);
-
- list f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
- SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
- expected.push_back(f3);
-
- list f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
- SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
- expected.push_back(f4);
-
- // construct it from the factor graph graph
- LinearFactorGraph factorGraph = createLinearFactorGraph();
- SymbolicFactorGraph actual(factorGraph);
-
- CHECK(assert_equal(expected, actual));
-}
-
-/* ************************************************************************* */
-TEST( SymbolicFactorGraph, find_factors_and_remove )
-{
- // construct it from the factor graph graph
- LinearFactorGraph factorGraph = createLinearFactorGraph();
- SymbolicFactorGraph actual(factorGraph);
- SymbolicFactor::shared_ptr f1 = actual[0];
- SymbolicFactor::shared_ptr f3 = actual[2];
- actual.find_factors_and_remove("x2");
-
- // construct expected graph after find_factors_and_remove
- SymbolicFactorGraph expected;
- SymbolicFactor::shared_ptr null;
- expected.push_back(f1);
- expected.push_back(null);
- expected.push_back(f3);
- expected.push_back(null);
-
- CHECK(assert_equal(expected, actual));
-}
-/* ************************************************************************* */
-TEST( SymbolicFactorGraph, factor_lookup)
-{
- // create a test graph
- LinearFactorGraph factorGraph = createLinearFactorGraph();
- SymbolicFactorGraph fg(factorGraph);
-
- // ask for all factor indices connected to x1
- list x1_factors = fg.factors("x1");
- int x1_indices[] = { 0, 1, 2 };
- list x1_expected(x1_indices, x1_indices + 3);
- CHECK(x1_factors==x1_expected);
-
- // ask for all factor indices connected to x2
- list x2_factors = fg.factors("x2");
- int x2_indices[] = { 1, 3 };
- list x2_expected(x2_indices, x2_indices + 2);
- CHECK(x2_factors==x2_expected);
-}
-
/* ************************************************************************* */
TEST( SymbolicBayesChain, constructor )
{
diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp
new file mode 100644
index 000000000..5c92c8819
--- /dev/null
+++ b/cpp/testSymbolicFactorGraph.cpp
@@ -0,0 +1,107 @@
+/**
+ * @file testSymbolicBayesChain.cpp
+ * @brief Unit tests for a symbolic Bayes chain
+ * @author Frank Dellaert
+ */
+
+#include
+
+#include "smallExample.h"
+#include "FactorGraph-inl.h"
+#include "SymbolicFactorGraph.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, symbolicFactorGraph )
+{
+ // construct expected symbolic graph
+ SymbolicFactorGraph expected;
+
+ list f1_keys; f1_keys.push_back("x1");
+ SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
+ expected.push_back(f1);
+
+ list f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
+ SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
+ expected.push_back(f2);
+
+ list f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
+ SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
+ expected.push_back(f3);
+
+ list f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
+ SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
+ expected.push_back(f4);
+
+ // construct it from the factor graph graph
+ LinearFactorGraph factorGraph = createLinearFactorGraph();
+ SymbolicFactorGraph actual(factorGraph);
+
+ CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, findAndRemoveFactors )
+{
+ // construct it from the factor graph graph
+ LinearFactorGraph factorGraph = createLinearFactorGraph();
+ SymbolicFactorGraph actual(factorGraph);
+ SymbolicFactor::shared_ptr f1 = actual[0];
+ SymbolicFactor::shared_ptr f3 = actual[2];
+ actual.findAndRemoveFactors("x2");
+
+ // construct expected graph after find_factors_and_remove
+ SymbolicFactorGraph expected;
+ SymbolicFactor::shared_ptr null;
+ expected.push_back(f1);
+ expected.push_back(null);
+ expected.push_back(f3);
+ expected.push_back(null);
+
+ CHECK(assert_equal(expected, actual));
+}
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, factors)
+{
+ // create a test graph
+ LinearFactorGraph factorGraph = createLinearFactorGraph();
+ SymbolicFactorGraph fg(factorGraph);
+
+ // ask for all factor indices connected to x1
+ list x1_factors = fg.factors("x1");
+ int x1_indices[] = { 0, 1, 2 };
+ list x1_expected(x1_indices, x1_indices + 3);
+ CHECK(x1_factors==x1_expected);
+
+ // ask for all factor indices connected to x2
+ list x2_factors = fg.factors("x2");
+ int x2_indices[] = { 1, 3 };
+ list x2_expected(x2_indices, x2_indices + 2);
+ CHECK(x2_factors==x2_expected);
+}
+
+/* ************************************************************************* */
+TEST( LinearFactorGraph, removeAndCombineFactors )
+{
+ // create a test graph
+ LinearFactorGraph factorGraph = createLinearFactorGraph();
+ SymbolicFactorGraph fg(factorGraph);
+
+ // combine all factors connected to x1
+ SymbolicFactor::shared_ptr actual = fg.removeAndCombineFactors("x1");
+
+ list keys; keys.push_back("l1"); keys.push_back("x1"); keys.push_back("x2");
+ SymbolicFactor expected(keys);
+
+ // check if the two factors are the same
+ CHECK(assert_equal(expected,*actual));
+}
+
+/* ************************************************************************* */
+int main() {
+ TestResult tr;
+ return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */