diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 83e483351..ea507e3d2 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -64,23 +64,25 @@ void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; - // Obtain the new linearization point - const Values newLinPoint = estimate(); + if(factors_.size() > 0) { + // Obtain the new linearization point + const Values newLinPoint = estimate(); - isam_.clear(); + isam_.clear(); - // Compute an ordering - ordering_ = *factors_.orderingCOLAMD(newLinPoint); + // Compute an ordering + ordering_ = *factors_.orderingCOLAMD(newLinPoint); - // Create a linear factor graph at the new linearization point - boost::shared_ptr gfg( - factors_.linearize(newLinPoint, ordering_)->template dynamicCastFactors()); + // Create a linear factor graph at the new linearization point + boost::shared_ptr gfg( + factors_.linearize(newLinPoint, ordering_)->template dynamicCastFactors()); - // Just recreate the whole BayesTree - isam_.update(*gfg); + // Just recreate the whole BayesTree + isam_.update(*gfg); - // Update linearization point - linPoint_ = newLinPoint; + // Update linearization point + linPoint_ = newLinPoint; + } } /* ************************************************************************* */