From 51949ba61425d7eca03a49eadf2077fee48795b6 Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Wed, 14 Mar 2012 15:49:16 +0000
Subject: [PATCH] Fixed iSAM2 bug if back-substitution is triggered before the
 first elimination.

---
 gtsam/nonlinear/GaussianISAM2-inl.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h
index 822eb9674..b52e22629 100644
--- a/gtsam/nonlinear/GaussianISAM2-inl.h
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -109,7 +109,8 @@ namespace gtsam {
     vector<bool> changed(keys.size(), false);
     int count = 0;
     // starting from the root, call optimize on each conditional
-    internal::optimizeWildfire(root, threshold, changed, keys, delta, count);
+    if(root)
+      internal::optimizeWildfire(root, threshold, changed, keys, delta, count);
     return count;
   }