diff --git a/.cproject b/.cproject
index fb4a2edae..6fb1f3e80 100644
--- a/.cproject
+++ b/.cproject
@@ -753,10 +753,18 @@
true
true
-
+
make
-j2
-testKey.run
+testISAM.run
+true
+true
+true
+
+
+make
+-j2
+testGaussianISAM.run
true
true
true
diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index 3ae4c9b64..547b7ed1b 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -402,11 +402,11 @@ namespace gtsam {
/* ************************************************************************* */
template
template
- void BayesTree::removeTop(const boost::shared_ptr& newFactor,
+ void BayesTree::removeTop(const list& keys,
FactorGraph &factors, typename BayesTree::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
- template
- pair, typename BayesTree::Cliques>
- BayesTree::removeTop(const FactorGraph& newFactors) {
- // Remove the contaminated part of the Bayes tree
- FactorGraph factors;
- Cliques orphans;
-
- BOOST_FOREACH(boost::shared_ptr factor, newFactors)
- this->removeTop(factor, factors, orphans);
-
- return make_pair(factors,orphans);
- }
-
/* ************************************************************************* */
}
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index 999a649f8..30ae356bf 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -174,21 +174,13 @@ namespace gtsam {
std::pair, 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
- void removeTop(const boost::shared_ptr& newFactor,
+ void removeTop(const std::list& keys,
FactorGraph &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
- std::pair, Cliques> removeTop(const FactorGraph& newFactors);
-
}; // BayesTree
} /// namespace gtsam
diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h
index 7bd75702e..f4ccd853f 100644
--- a/cpp/ISAM-inl.h
+++ b/cpp/ISAM-inl.h
@@ -31,7 +31,7 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree
FactorGraph factors;
- boost::tie(factors, orphans) = removeTop(newFactors);
+ removeTop(newFactors.keys(), factors, orphans);
// add the factors themselves
factors.push_back(newFactors);
diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h
index 708424c44..ffc236790 100644
--- a/cpp/ISAM2-inl.h
+++ b/cpp/ISAM2-inl.h
@@ -119,6 +119,9 @@ namespace gtsam {
void ISAM2::update_internal(const NonlinearFactorGraph& 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 affectedFactors;
- boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
+// list keysToBeRemoved = newFactorsLinearized.keys(); // todo
+ list keysToBeRemoved = nonlinearFactors_.keys();
+ this->removeTop(keysToBeRemoved, affectedFactors, orphans);
// relinearize the affected factors ...
list affectedKeys = affectedFactors.keys();
@@ -172,7 +177,6 @@ namespace gtsam {
// update solution - todo: potentially expensive
delta_ = optimize2(*this);
-
}
template
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index 628e8d6fb..6f030b914 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -214,7 +214,7 @@ TEST( BayesTree, removeTop )
// Remove the contaminated part of the Bayes tree
FactorGraph factors;
SymbolicBayesTree::Cliques orphans;
- bayesTree.removeTop(newFactor, factors, orphans);
+ bayesTree.removeTop(newFactor->keys(), factors, orphans);
// Check expected outcome
SymbolicFactorGraph expected;
@@ -231,7 +231,7 @@ TEST( BayesTree, removeTop )
boost::shared_ptr newFactor2(new SymbolicFactor("B"));
FactorGraph factors2;
SymbolicBayesTree::Cliques orphans2;
- bayesTree.removeTop(newFactor2, factors2, orphans2);
+ bayesTree.removeTop(newFactor2->keys(), factors2, orphans2);
SymbolicFactorGraph expected2;
CHECK(assert_equal((FactorGraph)expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2;
@@ -252,7 +252,7 @@ TEST( BayesTree, removeTop2 )
// Remove the contaminated part of the Bayes tree
FactorGraph factors;
SymbolicBayesTree::Cliques orphans;
- boost::tie(factors,orphans) = bayesTree.removeTop(newFactors);
+ bayesTree.removeTop(newFactors.keys(), factors, orphans);
// Check expected outcome
SymbolicFactorGraph expected;