BayesTree::update generalized and moved from testIncremental to BayesTree; testIncremental removed

.cproject

@@ -300,7 +300,6 @@
 <buildTargets>
 <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -308,7 +307,6 @@
 </target>
 <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -324,7 +322,6 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -340,6 +337,7 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -347,7 +345,6 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -355,6 +352,7 @@
 </target>
 <target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianConditional.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -362,7 +360,6 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -370,7 +367,6 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -378,6 +374,7 @@
 </target>
 <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -385,7 +382,6 @@
 </target>
 <target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testGaussianFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -393,7 +389,6 @@
 </target>
 <target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testGaussianFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -401,6 +396,7 @@
 </target>
 <target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -408,7 +404,6 @@
 </target>
 <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testPose3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -416,6 +411,7 @@
 </target>
 <target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVectorConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -423,6 +419,7 @@
 </target>
 <target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPoint2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -430,7 +427,6 @@
 </target>
 <target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -438,7 +434,6 @@
 </target>
 <target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>timeGaussianFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -446,7 +441,6 @@
 </target>
 <target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>timeGaussianFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -454,7 +448,6 @@
 </target>
 <target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testGaussianBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -462,6 +455,7 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -469,7 +463,6 @@
 </target>
 <target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSymbolicBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -477,6 +470,7 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -484,7 +478,6 @@
 </target>
 <target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVector.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -492,21 +485,14 @@
 </target>
 <target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testMatrix.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testIncremental.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-<buildCommand>make</buildCommand>
-<buildTarget>testIncremental.run</buildTarget>
-<stopOnError>true</stopOnError>
-<useDefaultCommand>true</useDefaultCommand>
-<runAllBuilders>true</runAllBuilders>
-</target>
 <target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVSLAMGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -514,6 +500,7 @@
 </target>
 <target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearEquality.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -521,7 +508,6 @@
 </target>
 <target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSQP.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -529,6 +515,7 @@
 </target>
 <target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearConstraint.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -536,6 +523,7 @@
 </target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -543,6 +531,7 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -550,6 +539,7 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/BayesTree-inl.h

@@ -2,6 +2,8 @@
  * @file    BayesTree.cpp
  * @brief   Bayes Tree is a tree of cliques of a Bayes Chain
  * @author  Frank Dellaert
+ * @author  Michael Kaess
+ * @author  Viorela Ila
  */
 
 #include <boost/foreach.hpp>
@@ -417,6 +419,45 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+	template<class Conditional>
+	template<class Factor>
+	void BayesTree<Conditional>::update(const FactorGraph<Factor>& newFactors) {
+		// Remove the contaminated part of the Bayes tree
+		FactorGraph<Factor> factors;
+		typename BayesTree<Conditional>::Cliques orphans;
+		BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors) {
+
+			FactorGraph<Factor> factors1;
+			typename BayesTree<Conditional>::Cliques orphans1;
+			boost::tie(factors1, orphans1) = removeTop<Factor>(factor);
+
+			factors.push_back(factors1);
+			orphans.splice (orphans.begin(), orphans1);
+		}
+
+		// add the factors themselves
+		factors.push_back(newFactors);
+
+		// create an ordering for the new and contaminated factors
+		Ordering ordering = factors.getOrdering();
+
+		// eliminate into a Bayes net
+		BayesNet<Conditional> bayesNet = eliminate<Factor, Conditional>(factors,ordering);
+
+		// insert conditionals back in, straight into the topless bayesTree
+		typename BayesNet<Conditional>::const_reverse_iterator rit;
+		for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
+			insert(*rit);
+
+		// add orphans to the bottom of the new tree
+		BOOST_FOREACH(typename BayesTree<Conditional>::sharedClique orphan, orphans) {
+			string key = orphan->separator_.front(); // todo: assumes there is a separator...
+			typename BayesTree<Conditional>::sharedClique parent = (*this)[key];
+			parent->children_ += orphan;
+		}
+
+	}
+	/* ************************************************************************* */
 
 }
 /// namespace gtsam

