VectorConfig/Config confusion resolved; planarSLAM integrated

cpp/GaussianISAM2.cpp

@@ -11,7 +11,9 @@ using namespace gtsam;
 
 // Explicitly instantiate so we don't have to include everywhere
 #include "ISAM2-inl.h"
+
 //template class ISAM2<GaussianConditional, VectorConfig>;
+//template class ISAM2<GaussianConditional, planarSLAM::Config>;
 
 namespace gtsam {
 
@@ -25,8 +27,6 @@ void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result)
     result.insert(cg->key(), x);   // store result in partial solution
   }
 	BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
-//	list<GaussianISAM2::Clique::shared_ptr>::const_iterator child;
-//	for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
 		optimize2(child, result);
 	}
 }
@@ -39,4 +39,27 @@ VectorConfig optimize2(const GaussianISAM2& bayesTree) {
 	return result;
 }
 
+#if 0
+/* ************************************************************************* */
+void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result) {
+	// parents are assumed to already be solved and available in result
+	GaussianISAM2_P::Clique::const_reverse_iterator it;
+	for (it = clique->rbegin(); it!=clique->rend(); it++) {
+		GaussianConditional::shared_ptr cg = *it;
+    result.insert(cg->key(), cg->solve(result));   // store result in partial solution
+  }
+	BOOST_FOREACH(GaussianISAM2_P::sharedClique child, clique->children_) {
+		optimize2(child, result);
+	}
+}
+#endif
+
+/* ************************************************************************* */
+VectorConfig optimize2(const GaussianISAM2_P& bayesTree) {
+	VectorConfig result;
+	// starting from the root, call optimize on each conditional
+	optimize2(bayesTree.root(), result);
+	return result;
+}
+
 } /// namespace gtsam

cpp/GaussianISAM2.h

@@ -11,6 +11,7 @@
 #include "ISAM2.h"
 #include "GaussianConditional.h"
 #include "GaussianFactor.h"
+#include "planarSLAM.h"
 
 namespace gtsam {
 
@@ -22,4 +23,15 @@ namespace gtsam {
 	// optimize the BayesTree, starting from the root
 	VectorConfig optimize2(const GaussianISAM2& bayesTree);
 
+
+	// todo: copy'n'paste to avoid template hell
+
+	typedef ISAM2<GaussianConditional, planarSLAM::Config> GaussianISAM2_P;
+
+	// recursively optimize this conditional and all subtrees
+//	void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result);
+
+	// optimize the BayesTree, starting from the root
+	VectorConfig optimize2(const GaussianISAM2_P& bayesTree);
+
 }/// namespace gtsam

cpp/ISAM2-inl.h

@@ -23,7 +23,7 @@ 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 Symbol& key) {
+	boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Symbol& 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
@@ -45,7 +45,7 @@ namespace gtsam {
 	}
 
 	// from GaussianFactorGraph.cpp, see _eliminateOne above
-	GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
+	GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
 		GaussianBayesNet chordalBayesNet; // empty
 		BOOST_FOREACH(const Symbol& key, ordering) {
 			GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
@@ -54,7 +54,7 @@ namespace gtsam {
 		return chordalBayesNet;
 	}
 
-	GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
+	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);
@@ -67,9 +67,9 @@ 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), linPoint_(config), estimate_(config) {
+	: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config) {
 		// todo: repeats calculation above, just to set "cached"
-		_eliminate_const(nlfg.linearize(config), cached, ordering);
+		_eliminate_const(nlfg.linearize(config), cached_, ordering);
 	}
 
 	/* ************************************************************************* */
@@ -108,7 +108,7 @@ namespace gtsam {
 			it--;
 			const Symbol& key = *it;
 			// retrieve the cached factor and add to boundary
-			cachedBoundary.push_back(cached[key]);
+			cachedBoundary.push_back(cached_[key]);
 		}
 
 		return cachedBoundary;
@@ -119,13 +119,16 @@ namespace gtsam {
 	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
 			const Config& config, Cliques& orphans) {
 
+#if 0 // todo - temporarily disabled --------------------------------------------------------------------------------------------------
 		// copy variables into config_, but don't overwrite existing entries (current linearization point!)
 		for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) {
 			if (!linPoint_.contains(it->first)) {
 				linPoint_.insert(it->first, it->second);
-				estimate_.insert(it->first, it->second);
 			}
 		}
+#else
+		linPoint_ = config;
+#endif
 
 		FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
 
@@ -155,7 +158,7 @@ namespace gtsam {
 		}
 
 		// eliminate into a Bayes net
-		BayesNet<Conditional> bayesNet = _eliminate(factors, cached, ordering);
+		BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
 
 		// remember the new factors for later relinearization
 		nonlinearFactors_.push_back(newFactors);
@@ -176,9 +179,7 @@ namespace gtsam {
 		}
 
 		// update solution - todo: potentially expensive
-		VectorConfig delta = optimize2(*this);
+		delta_ = optimize2(*this);
-//		delta.print();
-		estimate_ += delta;
 
 	}
 

cpp/ISAM2.h

@@ -24,7 +24,7 @@
 
 namespace gtsam {
 
-	typedef std::map<Symbol, GaussianFactor::shared_ptr> cachedFactors;
+	typedef std::map<Symbol, GaussianFactor::shared_ptr> CachedFactors;
 
 	template<class Conditional, class Config>
 	class ISAM2: public BayesTree<Conditional> {
@@ -34,14 +34,14 @@ namespace gtsam {
 		// current linearization point
 		Config linPoint_;
 
-		// most recent estimate
+		// most recent solution
-		Config estimate_;
+		VectorConfig delta_;
 
 		// for keeping all original nonlinear factors
 		NonlinearFactorGraph<Config> nonlinearFactors_;
 
 		// cached intermediate results for restarting computation in the middle
-		cachedFactors cached;
+		CachedFactors cached_;
 
 	public:
 
@@ -65,7 +65,7 @@ namespace gtsam {
 		void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
 		void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
 
-		const Config estimate() {return estimate_;}
+		const Config estimate() {return expmap(linPoint_, delta_);}
 
 	private:
 		FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys);