relinearizing part of a BayesTree, requiring caching of intermediate results during elimination

cpp/BayesTree-inl.h

@@ -235,12 +235,12 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional>
 	void BayesTree<Conditional>::print(const string& s) const {
-		cout << s << ": size == " << size() << endl;
-		if (nodes_.empty()) return;
 		if (root_.use_count() == 0) {
-			printf("WARNING: Forest...\n");
+			printf("WARNING: BayesTree.print encountered a forest...\n");
 			return;
 		}
+		cout << s << ": size == " << size() << endl;
+		if (nodes_.empty()) return;
 		root_->printTree("");
 	}
 

cpp/ISAM2-inl.h

@@ -22,6 +22,44 @@ namespace gtsam {
 
 	using namespace std;
 
+	// from inference-inl.h - need to additionally return the newly created factor for caching
+	boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const string& key) {
+
+		// 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<GaussianFactor> joint_factor = removeAndCombineFactors(graph,key);
+
+		// eliminate that joint factor
+		boost::shared_ptr<GaussianFactor> factor;
+		boost::shared_ptr<GaussianConditional> conditional;
+		boost::tie(conditional, factor) = joint_factor->eliminate(key);
+
+		// remember the intermediate result to be able to later restart computation in the middle
+		cached[key] = factor;
+
+		// add new factor on separator back into the graph
+		if (!factor->empty()) graph.push_back(factor);
+
+		// return the conditional Gaussian
+		return conditional;
+	}
+
+	// from GaussianFactorGraph.cpp, see _eliminateOne above
+	GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
+		GaussianBayesNet chordalBayesNet; // empty
+		BOOST_FOREACH(string key, ordering) {
+			GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
+			chordalBayesNet.push_back(cg);
+		}
+		return chordalBayesNet;
+	}
+
+	GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
+		// make a copy that can be modified locally
+		FactorGraph<GaussianFactor> graph_ignored = graph;
+		return _eliminate(graph_ignored, cached, ordering);
+	}
+
 	/** Create an empty Bayes Tree */
 	template<class Conditional, class Config>
 	ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
@@ -29,7 +67,10 @@ namespace gtsam {
 	/** Create a Bayes Tree from a nonlinear factor graph */
 	template<class Conditional, class Config>
 	ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
-	: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), config_(config) {}
+	: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), config_(config) {
+		// todo: repeats calculation above, just to set "cached"
+		_eliminate_const(nlfg.linearize(config), cached, ordering);
+	}
 
 	/* ************************************************************************* */
 	template<class Conditional, class Config>
@@ -42,7 +83,6 @@ namespace gtsam {
 				config_.insert(it->first, it->second);
 			}
 		}
-		nonlinearFactors_.push_back(newFactors);
 
 		FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(config_);
 
@@ -50,8 +90,8 @@ namespace gtsam {
 		FactorGraph<GaussianFactor> affectedFactors;
 		boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
 
-
 #if 1
+#if 0
 		// find the corresponding original nonlinear factors, and relinearize them
 		NonlinearFactorGraph<Config> nonlinearAffectedFactors;
 		set<int> idxs; // avoid duplicates by putting index into set
@@ -75,29 +115,92 @@ namespace gtsam {
 		BOOST_FOREACH(int idx, idxs) {
 			nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
 		}
+
 		FactorGraph<GaussianFactor> factors = nonlinearAffectedFactors.linearize(config_);
 
+#else
+
+		NonlinearFactorGraph<Config> nonlinearAffectedFactors;
+
+		// retrieve all factors that ONLY contain the affected variables
+		// (note that the remaining stuff is summarized in the cached factors)
+		list<string> affectedKeys = affectedFactors.keys();
+		typename FactorGraph<NonlinearFactor<Config> >::iterator it;
+		for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
+			bool inside = true;
+			BOOST_FOREACH(string key, (*it)->keys()) {
+				if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end())
+					inside = false;
+			}
+			if (inside)
+				nonlinearAffectedFactors.push_back(*it);
+		}
+
+		FactorGraph<GaussianFactor> factors = nonlinearAffectedFactors.linearize(config_);
+
+		// recover intermediate factors from cache that are passed into the affected area
+		FactorGraph<GaussianFactor> cachedBoundary;
+		BOOST_FOREACH(sharedClique orphan, orphans) {
+			// find the last variable that is not part of the separator
+			string oneTooFar = orphan->separator_.front();
+			list<string> keys = orphan->keys();
+			list<string>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
+			it--;
+			string key = *it;
+			// retrieve the cached factor and add to boundary
+			cachedBoundary.push_back(cached[key]);
+		}
+		factors.push_back(cachedBoundary);
+
+#endif
+
+
+#if 0
+		printf("**************\n");
+		nonlinearFactors_.linearize(config).print("all factors");
+		printf("--------------\n");
+		newFactorsLinearized.print("newFactorsLinearized");
+		printf("--------------\n");
+		factors.print("factors");
+		printf("--------------\n");
+		affectedFactors.print("affectedFactors");
+		printf("--------------\n");
+#endif
+
 		// add the new factors themselves
 		factors.push_back(newFactorsLinearized);
 #endif
 
