added statistics for evaluation, disabled timing

inference/ISAM2-inl.h

@@ -19,7 +19,7 @@ using namespace boost::assign;
 #include <gtsam/inference/ISAM2.h>
 
 
-#if 1 // timing - note: adds some time when applied in inner loops
+#if 0 // 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 {
@@ -310,8 +310,11 @@ boost::shared_ptr<set<Symbol> > ISAM2<Conditional, Config>::recalculate(const li
 
 	// insert conditionals back in, straight into the topless bayesTree
 	typename BayesNet<Conditional>::const_reverse_iterator rit;
-	for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
+	for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit ) {
 		this->insert(*rit, index);
+	}
+
+	lastNnzTop = calculate_nnz(this->root());
 
 	// Save number of affectedCliques
 	lastAffectedCliqueCount = this->size();
@@ -319,7 +322,7 @@ boost::shared_ptr<set<Symbol> > ISAM2<Conditional, Config>::recalculate(const li
 
 	// 4. Insert the orphans back into the new Bayes tree.
 
-	tic("re-orphans");
+	tic("re-orphan");
 	// add orphans to the bottom of the new tree
 	BOOST_FOREACH(sharedClique orphan, orphans) {
 		Symbol parentRepresentative = findParentClique(orphan->separator_, index);
@@ -327,7 +330,7 @@ boost::shared_ptr<set<Symbol> > ISAM2<Conditional, Config>::recalculate(const li
 		parent->children_ += orphan;
 		orphan->parent_ = parent; // set new parent!
 	}
-	toc("re-orphans");
+	toc("re-orphan");
 
 	// Output: BayesTree(this)
 
@@ -375,9 +378,8 @@ void ISAM2<Conditional, Config>::update(
 	lastAffectedFactorCount = 0;
 	lastAffectedCliqueCount = 0;
 	lastAffectedMarkedCount = 0;
-	lastNonlinearMarkedCount = 0;
-	lastNonlinearAffectedVariableCount = 0;
-	lastNonlinearAffectedFactorCount = 0;
+	lastBacksubVariableCount = 0;
+	lastNnzTop = 0;
 
 	tic("all");
 
@@ -405,7 +407,7 @@ void ISAM2<Conditional, Config>::update(
 #endif
 
 	VectorConfig deltaMarked;
-	if (relinearize && count%10 == 0) { // todo: every n steps
+	if (relinearize) { // && count%10 == 0) { // todo: every n steps
 		tic("step4");
 		// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
 		list<Symbol> markedRelin;
@@ -462,11 +464,12 @@ void ISAM2<Conditional, Config>::update(
 
 	tic("step9");
 	// 9. Solve
-//	if (wildfire_threshold<=0.) {
-//		delta_ = *(optimize2(this->root()));
-//	} else {
-		optimize2(this->root(), wildfire_threshold, *replacedKeys, delta_); // modifies delta_
-//	}
+	if (wildfire_threshold<=0.) {
+		delta_ = *(optimize2(this->root()));
+		lastBacksubVariableCount = theta_.size();
+	} else {
+		lastBacksubVariableCount = optimize2(this->root(), wildfire_threshold, *replacedKeys, delta_); // modifies delta_
+	}
 	toc("step9");
 
 	toc("all");

inference/ISAM2.h

@@ -80,9 +80,8 @@ public:
 	size_t lastAffectedFactorCount;
 	size_t lastAffectedCliqueCount;
 	size_t lastAffectedMarkedCount;
-	size_t lastNonlinearMarkedCount;
-	size_t lastNonlinearAffectedVariableCount;
-	size_t lastNonlinearAffectedFactorCount;
+	size_t lastBacksubVariableCount;
+	size_t lastNnzTop;
 
 private:
 

slam/GaussianISAM2.cpp

@@ -19,7 +19,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
-		set<Symbol>& changed, const set<Symbol>& replaced, VectorConfig& delta) {
+		set<Symbol>& changed, const set<Symbol>& replaced, VectorConfig& delta, int& count) {
 	// 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
@@ -47,6 +47,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
 		if (found) {
 			// Solve for that variable
 			Vector d = cg->solve(delta);
+			count++;
 			// have to process children; only if none of the variables in the
 			// clique were affected, and none of the variables in the clique
 			// had a variable in the separator that changed significantly
@@ -81,7 +82,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold,
 	}
 	if (process_children) {
 		BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
-			optimize2(child, threshold, changed, replaced, delta);
+			optimize2(child, threshold, changed, replaced, delta, count);
 		}
 	}
 }
@@ -112,10 +113,12 @@ boost::shared_ptr<VectorConfig> optimize2(const GaussianISAM2::sharedClique& roo
 }
 
 /* ************************************************************************* */
-void optimize2(const GaussianISAM2::sharedClique& root, double threshold, const set<Symbol>& keys, VectorConfig& delta) {
+int optimize2(const GaussianISAM2::sharedClique& root, double threshold, const set<Symbol>& keys, VectorConfig& delta) {
 	set<Symbol> changed;
+	int count = 0;
 	// starting from the root, call optimize on each conditional
-	optimize2(root, threshold, changed, keys, delta);
+	optimize2(root, threshold, changed, keys, delta, count);
+	return count;
 }
 
 /* ************************************************************************* */

slam/GaussianISAM2.h

@@ -29,7 +29,8 @@ namespace gtsam {
 	// non-replaced variables that can be ignored, ie. the old delta entry is kept
 	// and recursive backsubstitution might eventually stop if none of the changed
 	// variables are contained in the subtree.
-	void optimize2(const GaussianISAM2::sharedClique& root,
+	// returns the number of variables that were solved for
+	int optimize2(const GaussianISAM2::sharedClique& root,
 			double threshold, const std::set<Symbol>& replaced, VectorConfig& delta);
 
 	// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)