allow pure linear steps (disabled)

cpp/ISAM2-inl.h

@@ -132,13 +132,19 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional, class Config>
 	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
-			const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) {
+			const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold, bool relinearize) {
 
 		//		marked_ = nonlinearFactors_.keys(); // debug only ////////////
 
+		// only relinearize if requested in previous step AND necessary (ie. at least one variable changes)
+		relinearize = true; // todo - switched off
+		bool relinFromLast = true; //marked_.size() > 0;
+
 		//// 1 - relinearize selected variables
 
+		if (relinFromLast) {
 			theta_ = expmap(theta_, deltaMarked_);
+		}
 
 		//// 2 - Add new factors (for later relinearization)
 
@@ -154,6 +160,8 @@ namespace gtsam {
 		// todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!!
 		// It's about factors!!!
 
+		if (relinFromLast) {
+			// mark variables that have to be removed as invalid (removeFATtop)
 			// basically calculate all the keys contained in the factors that contain any of the keys...
 			// the goal is to relinearize all variables directly affected by new factors
 			list<int> allAffected = getAffectedFactors(marked_);
@@ -164,8 +172,9 @@ namespace gtsam {
 			}
 			marked_.clear();
 			marked_.insert(marked_.begin(), accumulate.begin(), accumulate.end());
+		} // else: marked_ is empty anyways
 
-		// merge keys of new factors with mask
+		// also mark variables that are affected by new factors as invalid
 		const list<Symbol> newKeys = newFactors.keys();
 		marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end());
 		// eliminate duplicates
@@ -181,6 +190,9 @@ namespace gtsam {
 		//// 6 - find factors connected to affected variables
 		//// 7 - linearize
 
+		FactorGraph<GaussianFactor> factors;
+
+		if (relinFromLast) {
 			// ordering provides all keys in conditionals, there cannot be others because path to root included
 			set<Symbol> affectedKeys;
 			list<Symbol> tmp = affectedBayesNet.ordering();
@@ -188,17 +200,18 @@ namespace gtsam {
 
 			// todo - remerge in keys of new factors
 			affectedKeys.insert(newKeys.begin(), newKeys.end());
-#if 0 // no longer needed for set
-		// eliminate duplicates
-		affectedKeys.sort();
-		affectedKeys.unique();
-#endif
 
-		FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
+			factors = relinearizeAffectedFactors(affectedKeys);
 
 			// add the cached intermediate results from the boundary of the orphans ...
 			FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
 			factors.push_back(cachedBoundary);
+		} else {
+			// reuse the old factors
+			FactorGraph<GaussianFactor> tmp(affectedBayesNet);
+			factors.push_back(tmp);
+			factors.push_back(newFactors.linearize(theta_));
+		}
 
 		//// 8 - eliminate and add orphans back in
 
@@ -232,6 +245,8 @@ namespace gtsam {
 
 		marked_.clear();
 		deltaMarked_ = VectorConfig(); // clear
+		if (relinearize) { // decides about next step!!!
+
 			for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
 				Symbol key = it->first;
 				Vector v = it->second;
@@ -243,16 +258,17 @@ namespace gtsam {
 
 			// not part of the formal algorithm, but needed to allow initialization of new variables outside by the user
 			thetaFuture_ = expmap(thetaFuture_, deltaMarked_);
+		}
 
 	}
 
 	template<class Conditional, class Config>
 	void ISAM2<Conditional, Config>::update(
 			const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
-			double wildfire_threshold, double relinearize_threshold) {
+			double wildfire_threshold, double relinearize_threshold, bool relinearize) {
 
 		Cliques orphans;
-		this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold);
+		this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold, relinearize);
 
 	}
 

cpp/ISAM2.h

@@ -70,9 +70,9 @@ namespace gtsam {
 		 */
 		void update_internal(const NonlinearFactorGraph<Config>& newFactors,
 				const Config& newTheta, Cliques& orphans,
-				double wildfire_threshold, double relinearize_threshold);
+				double wildfire_threshold, double relinearize_threshold, bool relinearize);
 		void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
-				double wildfire_threshold = 0., double relinearize_threshold = 0.);
+				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!)
 		const Config getLinearizationPoint() const {return thetaFuture_;}