-		affectedFactors.push_back(newFactorsLinearized);
-
 		// create an ordering for the new and contaminated factors
 		Ordering ordering;
 		if (true) {
-			ordering = /*affectedF*/factors.getOrdering();
+			ordering = factors.getOrdering();
 		} else {
-			list<string> keys = /*affectedF*/factors.keys();
+			list<string> keys = factors.keys();
 			keys.sort(); // todo: correct sorting order?
 			ordering = keys;
 		}
 
+#if 0
+		ordering.print();
+
+		factors.print("factors BEFORE");
+		printf("--------------\n");
+#endif
+
 		// eliminate into a Bayes net
-		BayesNet<Conditional> bayesNet = eliminate<GaussianFactor, Conditional>(affectedFactors,ordering);
+		BayesNet<Conditional> bayesNet = _eliminate(factors, cached, ordering);
 
 #if 1
-		BayesNet<Conditional> bayesNetTest = eliminate<GaussianFactor, Conditional>(factors,ordering); // todo - debug only
+		// check if relinearized agrees with correct solution
+		affectedFactors.push_back(newFactorsLinearized);
+
+#if 0
+		affectedFactors.print("affectedFactors BEFORE");
+		printf("--------------\n");
+#endif
+
+		BayesNet<Conditional> bayesNetTest = eliminate<GaussianFactor, GaussianConditional>(affectedFactors, ordering);
 		if (!bayesNet.equals(bayesNetTest)) {
 			printf("differ\n");
 			bayesNet.print();
@@ -106,6 +209,8 @@ namespace gtsam {
 		}
 #endif
 
+		nonlinearFactors_.push_back(newFactors);
+
 		// insert conditionals back in, straight into the topless bayesTree
 		typename BayesNet<Conditional>::const_reverse_iterator rit;
 		for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
@@ -125,6 +230,15 @@ namespace gtsam {
 
 	template<class Conditional, class Config>
 	void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors, const Config& config) {
+
+#if 0
+		printf("8888888888888888\n");
+		try {
+		this->print("BayesTree");
+		} catch (char * c) {};
+		printf("8888888888888888\n");
+#endif
+
 		Cliques orphans;
 		this->update_internal(newFactors, config, orphans);
 	}

cpp/ISAM2.h

@@ -23,6 +23,8 @@
 
 namespace gtsam {
 
+	typedef std::map<std::string, GaussianFactor::shared_ptr> cachedFactors;
+
 	template<class Conditional, class Config>
 	class ISAM2: public BayesTree<Conditional> {
 
@@ -32,6 +34,9 @@ namespace gtsam {
 		Config config_;
 		NonlinearFactorGraph<Config> nonlinearFactors_;
 
+		// cached intermediate results for restarting computation in the middle
+		cachedFactors cached;
+
 	public:
 
 		/** Create an empty Bayes Tree */

cpp/testGaussianISAM2.cpp

@@ -19,7 +19,7 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 TEST( ISAM2, ISAM2_smoother )
 {
 	// Create smoother with 7 nodes