cpp/BayesTree.h

@@ -177,6 +177,12 @@ namespace gtsam {
 		template<class Factor>
 		std::pair<FactorGraph<Factor>, Cliques> removeTop(const boost::shared_ptr<Factor>& newFactor);
 
+		/**
+		 * iSAM.
+		 */
+		template<class Factor>
+		void update(const FactorGraph<Factor>& newFactors);
+
 	}; // BayesTree
 
 } /// namespace gtsam

cpp/Makefile.am

@@ -64,15 +64,13 @@ example = smallExample.cpp
 headers += inference.h inference-inl.h
 headers += FactorGraph.h FactorGraph-inl.h
 headers += BayesNet.h BayesNet-inl.h BayesTree.h BayesTree-inl.h
-check_PROGRAMS += testFactorgraph testBayesTree testInference testIncremental
+check_PROGRAMS += testFactorgraph testBayesTree testInference
 testFactorgraph_SOURCES        = testFactorgraph.cpp
 testBayesTree_SOURCES          = $(example) testBayesTree.cpp
 testInference_SOURCES          = $(example) testInference.cpp
-testIncremental_SOURCES        = $(example) testIncremental.cpp
 testFactorgraph_LDADD          = libgtsam.la
 testBayesTree_LDADD            = libgtsam.la
 testInference_LDADD            = libgtsam.la
-testIncremental_LDADD          = libgtsam.la
 
 # Symbolic Inference
 headers += SymbolicConditional.h

cpp/testBayesTree.cpp

@@ -2,6 +2,8 @@
  * @file    testBayesTree.cpp
  * @brief   Unit tests for Bayes Tree
  * @author  Frank Dellaert
+ * @author  Michael Kaess
+ * @author  Viorela Ila
  */
 
 #include <boost/assign/std/list.hpp> // for operator +=
@@ -435,6 +437,58 @@ TEST( BayesTree, removeTop )
   CHECK(assert_equal(expectedOrphans2, orphans));
 }
 
