12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove debug info

release/4.3a0
thduynguyen 2014-11-21 11:13:39 -05:00
parent b6ab7a4dfa
commit f96e7ef32f
2 changed files with 1 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/linearAlgorithms-inst.h
View File

@ -87,8 +87,7 @@ namespace gtsam
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) { if(soln.hasNaN()) {
std::cout << "OptimizeClique failed: solution has NaN!" << std::endl; std::cout << "linearAlgorithms::OptimizeClique failed: solution has NaN!" << std::endl;
clique->print("Problematic clique: ");
throw IndeterminantLinearSystemException(c.keys().front()); throw IndeterminantLinearSystemException(c.keys().front());
} }

8
gtsam/nonlinear/ISAM2-inl.h
View File

@ -170,7 +170,6 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
GaussianConditional& c = *clique->conditional(); GaussianConditional& c = *clique->conditional();
// Solve matrix // Solve matrix
Vector xS; Vector xS;
Vector xS0; // Duy: for debug only
{ {
// Count dimensions of vector // Count dimensions of vector
DenseIndex dim = 0; DenseIndex dim = 0;
@ -190,19 +189,12 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
vectorPos += parentVector.size(); vectorPos += parentVector.size();
} }
} }
xS0 = xS;
xS = c.getb() - c.get_S() * xS; xS = c.getb() - c.get_S() * xS;
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) { if(soln.hasNaN()) {
std::cout << "iSAM2 failed: solution has NaN!!" << std::endl; 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()); throw IndeterminantLinearSystemException(c.keys().front());
} }