From 1dbda3f7ed214709b7c45dd357eadd8a4aebb0b5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 22 Aug 2012 22:40:27 +0000 Subject: [PATCH] Standardized and corrected error handling on the linear size - underconstrained and negative systems always throw IndeterminantLinearSystemException, and all assertions checking for infinite values are removed. Also, we were not properly checking the result of Eigen's Cholesky, so sometimes elimination continued with incorrect matrices despite being underconstrained when Cholesky failed but did not produce NaN's. --- gtsam/base/cholesky.cpp | 65 +++++-- gtsam/base/cholesky.h | 50 +---- gtsam/base/tests/testCholesky.cpp | 40 ++++ gtsam/linear/GaussianConditional.cpp | 13 +- gtsam/linear/GaussianFactorGraph.cpp | 70 +------ gtsam/linear/HessianFactor.cpp | 12 +- gtsam/linear/JacobianFactor.cpp | 35 ++-- gtsam/linear/linearExceptions.h | 180 ++++++++++++++++++ gtsam/nonlinear/ISAM2.h | 2 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 4 +- 10 files changed, 309 insertions(+), 162 deletions(-) create mode 100644 gtsam/linear/linearExceptions.h 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.