diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h
index bf2cacc84..4471567fc 100644
--- a/gtsam/inference/ISAM-inst.h
+++ b/gtsam/inference/ISAM-inst.h
@@ -22,43 +22,48 @@
 
 namespace gtsam {
 
-  /* ************************************************************************* */
-  template<class BAYESTREE>
-  void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function)
-  {
-    // Remove the contaminated part of the Bayes tree
-    BayesNetType bn;
-    if (!this->empty()) {
-      const KeySet newFactorKeys = newFactors.keys();
-      this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
-    }
-
-    // Add the removed top and the new factors
-    FactorGraphType factors;
-    factors += bn;
-    factors += newFactors;
-
-    // Add the orphaned subtrees
-    for(const sharedClique& orphan: orphans)
-      factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
-
-    // eliminate into a Bayes net
-    const VariableIndex varIndex(factors);
-    const KeySet newFactorKeys = newFactors.keys();
-    const Ordering constrainedOrdering =
-      Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
-    Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
-    this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
-    this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
+/* ************************************************************************* */
+template<class BAYESTREE>
+void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
+    Cliques& orphans, const Eliminate& function) {
+  // Remove the contaminated part of the Bayes tree
+  BayesNetType bn;
+  const KeySet newFactorKeys = newFactors.keys();
+  if (!this->empty()) {
+    std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
+    this->removeTop(keyVector, bn, orphans);
   }
 
-  /* ************************************************************************* */
-  template<class BAYESTREE>
-  void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function)
-  {
-    Cliques orphans;
-    this->update_internal(newFactors, orphans, function);
-  }
+  // Add the removed top and the new factors
+  FactorGraphType factors;
+  factors += bn;
+  factors += newFactors;
+
+  // Add the orphaned subtrees
+  for (const sharedClique& orphan : orphans)
+    factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
+
+  // Get an ordering where the new keys are eliminated last
+  const VariableIndex index(factors);
+  const Ordering ordering = Ordering::ColamdConstrainedLast(index,
+      std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
+
+  // eliminate all factors (top, added, orphans) into a new Bayes tree
+  auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
+
+  // Re-add into Bayes tree data structures
+  this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
+      bayesTree->roots().end());
+  this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
+}
+
+/* ************************************************************************* */
+template<class BAYESTREE>
+void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
+    const Eliminate& function) {
+  Cliques orphans;
+  this->update_internal(newFactors, orphans, function);
+}
 
 }
 /// namespace gtsam