fixed bug when reorder/relinearize is called with no factors

gtsam/nonlinear/NonlinearISAM-inl.h

@@ -64,6 +64,7 @@ void NonlinearISAM<Values>::reorder_relinearize() {
 
 //  cout << "Reordering, relinearizing..." << endl;
 
+  if(factors_.size() > 0) {
     // Obtain the new linearization point
     const Values newLinPoint = estimate();
 
@@ -81,6 +82,7 @@ void NonlinearISAM<Values>::reorder_relinearize() {
 
     // Update linearization point
     linPoint_ = newLinPoint;
+  }
 }
 
 /* ************************************************************************* */