remove debug info
parent
b6ab7a4dfa
commit
f96e7ef32f
|
@ -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());
|
||||
}
|
||||
|
||||
|
|
|
@ -170,7 +170,6 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& 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>& clique, double thresh
|
|||
vectorPos += parentVector.size();
|
||||
}
|
||||
}
|
||||
xS0 = xS;
|
||||
xS = c.getb() - c.get_S() * xS;
|
||||
Vector soln = c.get_R().triangularView<Eigen::Upper>().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());
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue