diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index cad43381b..26cd3fb5e 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -168,11 +168,10 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView()); if(ldlt.vectorD().unaryExpr(boost::bind(less(), _1, 0.0)).any()) { - if(debug) { - gtsam::print(Matrix(ldlt.matrixU()), "U: "); - gtsam::print(Vector(ldlt.vectorD()), "D: "); - } - throw NegativeMatrixException(); + if(ISDEBUG("detailed_exceptions")) + throw NegativeMatrixException(NegativeMatrixException::Detail(ABC, ldlt.matrixU(), ldlt.vectorD())); + else + throw NegativeMatrixException(); } Vector sqrtD = ldlt.vectorD().cwiseSqrt(); diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 46463dbc9..be108b081 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -18,13 +18,29 @@ #pragma once #include +#include namespace gtsam { /** * An exception indicating an attempt to factor a negative or indefinite matrix. + * If detailed exceptions are enabled + * \todo fill this in */ -class NegativeMatrixException : public std::exception { }; +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(_A), 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() {} +}; /** * "Careful" Cholesky computes the positive square-root of a positive symmetric