Add the ability to specify the pivot threshold in Covariance::Options

https: //github.com/ceres-solver/ceres-solver/issues/777
Change-Id: I481612b7bc727d5cd0dc21a0e0dbaf356722ba22

docs/source/nnls_covariance.rst

diff --git a/docs/source/nnls_covariance.rst b/docs/source/nnls_covariance.rst
index 66afd44..c85bedd 100644
--- a/docs/source/nnls_covariance.rst
+++ b/docs/source/nnls_covariance.rst
@@ -187,6 +187,23 @@
       well as rank deficient Jacobians.
 
 
+.. member:: double Covariance::Options::column_pivot_threshold
+
+   Default: :math:`-1`
+
+    During QR factorization, if a column with Euclidean norm less than
+    ``column_pivot_threshold`` is encountered it is treated as zero.
+
+    If ``column_pivot_threshold < 0``, then an automatic default value
+    of `20*(m+n)*eps*sqrt(max(diag(J’*J)))` is used.  Here `m` and `n`
+    are the number of rows and columns of the Jacobian (`J`)
+    respectively.
+
+    This is an advanced option meant for users who know enough about
+    their Jacobian matrices that they can determine a value better
+    than the default.
+
+
 .. member:: int Covariance::Options::min_reciprocal_condition_number
 
    Default: :math:`10^{-14}`

include/ceres/covariance.h

diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h
index 60bcc80..00d00bd 100644
--- a/include/ceres/covariance.h
+++ b/include/ceres/covariance.h
@@ -246,6 +246,20 @@
     // used.
     CovarianceAlgorithmType algorithm_type = SPARSE_QR;
 
+    // During QR factorization, if a column with Euclidean norm less
+    // than column_pivot_threshold is encountered it is treated as
+    // zero.
+    //
+    // If column_pivot_threshold < 0, then an automatic default value
+    // of 20*(m+n)*eps*sqrt(max(diag(J’*J))) is used. Here m and n are
+    // the number of rows and columns of the Jacobian (J)
+    // respectively.
+    //
+    // This is an advanced option meant for users who know enough
+    // about their Jacobian matrices that they can determine a value
+    // better than the default.
+    double column_pivot_threshold = -1;
+
     // If the Jacobian matrix is near singular, then inverting J'J
     // will result in unreliable results, e.g, if
     //

internal/ceres/covariance_impl.cc

diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index 324b553..ef0aac0 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -628,13 +628,15 @@
   // more efficient, both in runtime as well as the quality of
   // ordering computed. So, it maybe worth doing that analysis
   // separately.
-  const SuiteSparse_long rank = SuiteSparseQR<double>(SPQR_ORDERING_BESTAMD,
-                                                      SPQR_DEFAULT_TOL,
-                                                      cholmod_jacobian.ncol,
-                                                      &cholmod_jacobian,
-                                                      &R,
-                                                      &permutation,
-                                                      &cc);
+  const SuiteSparse_long rank = SuiteSparseQR<double>(
+      SPQR_ORDERING_BESTAMD,
+      options_.column_pivot_threshold < 0 ? SPQR_DEFAULT_TOL
+                                          : options_.column_pivot_threshold,
+      cholmod_jacobian.ncol,
+      &cholmod_jacobian,
+      &R,
+      &permutation,
+      &cc);
   event_logger.AddEvent("Numeric Factorization");
   if (R == nullptr) {
     LOG(ERROR) << "Something is wrong. SuiteSparseQR returned R = nullptr.";
@@ -830,19 +832,23 @@
           jacobian.values.data());
   event_logger.AddEvent("ConvertToSparseMatrix");
 
-  Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int>> qr_solver(
-      sparse_jacobian);
+  Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int>> qr;
+  if (options_.column_pivot_threshold > 0) {
+    qr.setPivotThreshold(options_.column_pivot_threshold);
+  }
+
+  qr.compute(sparse_jacobian);
   event_logger.AddEvent("QRDecomposition");
 
-  if (qr_solver.info() != Eigen::Success) {
+  if (qr.info() != Eigen::Success) {
     LOG(ERROR) << "Eigen::SparseQR decomposition failed.";
     return false;
   }
 
-  if (qr_solver.rank() < jacobian.num_cols) {
+  if (qr.rank() < jacobian.num_cols) {
     LOG(ERROR) << "Jacobian matrix is rank deficient. "
                << "Number of columns: " << jacobian.num_cols
-               << " rank: " << qr_solver.rank();
+               << " rank: " << qr.rank();
     return false;
   }
 
@@ -852,7 +858,7 @@
 
   // Compute the inverse column permutation used by QR factorization.
   Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation =
-      qr_solver.colsPermutation().inverse();
+      qr.colsPermutation().inverse();
 
   // The following loop exploits the fact that the i^th column of A^{-1}
   // is given by the solution to the linear system
@@ -875,9 +881,9 @@
         if (row_end != row_begin) {
           double* solution = workspace.get() + thread_id * num_cols;
           SolveRTRWithSparseRHS<int>(num_cols,
-                                     qr_solver.matrixR().innerIndexPtr(),
-                                     qr_solver.matrixR().outerIndexPtr(),
-                                     &qr_solver.matrixR().data().value(0),
+                                     qr.matrixR().innerIndexPtr(),
+                                     qr.matrixR().outerIndexPtr(),
+                                     &qr.matrixR().data().value(0),
                                      inverse_permutation.indices().coeff(r),
                                      solution);