diff --git a/.cproject b/.cproject
index cda3d60e5..71be9ac62 100644
--- a/.cproject
+++ b/.cproject
@@ -633,6 +633,14 @@
true
true
+
+make
+
+testGaussianISAM2.run
+true
+true
+true
+
make
diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index 58f489520..819a5b0fa 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -400,7 +400,6 @@ namespace gtsam {
}
/* ************************************************************************* */
- // TODO: add to factors and orphans
template
template
void BayesTree::removeTop(const boost::shared_ptr& newFactor,
diff --git a/cpp/GaussianISAM.cpp b/cpp/GaussianISAM.cpp
index ed3f37bef..aa7ff3026 100644
--- a/cpp/GaussianISAM.cpp
+++ b/cpp/GaussianISAM.cpp
@@ -1,6 +1,6 @@
/**
* @file GaussianISAM
- * @brief
+ * @brief Linear ISAM only
* @author Michael Kaess
*/
diff --git a/cpp/GaussianISAM.h b/cpp/GaussianISAM.h
index 63e533d27..ef1f15e22 100644
--- a/cpp/GaussianISAM.h
+++ b/cpp/GaussianISAM.h
@@ -1,6 +1,6 @@
/**
* @file GaussianISAM
- * @brief
+ * @brief Linear ISAM only
* @author Michael Kaess
*/
diff --git a/cpp/GaussianISAM2.cpp b/cpp/GaussianISAM2.cpp
new file mode 100644
index 000000000..64d147237
--- /dev/null
+++ b/cpp/GaussianISAM2.cpp
@@ -0,0 +1,42 @@
+/**
+ * @file GaussianISAM2
+ * @brief Full non-linear ISAM
+ * @author Michael Kaess
+ */
+
+#include "GaussianISAM2.h"
+
+using namespace std;
+using namespace gtsam;
+
+// Explicitly instantiate so we don't have to include everywhere
+#include "ISAM2-inl.h"
+template class ISAM2;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
+ // parents are assumed to already be solved and available in result
+ GaussianISAM2::Clique::const_reverse_iterator it;
+ for (it = clique->rbegin(); it!=clique->rend(); it++) {
+ GaussianConditional::shared_ptr cg = *it;
+ Vector x = cg->solve(result); // Solve for that variable
+ result.insert(cg->key(), x); // store result in partial solution
+ }
+ BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
+// list::const_iterator child;
+// for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
+ optimize2(child, result);
+ }
+}
+
+/* ************************************************************************* */
+VectorConfig optimize2(const GaussianISAM2& bayesTree) {
+ VectorConfig result;
+ // starting from the root, call optimize on each conditional
+ optimize2(bayesTree.root(), result);
+ return result;
+}
+
+} /// namespace gtsam
diff --git a/cpp/GaussianISAM2.h b/cpp/GaussianISAM2.h
new file mode 100644
index 000000000..76ee7a780
--- /dev/null
+++ b/cpp/GaussianISAM2.h
@@ -0,0 +1,25 @@
+/**
+ * @file GaussianISAM
+ * @brief Full non-linear ISAM.
+ * @author Michael Kaess
+ */
+
+// \callgraph
+
+#pragma once
+
+#include "ISAM2.h"
+#include "GaussianConditional.h"
+#include "GaussianFactor.h"
+
+namespace gtsam {
+
+ typedef ISAM2 GaussianISAM2;
+
+ // recursively optimize this conditional and all subtrees
+ void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result);
+
+ // optimize the BayesTree, starting from the root
+ VectorConfig optimize2(const GaussianISAM2& bayesTree);
+
+}/// namespace gtsam
diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h
new file mode 100644
index 000000000..24a828002
--- /dev/null
+++ b/cpp/ISAM2-inl.h
@@ -0,0 +1,85 @@
+/**
+ * @file ISAM2-inl.h
+ * @brief Incremental update functionality (ISAM2) for BayesTree.
+ * @author Michael Kaess
+ */
+
+#include
+#include // for operator +=
+using namespace boost::assign;
+
+#include "NonlinearFactorGraph.h"
+#include "GaussianFactor.h"
+#include "VectorConfig.h"
+
+#include "Conditional.h"
+#include "BayesTree-inl.h"
+#include "ISAM2.h"
+
+namespace gtsam {
+
+ using namespace std;
+
+ /** Create an empty Bayes Tree */
+ template
+ ISAM2::ISAM2() : BayesTree() {}
+
+ /** Create a Bayes Tree from a Bayes Net */
+ template
+ ISAM2::ISAM2(const BayesNet& bayesNet) : BayesTree(bayesNet) {}
+
+ /* ************************************************************************* */
+ template
+ void ISAM2::update_internal(const NonlinearFactorGraph& newFactorsXXX, Cliques& orphans) {
+
+ Config xxx;
+ FactorGraph newFactors; //todo = newFactorsXXX.linearize(xxx);
+
+ // Remove the contaminated part of the Bayes tree
+ FactorGraph factors;
+ boost::tie(factors, orphans) = this->removeTop(newFactors);
+
+ // add the factors themselves
+ factors.push_back(newFactors);
+
+ // create an ordering for the new and contaminated factors
+ Ordering ordering;
+ if (true) {
+ ordering = factors.getOrdering();
+ } else {
+ list keys = factors.keys();
+ keys.sort(); // todo: correct sorting order?
+ ordering = keys;
+ }
+
+ // eliminate into a Bayes net
+ BayesNet bayesNet = eliminate(factors,ordering);
+
+ // insert conditionals back in, straight into the topless bayesTree
+ typename BayesNet::const_reverse_iterator rit;
+ for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
+ this->insert(*rit);
+
+ int count = 0;
+ // add orphans to the bottom of the new tree
+ BOOST_FOREACH(sharedClique orphan, orphans) {
+
+ string key = orphan->separator_.front();
+ sharedClique parent = (*this)[key];
+
+ parent->children_ += orphan;
+ orphan->parent_ = parent; // set new parent!
+ }
+
+ }
+
+ template
+ void ISAM2::update(const NonlinearFactorGraph& newFactors) {
+ Cliques orphans;
+ this->update_internal(newFactors, orphans);
+ }
+
+/* ************************************************************************* */
+
+}
+/// namespace gtsam
diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h
new file mode 100644
index 000000000..6f2a6ac3c
--- /dev/null
+++ b/cpp/ISAM2.h
@@ -0,0 +1,55 @@
+/**
+ * @file ISAM2.h
+ * @brief Incremental update functionality (ISAM2) for BayesTree.
+ * @author Michael Kaess
+ */
+
+// \callgraph
+
+#pragma once
+
+#include