relinearization working, but currently applied to all variables

.cproject

@@ -753,10 +753,18 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testKey.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testISAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>
-<buildTarget>testKey.run</buildTarget>
+<buildTarget>testISAM.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
+<target name="testGaussianISAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments>-j2</buildArguments>
+<buildTarget>testGaussianISAM.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>

cpp/BayesTree-inl.h

@@ -402,11 +402,11 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional>
 	template<class Factor>
-  void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
+  void BayesTree<Conditional>::removeTop(const list<Symbol>& keys,
 		FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
 
 		// process each key of the new factor
-		BOOST_FOREACH(const Symbol& key, newFactor->keys())
+		BOOST_FOREACH(const Symbol& key, keys)
 			try {
 				// get the clique and remove it from orphans (if it exists)
 				sharedClique clique = (*this)[key];
@@ -424,21 +424,6 @@ namespace gtsam {
 			}
 	}
 
-	/* ************************************************************************* */
-	template<class Conditional>
-	template<class Factor>
-  pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
-	BayesTree<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
-		// Remove the contaminated part of the Bayes tree
-		FactorGraph<Factor> factors;
-		Cliques orphans;
-
-		BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
-			this->removeTop<Factor>(factor, factors, orphans);
-
-		return make_pair(factors,orphans);
-	}
-
 /* ************************************************************************* */
 
 }

cpp/BayesTree.h

@@ -174,21 +174,13 @@ namespace gtsam {
 		std::pair<FactorGraph<Factor>, Cliques> removePath(sharedClique clique);
 
 		/**
-		 * Given a factor, turn "contaminated" part of the tree back into a factor graph.
+		 * Given a list of keys, turn "contaminated" part of the tree back into a factor graph.
 		 * Factors and orphans are added to the in/out arguments.
 		 */
 		template<class Factor>
-		void removeTop(const boost::shared_ptr<Factor>& newFactor,
+		void removeTop(const std::list<Symbol>& keys,
 				FactorGraph<Factor> &factors, Cliques& orphans);
 
-		/**
-		 * Given a set of factors, turn "contaminated" part of the tree back into a
-		 * factor graph and return it along with a list of orphaned subtree roots.
-		 * Used for incrementally updating a BayesTree given new measurements (factors).
-		 */
-		template<class Factor>
-		std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors);
-
 	}; // BayesTree
 
 } /// namespace gtsam

cpp/ISAM-inl.h

@@ -31,7 +31,7 @@ namespace gtsam {
 
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<Factor> factors;
-		boost::tie(factors, orphans) = removeTop(newFactors);
+		removeTop(newFactors.keys(), factors, orphans);
 
 		// add the factors themselves
 		factors.push_back(newFactors);

cpp/ISAM2-inl.h

@@ -119,6 +119,9 @@ namespace gtsam {
 	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
 			const Config& config, Cliques& orphans) {
 
+		// todo: updates all variables... needs to be synced with removeTop!!!
+		linPoint_ = expmap(linPoint_, delta_);
+
 		// add new variables
 		linPoint_.insert(config);
 
@@ -126,7 +129,9 @@ namespace gtsam {
 
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<GaussianFactor> affectedFactors;
-		boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
+//		list<Symbol> keysToBeRemoved = newFactorsLinearized.keys(); // todo
+		list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
+		this->removeTop(keysToBeRemoved, affectedFactors, orphans);
 
 		// relinearize the affected factors ...
 		list<Symbol> affectedKeys = affectedFactors.keys();
@@ -172,7 +177,6 @@ namespace gtsam {
 
 		// update solution - todo: potentially expensive
 		delta_ = optimize2(*this);
-
 	}
 
 	template<class Conditional, class Config>

cpp/testBayesTree.cpp

@@ -214,7 +214,7 @@ TEST( BayesTree, removeTop )
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
 	SymbolicBayesTree::Cliques orphans;
-	bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
+	bayesTree.removeTop<SymbolicFactor>(newFactor->keys(), factors, orphans);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;
@@ -231,7 +231,7 @@ TEST( BayesTree, removeTop )
 	boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
 	FactorGraph<SymbolicFactor> factors2;
 	SymbolicBayesTree::Cliques orphans2;
-	bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
+	bayesTree.removeTop<SymbolicFactor>(newFactor2->keys(), factors2, orphans2);
 	SymbolicFactorGraph expected2;
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
 	SymbolicBayesTree::Cliques expectedOrphans2;
@@ -252,7 +252,7 @@ TEST( BayesTree, removeTop2 )
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
 	SymbolicBayesTree::Cliques orphans;
-	boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactors);
+	bayesTree.removeTop<SymbolicFactor>(newFactors.keys(), factors, orphans);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;