diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 86114f4d8..8a02907c9 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -24,6 +24,7 @@ #include #include +#include using namespace std; @@ -32,18 +33,23 @@ namespace gtsam { static const double negativePivotThreshold = -1e-1; static const double zeroPivotThreshold = 1e-6; static const double underconstrainedPrior = 1e-5; + static const int underconstrainedExponentDifference = 12; /* ************************************************************************* */ -static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) { +static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) { + + const bool debug = ISDEBUG("choleskyCareful"); // Get pivot value double alpha = ATA(k,k); // Correct negative pivots from round-off error if(alpha < negativePivotThreshold) { - cout << "pivot = " << alpha << endl; - print(ATA, "Partially-factorized matrix: "); - throw(CarefulCholeskyNegativeMatrixException()); + if(debug) { + cout << "pivot = " << alpha << endl; + print(ATA, "Partially-factorized matrix: "); + } + return -1; } else if(alpha < 0.0) alpha = 0.0; @@ -66,14 +72,14 @@ static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) { // ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() // .rankUpdate(V.adjoint(), -1); } - - return true; + return 1; } else { // For zero pivots, add the underconstrained variable prior ATA(k,k) = underconstrainedPrior; for(size_t j=k+1; j choleskyCareful(Matrix& ATA, int order) { // Negative order means factor the entire matrix if(order < 0) - order = n; + order = int(n); assert(size_t(order) <= n); // The index of the row after the last non-zero row of the square-root factor size_t maxrank = 0; - bool fullRank = true; + bool success = true; // Factor row-by-row for(size_t k = 0; k < size_t(order); ++k) { - if(choleskyStep(ATA, k, size_t(order))) { + int stepResult = choleskyStep(ATA, k, size_t(order)); + if(stepResult == 1) { if(debug) cout << "choleskyCareful: Factored through " << k << endl; if(debug) print(ATA, "ATA: "); maxrank = k+1; - } else { - fullRank = false; - if(debug) cout << "choleskyCareful: Skipping " << k << endl; - } + } else if(stepResult == -1) { + success = false; + break; + } /* else if(stepResult == 0) Found zero pivot */ } - return make_pair(maxrank, fullRank); + return make_pair(maxrank, success); } /* ************************************************************************* */ -void choleskyPartial(Matrix& ABC, size_t nFrontal) { +bool choleskyPartial(Matrix& ABC, size_t nFrontal) { const bool debug = ISDEBUG("choleskyPartial"); @@ -125,9 +132,8 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { // Compute Cholesky factorization of A, overwrites A. tic(1, "lld"); - ABC.block(0,0,nFrontal,nFrontal).triangularView() = - ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt().matrixU(); - assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView().toDenseMatrix().unaryExpr(ptr_fun(isfinite)).all()); + Eigen::LLT llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt(); + ABC.block(0,0,nFrontal,nFrontal).triangularView() = llt.matrixU(); toc(1, "lld"); if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView()) << endl; @@ -138,7 +144,6 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { ABC.topLeftCorner(nFrontal,nFrontal).triangularView().transpose().solveInPlace( ABC.topRightCorner(nFrontal, n-nFrontal)); } - assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(ptr_fun(isfinite)).all()); if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; toc(2, "compute S"); @@ -148,9 +153,27 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); - assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().toDenseMatrix().unaryExpr(ptr_fun(isfinite)).all()); if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; toc(3, "compute L"); + + // Check last diagonal element - Eigen does not check it + bool ok; + if(llt.info() == Eigen::Success) { + if(nFrontal >= 2) { + int exp2, exp1; + (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2); + (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1); + ok = (exp2 - exp1 < underconstrainedExponentDifference); + } else if(nFrontal == 1) { + int exp1; + (void)frexp(ABC(0,0), &exp1); + ok = (exp1 > -underconstrainedExponentDifference); + } + } else { + ok = false; + } + + return ok; } } diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 46d342d24..8b1073107 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -22,45 +22,6 @@ namespace gtsam { -/** - * An exception indicating an attempt to factor a negative or indefinite matrix. - * If detailed exceptions are enabled, then the \c detail member will contain - * the matrices leading to the problem, see documentation for - * NegativeMatrixException::Detail. - */ -struct NegativeMatrixException : public std::exception { - /// Detail for NegativeMatrixException - struct Detail { - Matrix A; ///< The original matrix attempted to factor - Matrix U; ///< The produced upper-triangular factor - Matrix D; ///< The produced diagonal factor - Detail(const Matrix& A, const Matrix& U, const Matrix& D) /**< Detail constructor */ : A(A), U(U), D(D) {} - void print(const std::string& str = "") const { - std::cout << str << "\n"; - gtsam::print(A, " A: "); - gtsam::print(U, " U: "); - gtsam::print(D, " D: "); - } - }; - const boost::shared_ptr detail; ///< Detail - NegativeMatrixException() /**< Constructor with no detail */ {} - NegativeMatrixException(const Detail& _detail) /**< Constructor with detail */ : detail(new Detail(_detail)) {} - NegativeMatrixException(const boost::shared_ptr& _detail) /**< Constructor with detail */ : detail(_detail) {} - virtual ~NegativeMatrixException() throw() {} -}; - -/** - * An exception indicating an attempt to factor a negative or indefinite matrix. - * If detailed exceptions are enabled, then the \c detail member will contain - * the matrices leading to the problem, see documentation for - * CarefulCholeskyNegativeMatrixException::Detail. - */ -struct CarefulCholeskyNegativeMatrixException : public std::exception { - CarefulCholeskyNegativeMatrixException() throw() {} - virtual ~CarefulCholeskyNegativeMatrixException() throw() {} - const char* what() const throw() { return "The matrix was found to be non-positive-semidefinite when factoring with careful Cholesky."; } -}; - /** * "Careful" Cholesky computes the positive square-root of a positive symmetric * semi-definite matrix (i.e. that may be rank-deficient). Unlike standard @@ -70,6 +31,10 @@ struct CarefulCholeskyNegativeMatrixException : public std::exception { * non-zero row in the computed factor, so that it may be truncated to an * upper-trapazoidal matrix. * + * The second element of the return value is \c true if the matrix was factored + * successfully, or \c false if it was non-positive-semidefinite (i.e. + * indefinite or negative-(semi-)definite. + * * Note that this returned index is the rank of the matrix if and only if all * of the zero-rows of the factor occur after any non-zero rows. This is * (always?) the case during elimination of a fully-constrained least-squares @@ -77,6 +42,8 @@ struct CarefulCholeskyNegativeMatrixException : public std::exception { * * The optional order argument specifies the size of the square upper-left * submatrix to operate on, ignoring the rest of the matrix. + * + * */ std::pair choleskyCareful(Matrix& ATA, int order = -1); @@ -87,8 +54,11 @@ std::pair choleskyCareful(Matrix& ATA, int order = -1); * [B' C] * nFrontal determines the split between A, B, and C, with A being of size * nFrontal x nFrontal. + * + * @return \c true if the decomposition is successful, \c false if \c A was + * not positive-definite. */ -void choleskyPartial(Matrix& ABC, size_t nFrontal); +bool choleskyPartial(Matrix& ABC, size_t nFrontal); } diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 63fdec810..55c72543e 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -84,6 +84,46 @@ TEST(cholesky, BadScalingSVD) { DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41); } +/* ************************************************************************* */ +TEST(cholesky, underconstrained) { + Matrix L(6,6); L << + 1, 0, 0, 0, 0, 0, + 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0, + 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0, + 1.12108665967793, 1.01584408366945, 1, 0, 0, 0, + 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1, + 0.1197220685104, 1, 0, 0, 0, 0; + Matrix D1(6,6); D1 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 1.34342584865901, 0, + 0, 0, 0, 0, 0, 1e-12; + Matrix D2(6,6); D2 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0; + Matrix D3(6,6); D3 << + 0.814723686393179, 0, 0, 0, 0, 0, + 0, 0.811780089277421, 0, 0, 0, 0, + 0, 0, 1.82596950680844, 0, 0, 0, + 0, 0, 0, 0.240287537694585, 0, 0, + 0, 0, 0, 0, -0.5, 0, + 0, 0, 0, 0, 0, -0.6; + + Matrix A1 = L * D1 * L.transpose(); + Matrix A2 = L * D2 * L.transpose(); + Matrix A3 = L * D3 * L.transpose(); + + LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6))); + LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6))); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index e167d8ad8..b35bffd3c 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -17,8 +17,10 @@ #include #include +#include #include +#include #include #include #include @@ -189,6 +191,11 @@ void GaussianConditional::solveInPlace(VectorValues& x) const { Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents()); xS = this->get_d() - this->get_S() * xS; Vector soln = this->get_R().triangularView().solve(xS); + + // Check for indeterminant solution + if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) + throw IndeterminantLinearSystemException(this->keys().front()); + if(debug) { gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); gtsam::print(soln, "full back-substitution solution: "); @@ -199,8 +206,12 @@ void GaussianConditional::solveInPlace(VectorValues& x) const { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); - // TODO: verify permutation frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R())); + + // Check for indeterminant solution + if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite), boost::lambda::_1)).any()) + throw IndeterminantLinearSystemException(this->keys().front()); + GaussianConditional::const_iterator it; for (it = beginParents(); it!= endParents(); it++) { const Index i = *it; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6539f0cf4..dd86ffca6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -393,14 +393,7 @@ break; // some untouched non-zeros that should be zero. We zero them while // extracting submatrices next. tic(4, "partial Cholesky"); - try { - combinedFactor->partialCholesky(nrFrontals); - } catch - (std::exception &ex) { // catch exception from Cholesky - combinedFactor->print("combinedFactor"); - string reason = "EliminateCholesky failed while trying to eliminate the combined factor"; - throw invalid_argument(reason); - } + combinedFactor->partialCholesky(nrFrontals); toc(4, "partial Cholesky"); @@ -502,64 +495,9 @@ break; return EliminateQR(factors, nrFrontals); else { GaussianFactorGraph::EliminationResult ret; -#ifdef NDEBUG - static const bool diag = false; -#else - static const bool diag = !ISDEBUG("NoCholeskyDiagnostics"); -#endif - if (!diag) { - tic(2, "EliminateCholesky"); - ret = EliminateCholesky(factors, nrFrontals); - toc(2, "EliminateCholesky"); - } else { - try { - tic(2, "EliminateCholesky"); - ret = EliminateCholesky(factors, nrFrontals); - toc(2, "EliminateCholesky"); - } catch (const exception& e) { - cout << "Exception in EliminateCholesky: " << e.what() << endl; - SETDEBUG("EliminateCholesky", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("choleskyPartial", true); - factors.print("Combining factors: "); - EliminateCholesky(factors, nrFrontals); - throw; - } - } - - const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky"); - if (checkCholesky) { - GaussianFactorGraph::EliminationResult expected; - FactorGraph jacobians = convertToJacobians(factors); - try { - // Compare with QR - expected = EliminateJacobians(jacobians, nrFrontals); - } catch (...) { - cout << "Exception in QR" << endl; - throw; - } - - H actual_factor(*ret.second); - H expected_factor(*expected.second); - if (!assert_equal(*expected.first, *ret.first, 100.0) - || !assert_equal(expected_factor, actual_factor, 1.0)) { - cout << "Cholesky and QR do not agree" << endl; - - SETDEBUG("EliminateCholesky", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - jacobians.print("Jacobian Factors: "); - EliminateJacobians(jacobians, nrFrontals); - EliminateCholesky(factors, nrFrontals); - factors.print("Combining factors: "); - - throw runtime_error("Cholesky and QR do not agree"); - } - } - + tic(2, "EliminateCholesky"); + ret = EliminateCholesky(factors, nrFrontals); + toc(2, "EliminateCholesky"); return ret; } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cdf597411..6db6330b0 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -47,13 +48,7 @@ string SlotEntry::toString() const { /* ************************************************************************* */ void HessianFactor::assertInvariants() const { -#ifndef NDEBUG - // Check for non-finite values - for(size_t i=0; i<(size_t) matrix_.rows(); ++i) - for(size_t j=i; j<(size_t) matrix_.cols(); ++j) - if(!isfinite(matrix_(i,j))) - throw invalid_argument("HessianFactor contains non-finite matrix entries."); -#endif + GaussianFactor::assertInvariants(); // The base class checks for unique keys } /* ************************************************************************* */ @@ -473,7 +468,8 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt /* ************************************************************************* */ void HessianFactor::partialCholesky(size_t nrFrontals) { - choleskyPartial(matrix_, info_.offset(nrFrontals)); + if(!choleskyPartial(matrix_, info_.offset(nrFrontals))) + throw IndeterminantLinearSystemException(this->keys().front()); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 0fa3bd7f5..7974c33b4 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -15,6 +15,7 @@ * @date Dec 8, 2010 */ +#include #include #include #include @@ -50,12 +51,6 @@ namespace gtsam { assert(firstNonzeroBlocks_.size() == Ab_.rows()); for(size_t i=0; i() = Matrix::Zero(maxrank, matrix_.cols()); @@ -324,7 +317,6 @@ namespace gtsam { return model_->whiten(Ax); } - /* ************************************************************************* */ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const { @@ -410,11 +402,8 @@ namespace gtsam { size_t frontalDim = Ab_.range(0,nrFrontals).cols(); // Check for singular factor - if(model_->dim() < frontalDim) { - throw domain_error((boost::format( - "JacobianFactor is singular in variable %1%, discovered while attempting\n" - "to eliminate this variable.") % front()).str()); - } + if(model_->dim() < frontalDim) + throw IndeterminantLinearSystemException(this->keys().front()); // Extract conditional tic(3, "cond Rd"); diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h new file mode 100644 index 000000000..5e90f57b3 --- /dev/null +++ b/gtsam/linear/linearExceptions.h @@ -0,0 +1,180 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file linearExceptions.h + * @brief Exceptions that may be thrown by linear solver components + * @author Richard Roberts + * @date Aug 17, 2012 + */ +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + Thrown when a linear system is ill-posed. The most common cause for this + error is having underconstrained variables. Mathematically, the system is + either underdetermined, or its quadratic error function is concave in some + directions. + + Examples of situations causing this error are: + - A landmark observed by two cameras with a very small baseline will have + high uncertainty in its distance from the cameras but low uncertainty + in its bearing, creating a poorly-conditioned system. + - A landmark observed by only a single ProjectionFactor, RangeFactor, or + BearingFactor (the landmark is not completely constrained). + - An overall scale or rigid transformation ambiguity, for example missing + a prior or hard constraint on the first pose, or missing a scale + constraint between the first two cameras (in structure-from-motion). + + Mathematically, the following conditions cause this problem: + - Underdetermined system: This occurs when the variables are not + completely constrained by factors. Even if all variables are involved + in factors, the variables can still be numerically underconstrained, + (for example, if a landmark is observed by only one ProjectionFactor). + Mathematically in this case, the rank of the linear Jacobian or Hessian + is less than the number of scalars in the system. + - Indefinite system: This condition occurs when the system Hessian is + indefinite, i.e. non-positive-semidefinite. Note that this condition + can also indicate an underdetermined system, but when solving with + Cholesky, accumulation of numerical errors can cause the system to + become indefinite (have some negative eigenvalues) even if it is in + reality positive semi-definite (has some near-zero positive + eigenvalues). Alternatively, if the system contains some + HessianFactor's with negative eigenvalues, these can create a truly + indefinite system, whose quadratic error function has negative + curvature in some directions. + - Poorly-conditioned system: A system that is positive-definite (has a + unique solution) but is numerically ill-conditioned can accumulate + numerical errors during solving that cause the system to become + indefinite later during the elimination process. Note that poorly- + conditioned systems will usually have inaccurate solutions, even if + they do not always trigger this error. Systems with almost- + unconstrained variables or vastly different measurement uncertainties + or variable units can be poorly-conditioned. + + Resolving this problem: + - This exception contains the variable at which the problem was + discovered (IndeterminantLinearSystemException::nearbyVariable()). + Note, however, that this is not necessarily the variable where the + problem originates. For example, in the case that a prior on the + first camera was forgotten, it may only be another camera or landmark + where the problem can be detected. Where the problem is detected + depends on the graph structure and variable elimination ordering. + - MATLAB (or similar software) is a useful tool to diagnose this problem. + Use GaussianFactorGraph::sparseJacobian_(), + GaussianFactorGraph::sparseJacobian() + GaussianFactorGraph::denseJacobian(), and + GaussianFactorGraph::denseHessian() to output the linear graph in + matrix format. If using the MATLAB wrapper, the returned matrices can + be immediately inspected and analyzed using standard methods. If not + using the MATLAB wrapper, the output of these functions may be written + to text files and then loaded into MATLAB. + - When outputting linear graphs as Jacobian matrices, rows are ordered in + the same order as factors in the graph, which each factor occupying the + number of rows indicated by its JacobianFactor::rows() function. Each + column appears in elimination ordering, as in the Ordering class. Each + variable occupies the number of columns indicated by the + JacobianFactor::getDim() function. + - When outputting linear graphs as Hessian matrices, rows and columns are + ordered in elimination order and occupy scalars in the same way as + described for Jacobian columns in the previous bullet. + */ + class IndeterminantLinearSystemException : public std::exception { + Index j_; + mutable std::string what_; + public: + IndeterminantLinearSystemException(Index j) throw() : j_(j) {} + virtual ~IndeterminantLinearSystemException() throw() {} + Index nearbyVariable() const { return j_; } + virtual const char* what() const throw() { + if(what_.empty()) + what_ = +"\nIndeterminant linear system detected while working near variable with\n" +"index " + boost::lexical_cast(j_) + " in ordering.\n" +"\n\ +Thrown when a linear system is ill-posed. The most common cause for this\n\ +error is having underconstrained variables. Mathematically, the system is\n\ +either underdetermined, or its quadratic error function is concave in some\n\ +directions.\n\ +\n\ +Examples of situations causing this error are:\n\ +- A landmark observed by two cameras with a very small baseline will have\n\ + high uncertainty in its distance from the cameras but low uncertainty\n\ + in its bearing, creating a poorly-conditioned system.\n\ +- A landmark observed by only a single ProjectionFactor, RangeFactor, or\n\ + BearingFactor (the landmark is not completely constrained).\n\ +- An overall scale or rigid transformation ambiguity, for example missing\n\ + a prior or hard constraint on the first pose, or missing a scale\n\ + constraint between the first two cameras (in structure-from-motion).\n\ +\n\ +Mathematically, the following conditions cause this problem:\n\ +- Underdetermined system: This occurs when the variables are not\n\ + completely constrained by factors. Even if all variables are involved\n\ + in factors, the variables can still be numerically underconstrained,\n\ + (for example, if a landmark is observed by only one ProjectionFactor).\n\ + Mathematically in this case, the rank of the linear Jacobian or Hessian\n\ + is less than the number of scalars in the system.\n\ +- Indefinite system: This condition occurs when the system Hessian is\n\ + indefinite, i.e. non-positive-semidefinite. Note that this condition\n\ + can also indicate an underdetermined system, but when solving with\n\ + Cholesky, accumulation of numerical errors can cause the system to\n\ + become indefinite (have some negative eigenvalues) even if it is in\n\ + reality positive semi-definite (has some near-zero positive\n\ + eigenvalues). Alternatively, if the system contains some\n\ + HessianFactor's with negative eigenvalues, these can create a truly\n\ + indefinite system, whose quadratic error function has negative\n\ + curvature in some directions.\n\ +- Poorly-conditioned system: A system that is positive-definite (has a\n\ + unique solution) but is numerically ill-conditioned can accumulate\n\ + numerical errors during solving that cause the system to become\n\ + indefinite later during the elimination process. Note that poorly-\n\ + conditioned systems will usually have inaccurate solutions, even if\n\ + they do not always trigger this error. Systems with almost-\n\ + unconstrained variables or vastly different measurement uncertainties\n\ + or variable units can be poorly-conditioned.\n\ +\n\ +Resolving this problem:\n\ +- This exception contains the variable at which the problem was\n\ + discovered (IndeterminantLinearSystemException::nearbyVariable()).\n\ + Note, however, that this is not necessarily the variable where the\n\ + problem originates. For example, in the case that a prior on the\n\ + first camera was forgotten, it may only be another camera or landmark\n\ + where the problem can be detected. Where the problem is detected\n\ + depends on the graph structure and variable elimination ordering.\n\ +- MATLAB (or similar software) is a useful tool to diagnose this problem.\n\ + Use GaussianFactorGraph::sparseJacobian_(),\n\ + GaussianFactorGraph::sparseJacobian()\n\ + GaussianFactorGraph::denseJacobian(), and\n\ + GaussianFactorGraph::denseHessian() to output the linear graph in\n\ + matrix format. If using the MATLAB wrapper, the returned matrices can\n\ + be immediately inspected and analyzed using standard methods. If not\n\ + using the MATLAB wrapper, the output of these functions may be written\n\ + to text files and then loaded into MATLAB.\n\ +- When outputting linear graphs as Jacobian matrices, rows are ordered in\n\ + the same order as factors in the graph, which each factor occupying the\n\ + number of rows indicated by its JacobianFactor::rows() function. Each\n\ + column appears in elimination ordering, as in the Ordering class. Each\n\ + variable occupies the number of columns indicated by the\n\ + JacobianFactor::getDim() function.\n\ +- When outputting linear graphs as Hessian matrices, rows and columns are\n\ + ordered in elimination order and occupy scalars in the same way as\n\ + described for Jacobian columns in the previous bullet.\ +"; + return what_.c_str(); + } + }; + +} diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 52e5bccef..deb2fc9d1 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -140,7 +140,7 @@ struct ISAM2Params { * Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when * uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is * slower but more numerically stable in poorly-conditioned problems. We suggest using the default of Cholesky - * unless gtsam sometimes throws NegativeMatrixException when your problem's Hessian is actually positive + * unless gtsam sometimes throws IndefiniteLinearSystemException when your problem's Hessian is actually positive * definite. For positive definite problems, numerical error accumulation can cause the problem to become * numerically negative or indefinite as solving proceeds, especially when using Cholesky. */ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 256763c59..a77cac35a 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -18,10 +18,10 @@ #include +#include #include #include -#include // For NegativeMatrixException #include #include @@ -134,7 +134,7 @@ void LevenbergMarquardtOptimizer::iterate() { state_.lambda *= params_.lambdaFactor; } } - } catch(const NegativeMatrixException& e) { + } catch(const IndeterminantLinearSystemException& e) { if(lmVerbosity >= LevenbergMarquardtParams::LAMBDA) cout << "Negative matrix, increasing lambda" << endl; // Either we're not cautious, or the same lambda was worse than the current error.