Better error message when trying to convert a negative HessianFactor to a JacobianFactor

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;
     
@@ -65,6 +65,8 @@ static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) {
 
       // 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.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
+//    	    .rankUpdate(V.adjoint(), -1);
     }
 
     return true;

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
+ * If detailed exceptions are enabled, then the \c detail member will contain
- * \todo fill this in
+ * 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

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<Eigen::StrictlyLower>() =
         Matrix::Zero(maxrank, matrix_.cols());