bug fix in wildfire alg; more stats

inference/ISAM2-inl.h

@@ -19,7 +19,7 @@ using namespace boost::assign;
 #include <gtsam/inference/ISAM2.h>
 
 
-#if 1 // timing
+#if 1 // timing - note: adds some time when applied in inner loops
 #include <sys/time.h>
 // simple class for accumulating execution timing information by name
 class Timing {
@@ -236,10 +236,10 @@ namespace gtsam {
 		// 1. Remove top of Bayes tree and convert to a factor graph:
 		// (a) For each affected variable, remove the corresponding clique and all parents up to the root.
 		// (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
-		const list<Symbol> newKeys = newFactors.keys();
+		const list<Symbol> markedKeys = newFactors.keys();
 		Cliques orphans;
 		BayesNet<GaussianConditional> affectedBayesNet;
-		this->removeTop(newKeys, affectedBayesNet, orphans);
+		this->removeTop(markedKeys, affectedBayesNet, orphans);
 
 		//		FactorGraph<GaussianFactor> factors(affectedBayesNet);
 		// bug was here: we cannot reuse the original factors, because then the cached factors get messed up
@@ -258,16 +258,15 @@ namespace gtsam {
 		list<Symbol> tmp = affectedBayesNet.ordering();
 		affectedKeys.insert(tmp.begin(), tmp.end());
 
-		// Save number of affected variables
-		lastAffectedVariableCount = affectedKeys.size();
-
 		FactorGraph<GaussianFactor> factors(*relinearizeAffectedFactors(affectedKeys));  // todo: no need to relinearize here, should have cached linearized factors
 
-		// Save number of affected factors
+		lastAffectedMarkedCount = markedKeys.size();
+		lastAffectedVariableCount = affectedKeys.size();
 		lastAffectedFactorCount = factors.size();
-		// output for generating figures
+
 #ifdef PRINT_STATS
-		cout << "linear: #newKeys: " << newKeys.size() << " #affectedVariables: " << affectedKeys.size()
+		// output for generating figures
+		cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size()
 		  << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
 		  << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
 #endif
@@ -287,10 +286,10 @@ namespace gtsam {
 		// 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree])
 
 		// create an ordering for the new and contaminated factors
-		// newKeys are passed in: those variables will be forced to the end in the ordering
+		// markedKeys are passed in: those variables will be forced to the end in the ordering
-		set<Symbol> newKeysSet;
+		set<Symbol> markedKeysSet;
-		newKeysSet.insert(newKeys.begin(), newKeys.end());
+		markedKeysSet.insert(markedKeys.begin(), markedKeys.end());
-		Ordering ordering = factors.getConstrainedOrdering(newKeysSet); // intelligent ordering
+		Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering
 //		Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance
 
 		// eliminate into a Bayes net
@@ -401,7 +400,11 @@ namespace gtsam {
 			factors = relinearizeAffectedFactors(affectedKeys);
 			toc("nonlin_relin");
 
-			cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl;
+			lastNonlinearMarkedCount = marked.size();
+			lastNonlinearAffectedVariableCount = affectedKeys.size();
+			lastNonlinearAffectedFactorCount = factors->size();
+
+//			cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl;
 
 			// add the cached intermediate results from the boundary of the orphans ...
 			FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
@@ -438,6 +441,14 @@ namespace gtsam {
 			const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
 			double wildfire_threshold, double relinearize_threshold, bool relinearize) {
 
+		lastAffectedVariableCount = 0;
+		lastAffectedFactorCount = 0;
+		lastAffectedCliqueCount = 0;
+		lastAffectedMarkedCount = 0;
+		lastNonlinearMarkedCount = 0;
+		lastNonlinearAffectedVariableCount = 0;
+		lastNonlinearAffectedFactorCount = 0;
+
 		tic("all");
 
 		tic("step1");
@@ -473,7 +484,7 @@ namespace gtsam {
 		toc("step6");
 
 		// todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again
-//todo		delta_ = optimize2(*this, wildfire_threshold);
+		delta_ = optimize2(*this, wildfire_threshold);
 
 		toc("all");
 

inference/ISAM2.h

@@ -79,6 +79,10 @@ namespace gtsam {
 		size_t lastAffectedVariableCount;
 		size_t lastAffectedFactorCount;
 		size_t lastAffectedCliqueCount;
+		size_t lastAffectedMarkedCount;
+		size_t lastNonlinearMarkedCount;
+		size_t lastNonlinearAffectedVariableCount;
+		size_t lastNonlinearAffectedFactorCount;
 
 	private:
 

slam/GaussianISAM2.cpp

@@ -18,38 +18,89 @@ template class ISAM2<GaussianConditional, planarSLAM::Config>;
 namespace gtsam {
 
 /* ************************************************************************* */
-void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result) {
+void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<Symbol>& changed, VectorConfig& result) {
+	// if none of the variables in this clique (frontal and separator!) changed
+	// significantly, then by the running intersection property, none of the
+	// cliques in the children need to be processed
 	bool process_children = false;
+
 	// parents are assumed to already be solved and available in result
 	GaussianISAM2::Clique::const_reverse_iterator it;
 	for (it = clique->rbegin(); it!=clique->rend(); it++) {
 		GaussianConditional::shared_ptr cg = *it;
-    Vector x = cg->solve(result); // Solve for that variable
+
+		// only solve if at least one of the separator variables changed
+		// significantly, ie. is in the set "changed"
+		bool found = true;
+		if (cg->nrParents()>0) {
+			found = false;
+			BOOST_FOREACH(const Symbol& key, cg->parents()) {
+				if (changed.find(key)!=changed.end()) {
+					found = true;
+				}
+			}
+		}
+		if (found) {
+			// Solve for that variable
+			Vector x = cg->solve(result);
+			process_children = true;
+
+			// store result in partial solution
+			result.insert(cg->key(), x);
+
+			// if change is above threshold, add to set of changed variables
 			if (max(abs(x)) >= threshold) {
+				changed.insert(cg->key());
 				process_children = true;
 			}
-    result.insert(cg->key(), x);   // store result in partial solution
+		}
 	}
 	if (process_children) {
 		BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
-			optimize2(child, threshold, result);
+			optimize2(child, threshold, changed, result);
 		}
 	}
 }
 
+/* ************************************************************************* */
+// fast version without threshold
+void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
+	// parents are assumed to already be solved and available in result
+	GaussianISAM2::Clique::const_reverse_iterator it;
+	for (it = clique->rbegin(); it!=clique->rend(); it++) {
+		GaussianConditional::shared_ptr cg = *it;
+		Vector x = cg->solve(result);
+		// store result in partial solution
+		result.insert(cg->key(), x);
+	}
+	BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
+		optimize2(child, result);
+	}
+}
+
 /* ************************************************************************* */
 VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) {
 	VectorConfig result;
+	set<Symbol> changed;
 	// starting from the root, call optimize on each conditional
-	optimize2(bayesTree.root(), threshold, result);
+	if (threshold<=0.) {
+		optimize2(bayesTree.root(), result);
+	} else {
+		optimize2(bayesTree.root(), threshold, changed, result);
+	}
 	return result;
 }
 
 /* ************************************************************************* */
 VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) {
 	VectorConfig result;
+	set<Symbol> changed;
 	// starting from the root, call optimize on each conditional
-	optimize2(bayesTree.root(), threshold, result);
+	if (threshold<=0.) {
+		optimize2(bayesTree.root(), result);
+	} else {
+		optimize2(bayesTree.root(), threshold, changed, result);
+	}
 	return result;
 }
 
@@ -64,7 +115,7 @@ void nnz_internal(const GaussianISAM2::sharedClique& clique, int& result) {
 			dimSep += matrix_it->second.size2();
 		}
 		int dimR = cg->dim();
-    result += (dimR+1)*dimR/2 + dimSep*dimR;
+		result += ((dimR+1)*dimR)/2 + dimSep*dimR;
 	}
 	// traverse the children
 	BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {

slam/GaussianISAM2.h

@@ -18,9 +18,6 @@ namespace gtsam {
 
 	typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
 
-	// recursively optimize this conditional and all subtrees
-	void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result);
-
 	// optimize the BayesTree, starting from the root
 	VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.);