From 7226f910bb5faf3c3a1acabee927609f232e15cd Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 3 Apr 2012 22:06:06 +0000
Subject: [PATCH] Bug fix when relinearizing on first step in iSAM2

---
 gtsam/nonlinear/ISAM2.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 228b6e491..0c05d15c4 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -743,7 +743,8 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
   g.setZero();
 
   // Sum up contributions for each clique
-  gradientAtZeroTreeAdder(bayesTree.root(), g);
+  if(bayesTree.root())
+    gradientAtZeroTreeAdder(bayesTree.root(), g);
 }
 
 }