+
+/* ************************************************************************* */
+TEST( BayesTree, iSAM )
+{
+	// Conditionals for ASIA example from the tutorial with A and D evidence
+	SymbolicConditional::shared_ptr
+		B(new SymbolicConditional("B")),
+		L(new SymbolicConditional("L", "B")),
+		E(new SymbolicConditional("E", "B", "L")),
+		S(new SymbolicConditional("S", "L", "B")),
+		T(new SymbolicConditional("T", "E", "L")),
+		X(new SymbolicConditional("X", "E"));
+
+	// Create using insert
+	SymbolicBayesTree bayesTree;
+	bayesTree.insert(B);
+	bayesTree.insert(L);
+	bayesTree.insert(E);
+	bayesTree.insert(S);
+	bayesTree.insert(T);
+	bayesTree.insert(X);
+
+	// Now we modify the Bayes tree by inserting a new factor over B and S
+
+	// New conditionals in modified top of the tree
+	SymbolicConditional::shared_ptr
+		S_(new SymbolicConditional("S")),
+		L_(new SymbolicConditional("L", "S")),
+		E_(new SymbolicConditional("E", "L", "S")),
+		B_(new SymbolicConditional("B", "E", "L", "S"));
+
+	// Create expected Bayes tree
+	SymbolicBayesTree expected;
+	expected.insert(S_);
+	expected.insert(L_);
+	expected.insert(E_);
+	expected.insert(B_);
+	expected.insert(T);
+	expected.insert(X);
+
+	// create new factors to be inserted
+	SymbolicFactorGraph factorGraph;
+	factorGraph.push_factor("B","S");
+	factorGraph.push_factor("B");
+
+	// do incremental inference
+	bayesTree.update(factorGraph);
+
+	// Check whether the same
+  CHECK(assert_equal(expected,bayesTree));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;

cpp/testIncremental.cpp

@@ -1,124 +0,0 @@
-/**
- * @file    testIncremental.cpp
- * @brief   Unit tests for graph-based iSAM
- * @author  Michael Kaess
- * @author  Viorela Ila
- * @author  Frank Dellaert
- */
-
-#include <boost/tuple/tuple.hpp>
-#include <boost/assign/std/list.hpp> // for operator +=
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
-
-#include "SymbolicBayesNet.h"
-#include "SymbolicFactorGraph.h"
-#include "GaussianBayesNet.h"
-#include "Ordering.h"
-#include "BayesTree-inl.h"
-#include "smallExample.h"
-
-using namespace std;
-using namespace gtsam;
-
-typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
-typedef BayesTree<GaussianConditional> GaussianBayesTree;
-
-/* ************************************************************************* */
-
-void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor>& newFactors) {
-
-	// Remove the contaminated part of the Bayes tree
-	FactorGraph<SymbolicFactor> factors;
-	SymbolicBayesTree::Cliques orphans;
-	BOOST_FOREACH(boost::shared_ptr<SymbolicFactor> factor, newFactors) {
-
-		FactorGraph<SymbolicFactor> factors1;
-		SymbolicBayesTree::Cliques orphans1;
-		boost::tie(factors1, orphans1) = bayesTree.removeTop<SymbolicFactor>(factor);
-
-		factors.push_back(factors1);
-		orphans.splice (orphans.begin(), orphans1);
-	}
-
-	// add the factors themselves
-	factors.push_back(newFactors);
-
-	// create an ordering for the new and contaminated factors
-	Ordering ordering = factors.getOrdering();
-
-	// eliminate into a Bayes net
-	SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factors,ordering);
-
-	// insert conditionals back in, straight into the topless bayesTree
-	SymbolicBayesNet::const_reverse_iterator rit;
-	for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
-		bayesTree.insert(*rit);
-
-	// add orphans to the bottom of the new tree
-	BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) {
-		string key = orphan->separator_.front(); // todo: assumes there is a separator...
-		SymbolicBayesTree::sharedClique parent = bayesTree[key];
-		parent->children_ += orphan;
-	}
-
-}
-
-/* ************************************************************************* */
-TEST( BayesTree, iSAM )
-{
-	// Conditionals for ASIA example from the tutorial with A and D evidence
-	SymbolicConditional::shared_ptr
-		B(new SymbolicConditional("B")),
-		L(new SymbolicConditional("L", "B")),
-		E(new SymbolicConditional("E", "B", "L")),
-		S(new SymbolicConditional("S", "L", "B")),
-		T(new SymbolicConditional("T", "E", "L")),
-		X(new SymbolicConditional("X", "E"));
-
-	// Create using insert
-	SymbolicBayesTree bayesTree;
-	bayesTree.insert(B);
-	bayesTree.insert(L);
-	bayesTree.insert(E);
-	bayesTree.insert(S);
-	bayesTree.insert(T);
-	bayesTree.insert(X);
-
-	// Now we modify the Bayes tree by inserting a new factor over B and S
-
-	// New conditionals in modified top of the tree
-	SymbolicConditional::shared_ptr
-		S_(new SymbolicConditional("S")),
-		L_(new SymbolicConditional("L", "S")),
-		E_(new SymbolicConditional("E", "L", "S")),
-		B_(new SymbolicConditional("B", "E", "L", "S"));
-
-	// Create expected Bayes tree
-	SymbolicBayesTree expected;
-	expected.insert(S_);
-	expected.insert(L_);
-	expected.insert(E_);
-	expected.insert(B_);
-	expected.insert(T);
-	expected.insert(X);
-
-	// create new factors to be inserted
-	SymbolicFactorGraph factorGraph;
-	factorGraph.push_factor("B","S");
-	factorGraph.push_factor("B");
-
-	// do incremental inference
-	update(bayesTree, factorGraph);
-
-	// Check whether the same
-  CHECK(assert_equal(expected,bayesTree));
-}
-
-/* ************************************************************************* */
-int main() {
-	TestResult tr;
-	return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */