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

Removed print and cout statements.

release/4.3a0
krunalchande 2014-11-22 17:39:12 -05:00
parent 056aed4419
commit ca792cf041
7 changed files with 8 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/linear/GaussianConditional.cpp
View File

@ -129,10 +129,7 @@ namespace gtsam {
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution
if(soln.hasNaN()) {
std::cout << "GaussianConditional::solve failed: solution has NaN!!" << std::endl;
throw IndeterminantLinearSystemException(keys().front());
}
if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front());
// TODO, do we not have to scale by sigmas here? Copy/paste bug
@ -183,10 +180,7 @@ namespace gtsam {
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// Check for indeterminant solution
if (frontalVec.hasNaN()) {
std::cout << "GaussianConditional::solveTransposeInPlace failed: frontalVec has NaN!!" << std::endl;
throw IndeterminantLinearSystemException(this->keys().front());
}
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
for (const_iterator it = beginParents(); it!= endParents(); it++)
gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
@ -214,4 +208,3 @@ namespace gtsam {
}
}

1
gtsam/linear/HessianFactor.cpp
View File

@ -641,7 +641,6 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
// Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate);
} catch(CholeskyFailed&) {
std::cout << "EliminateCholesky failed!" << std::endl;
throw IndeterminantLinearSystemException(keys.front());
}

8
gtsam/linear/JacobianFactor.cpp
View File

@ -116,10 +116,8 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
// Check for indefinite system
if (!success) {
std::cout << "JacobianFactor constructor from HessianFactor failed!" << std::endl;
if (!success)
throw IndeterminantLinearSystemException(factor.keys().front());
}
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
@ -693,10 +691,8 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
SharedDiagonal conditionalNoiseModel;
if (model_) {
if ((DenseIndex) model_->dim() < frontalDim) {
std::cout << "Jacobian::splitConditional failed" << std::endl;
if ((DenseIndex) model_->dim() < frontalDim)
throw IndeterminantLinearSystemException(this->keys().front());
}
conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
}

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

@ -86,10 +86,7 @@ namespace gtsam
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution
if(soln.hasNaN()) {
std::cout << "linearAlgorithms::OptimizeClique failed: solution has NaN!" << std::endl;
throw IndeterminantLinearSystemException(c.keys().front());
}
if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
// Insert solution into a VectorValues
DenseIndex vectorPosition = 0;

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

@ -115,7 +115,6 @@ template<class CLIQUE>
bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count)
{
//clique->print("Input clique: ");
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
@ -193,10 +192,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
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;
throw IndeterminantLinearSystemException(c.keys().front());
}
if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
// Insert solution into a VectorValues
DenseIndex vectorPosition = 0;
@ -307,4 +303,3 @@ int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique) {
}

52
gtsam/nonlinear/Marginals.cpp
View File

@ -38,38 +38,10 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
// Compute BayesTree
factorization_ = factorization;
if(factorization_ == CHOLESKY) {
if(factorization_ == CHOLESKY)
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky);
std::cout<<"performing Cholesky"<<std::endl;
}
else if(factorization_ == QR) {
else if(factorization_ == QR)
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR);
std::cout<<"performing QR"<<std::endl;
}
}
Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, const std::string& str) {
Factorization factorization;
if (str == "QR") factorization = QR;
else if (str == "CHOLESKY") factorization = CHOLESKY;
gttic(MarginalsConstructor);
// Linearize graph
graph_ = *graph.linearize(solution);
// Store values
values_ = solution;
// Compute BayesTree
factorization_ = factorization;
if(factorization_ == CHOLESKY) {
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky);
std::cout<<"performing Cholesky"<<std::endl;
}
else if(factorization_ == QR) {
bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR);
std::cout<<"performing QR"<<std::endl;
}
}
/* ************************************************************************* */
@ -171,24 +143,4 @@ void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) c
cout << ". Use 'at' or 'operator()' to query matrix blocks." << endl;
}
Marginals::Factorization Marginals::factorizationTranslator(const std::string& str) const {
// std::string s = str; boost::algorithm::to_upper(s);
if (str == "QR") return Marginals::QR;
if (str == "CHOLESKY") return Marginals::CHOLESKY;
/* default is CHOLESKY */
return Marginals::CHOLESKY;
}
/* ************************************************************************* */
std::string Marginals::factorizationTranslator(const Marginals::Factorization& value) const {
std::string s;
switch (value) {
case Marginals::QR: s = "QR"; break;
case Marginals::CHOLESKY: s = "CHOLESKY"; break;
default: s = "UNDEFINED"; break;
}
return s;
}
} /* namespace gtsam */

11
gtsam/nonlinear/Marginals.h
View File

@ -54,7 +54,6 @@ public:
* @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
*/
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
Marginals(const NonlinearFactorGraph& graph, const Values& solution, const std::string& str);
/** print */
void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
@ -72,16 +71,6 @@ public:
/** Compute the joint marginal information of several variables */
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
Factorization factorizationTranslator(const std::string& str) const;
std::string factorizationTranslator(const Marginals::Factorization& value) const;
void setFactorization(const std::string& factorization) { this->factorization_ = factorizationTranslator(factorization); }
};
/**