update+relin combined for speed; new backsub/threshold confirmed to yield correct result

inference/ISAM2-inl.h

@@ -211,7 +211,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class Conditional, class Config>
-	void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) {
+void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors) {
 
 	// Input: BayesTree(this), newFactors
 
@@ -236,10 +236,11 @@ 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> markedKeys = newFactors.keys();
+	tic("re-removetop");
 	Cliques orphans;
 	BayesNet<GaussianConditional> affectedBayesNet;
 	this->removeTop(markedKeys, affectedBayesNet, orphans);
+	toc("re-removetop");
 
 	//		FactorGraph<GaussianFactor> factors(affectedBayesNet);
 	// bug was here: we cannot reuse the original factors, because then the cached factors get messed up
@@ -252,7 +253,7 @@ namespace gtsam {
 
 	// BEGIN OF COPIED CODE
 
-		tic("linear_lookup1");
+	tic("re-lookup");
 	// ordering provides all keys in conditionals, there cannot be others because path to root included
 	set<Symbol> affectedKeys;
 	list<Symbol> tmp = affectedBayesNet.ordering();
@@ -271,17 +272,23 @@ namespace gtsam {
 		  		<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
 #endif
 
-		toc("linear_lookup1");
+	toc("re-lookup");
 
+	tic("re-cached");
 	// add the cached intermediate results from the boundary of the orphans ...
 	FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
 	factors.push_back(cachedBoundary);
+	toc("re-cached");
 
 	// END OF COPIED CODE
 
 
 	// 2. Add the new factors \Factors' into the resulting factor graph
-		factors.push_back(newFactors);
+	tic("re-newfactors");
+	if (newFactors) {
+		factors.push_back(*newFactors);
+	}
+	toc("re-newfactors");
 
 	// 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])
 
@@ -293,10 +300,11 @@ namespace gtsam {
 	//		Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance
 
 	// eliminate into a Bayes net
-		tic("linear_eliminate");
+	tic("eliminate");
 	BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
-		toc("linear_eliminate");
+	toc("eliminate");
 
+	tic("re-assemble");
 	// Create Index from ordering
 	IndexTable<Symbol> index(ordering);
 
@@ -307,9 +315,11 @@ namespace gtsam {
 
 	// Save number of affectedCliques
 	lastAffectedCliqueCount = this->size();
+	tic("re-assemble");
 
 	// 4. Insert the orphans back into the new Bayes tree.
 
+	tic("re-orphans");
 	// add orphans to the bottom of the new tree
 	BOOST_FOREACH(sharedClique orphan, orphans) {
 		Symbol parentRepresentative = findParentClique(orphan->separator_, index);
@@ -317,10 +327,18 @@ namespace gtsam {
 		parent->children_ += orphan;
 		orphan->parent_ = parent; // set new parent!
 	}
+	toc("re-orphans");
 
 	// Output: BayesTree(this)
 }
 
+/* ************************************************************************* */
+template<class Conditional, class Config>
+void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) {
+	const list<Symbol> markedKeys = newFactors.keys();
+	recalculate(markedKeys, &newFactors);
+}
+
 /* ************************************************************************* */
 // find all variables that are directly connected by a measurement to one of the marked variables
 template<class Conditional, class Config>
@@ -342,7 +360,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class Conditional, class Config>
-	void ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) {
+list<Symbol> ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) {
 
 	// Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_
 
@@ -378,6 +396,10 @@ namespace gtsam {
 		//    add the cached marginal factors from its children, and re-eliminate.
 
 		// todo: for simplicity, currently simply remove the top and recreate it using the original ordering
+		//recalculate(affectedSymbols);
+		return affectedSymbols;
+
+#if 0
 
 		tic("nonlin-mess");
 		Cliques orphans;
@@ -430,9 +452,14 @@ namespace gtsam {
 			parent->children_ += orphan;
 			orphan->parent_ = parent; // set new parent!
 		}
+#endif
 
 		// Output: updated Bayes tree (this), updated linearization point theta_
 	}
+
+	list<Symbol> empty;
+	return empty;
+
 }
 
 /* ************************************************************************* */
@@ -463,9 +490,10 @@ namespace gtsam {
 
 	tic("step3");
 	// 3. Linearize new factor
-		boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
+	//		boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
 	toc("step3");
 
+#if 0 // original algorithm
 	tic("step4");
 	// 4. Linear iSAM step (alg 3)
 	linear_update(*linearFactors); // in: this
@@ -485,7 +513,37 @@ namespace gtsam {
 
 	// todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again
 	delta_ = optimize2(*this, wildfire_threshold);
+#else // new algorithm
 
+	tic("step4B");
+	// 4B. Mark linear update
+	list<Symbol> markedKeys = newFactors.keys();
+	toc("step4B");
+
+	tic("step5B");
+	// 5B. Mark nonlinear update
+	if (relinearize) {
+		list<Symbol> markedRelin = fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this
+
+		// merge with markedKeys
+		markedKeys.splice(markedKeys.begin(), markedRelin, markedRelin.begin(), markedRelin.end());
+		markedKeys.sort(); // remove duplicates
+		markedKeys.unique();
+	}
+	toc("step5B");
+
+	tic("step6B");
+	// 6B. Redo top of Bayes tree
+	boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
+	recalculate(markedKeys, &(*linearFactors));
+	toc("step6B");
+
+	tic("step7B");
+	// 7B. Solve
+	delta_ = optimize2(*this, wildfire_threshold);
+	toc("step7B");
+
+#endif
 	toc("all");
 
 	tictoc_print();

inference/ISAM2.h

@@ -90,9 +90,10 @@ namespace gtsam {
 	boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
 	FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
 
+	void recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
 	void linear_update(const FactorGraph<GaussianFactor>& newFactors);
 	void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
-		void fluid_relinearization(double relinearize_threshold);
+	std::list<Symbol> fluid_relinearization(double relinearize_threshold);
 
 }; // ISAM2
 

slam/GaussianISAM2.cpp

@@ -63,7 +63,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<
 }
 
 /* ************************************************************************* */
-// fast version without threshold
+// fast full 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;