cleanup

inference/ISAM2-inl.h

@@ -129,6 +129,7 @@ namespace gtsam {
 		return chordalBayesNet;
 	}
 
+	// special const version used in constructor below
 	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;
@@ -142,7 +143,8 @@ 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)), theta_(config), nonlinearFactors_(nlfg) {
+	: BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)),
+	  theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) {
 		// todo: repeats calculation above, just to set "cached"
 		// De-referencing shared pointer can be quite expensive because creates temporary
 		_eliminate_const(*nlfg.linearize(config), cached_, ordering);
@@ -213,7 +215,7 @@ namespace gtsam {
 
 		// Input: BayesTree(this), newFactors
 
-//#define PRINT_STATS // todo: figures for paper, disable for timing
+//#define PRINT_STATS // figures for paper, disable for timing
 #ifdef PRINT_STATS
 		static int counter = 0;
 		int maxClique = 0;
@@ -286,8 +288,8 @@ namespace gtsam {
 		// newKeys are passed in: those variables will be forced to the end in the ordering
 		set<Symbol> newKeysSet;
 		newKeysSet.insert(newKeys.begin(), newKeys.end());
-		Ordering ordering = factors.getConstrainedOrdering(newKeysSet);
+		Ordering ordering = factors.getConstrainedOrdering(newKeysSet); // intelligent ordering
-//		Ordering ordering = factors.getOrdering();
+//		Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance
 
 		// eliminate into a Bayes net
 		tic("linear_eliminate");

inference/ISAM2.h

@@ -35,19 +35,15 @@ namespace gtsam {
 		// current linearization point
 		Config theta_;
 
+		// the linear solution, an update to the estimate in theta
+		VectorConfig delta_;
+
 		// for keeping all original nonlinear factors
 		NonlinearFactorGraph<Config> nonlinearFactors_;
 
 		// cached intermediate results for restarting computation in the middle
 		CachedFactors cached_;
 
-		// the linear solution, an update to the estimate in theta
-		VectorConfig delta_;
-		VectorConfig deltaMarked_;
-
-		// variables that have been updated, requiring the corresponding factors to be relinearized
-		std::list<Symbol> marked_;
-
 	public:
 
 		/** Create an empty Bayes Tree */
@@ -57,37 +53,29 @@ namespace gtsam {
 		ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config);
 
 		/** Destructor */
-		virtual ~ISAM2() {
+		virtual ~ISAM2() {}
-		}
 
 		typedef typename BayesTree<Conditional>::sharedClique sharedClique;
 
 		typedef typename BayesTree<Conditional>::Cliques Cliques;
 
 		/**
-		 * ISAM2. (update_internal provides access to list of orphans for drawing purposes)
+		 * ISAM2.
 		 */
-		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);
 		void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
 				double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
 
-		// needed to create initial estimates (note that this will be the linearization point in the next step!)
+		// needed to create initial estimates
 		const Config getLinearizationPoint() const {return theta_;}
+
 		// estimate based on incomplete delta (threshold!)
 		const Config calculateEstimate() const {return theta_.expmap(delta_);}
-		// estimate based on full delta (note that this is based on the actual current linearization point)
+
+		// estimate based on full delta (note that this is based on the current linearization point)
 		const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));}
 
-		const std::list<Symbol>& getMarkedUnsafe() const { return marked_; }
-
 		const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
 
-		const Config& getThetaUnsafe() const { return theta_; }
-
-		const VectorConfig& getDeltaUnsafe() const { return delta_; }
-
 		size_t lastAffectedVariableCount;
 		size_t lastAffectedFactorCount;
 		size_t lastAffectedCliqueCount;
@@ -98,6 +86,10 @@ namespace gtsam {
 		boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
 		FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
 
+		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);
+
 	}; // ISAM2
 
 } /// namespace gtsam