diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 0eb23873c..c69993d45 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -45,7 +45,7 @@ static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) { if(alpha < negativePivotThreshold) { cout << "pivot = " << alpha << endl; print(ATA, "Partially-factorized matrix: "); - throw(invalid_argument("The matrix was found to be non-positive-semidefinite when factoring with careful Cholesky.")); + throw(CarefulCholeskyNegativeMatrixException()); } else if(alpha < 0.0) alpha = 0.0; @@ -64,7 +64,9 @@ static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) { V *= betainv; // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha - ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; + ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V; +// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView() +// .rankUpdate(V.adjoint(), -1); } return true; diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 0c4689113..fe0e68156 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -24,8 +24,9 @@ namespace gtsam { /** * An exception indicating an attempt to factor a negative or indefinite matrix. - * If detailed exceptions are enabled - * \todo fill this in + * 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 @@ -48,6 +49,18 @@ struct NegativeMatrixException : public std::exception { 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 diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2b9adbec0..57f068fc6 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -161,7 +161,18 @@ namespace gtsam { JacobianFactor::JacobianFactor(const HessianFactor& factor) : Ab_(matrix_) { keys_ = factor.keys_; Ab_.assignNoalias(factor.info_); - size_t maxrank = choleskyCareful(matrix_).first; + size_t maxrank; + try { + maxrank = choleskyCareful(matrix_).first; + } catch(const CarefulCholeskyNegativeMatrixException& e) { + cout << + "Attempting to convert a HessianFactor to a JacobianFactor, but for this\n" + "HessianFactor it is not possible because either the Hessian is negative or\n" + "indefinite, or the quadratic error function it describes becomes negative for\n" + "some values. Here is the HessianFactor on which this conversion was attempted:\n"; + factor.print(""); + throw; + } // Zero out lower triangle matrix_.topRows(maxrank).triangularView() = Matrix::Zero(maxrank, matrix_.cols());