diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 58cead05a..6bd853764 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -129,10 +129,7 @@ namespace gtsam { Vector soln = get_R().triangularView().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 { } } - diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index d4106cbfa..f282682b3 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -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()); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 06f6658f9..a63bbf473 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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() = @@ -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())); } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 1da0baa0c..4722a9b6d 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -86,10 +86,7 @@ namespace gtsam Vector soln = c.get_R().triangularView().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; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 0dd782ffd..36ebd7033 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -115,7 +115,6 @@ template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, FastSet& changed, const FastSet& 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, double thresh Vector soln = c.get_R().triangularView().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) { } - diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index e70aa300f..0dca18a1f 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -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"<& 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); } - - - - }; /**