diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index ea049b277..1da0baa0c 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -87,8 +87,7 @@ namespace gtsam // Check for indeterminant solution if(soln.hasNaN()) { - std::cout << "OptimizeClique failed: solution has NaN!" << std::endl; - clique->print("Problematic clique: "); + std::cout << "linearAlgorithms::OptimizeClique failed: solution has NaN!" << std::endl; throw IndeterminantLinearSystemException(c.keys().front()); } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 7dad5eeec..0dd782ffd 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -170,7 +170,6 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh GaussianConditional& c = *clique->conditional(); // Solve matrix Vector xS; - Vector xS0; // Duy: for debug only { // Count dimensions of vector DenseIndex dim = 0; @@ -190,19 +189,12 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh vectorPos += parentVector.size(); } } - xS0 = xS; xS = c.getb() - c.get_S() * xS; Vector soln = c.get_R().triangularView().solve(xS); // Check for indeterminant solution if(soln.hasNaN()) { std::cout << "iSAM2 failed: solution has NaN!!" << std::endl; - c.print("Clique conditional: "); - std::cout << "R: " << c.get_R() << std::endl; - std::cout << "S: " << c.get_S().transpose() << std::endl; - std::cout << "b: " << c.getb().transpose() << std::endl; - std::cout << "xS0: " << xS0.transpose() << std::endl; - std::cout << "xS: " << xS.transpose() << std::endl; throw IndeterminantLinearSystemException(c.keys().front()); }