Add a rank method with threshold control to JacobiSVD, and make solve uses it to return the minimal norm solution for rank-deficient problems

1 files changed, 69 insertions, 2 deletions

diff --git a/Eigen/src/SVD/JacobiSVD.h b/Eigen/src/SVD/JacobiSVD.h
index 4786768ff..fcf404587 100644
--- a/Eigen/src/SVD/JacobiSVD.h
+++ b/Eigen/src/SVD/JacobiSVD.h
@@ -528,6 +528,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD()
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {}
@@ -542,6 +543,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD(Index rows, Index cols, unsigned int computationOptions = 0)
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {
@@ -561,6 +563,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0)
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {
@@ -662,6 +665,69 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
       eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
       return m_nonzeroSingularValues;
     }
+    
+    /** \returns the rank of the matrix of which \c *this is the SVD.
+      *
+      * \note This method has to determine which singular values should be considered nonzero.
+      *       For that, it uses the threshold value that you can control by calling
+      *       setThreshold(const RealScalar&).
+      */
+    inline Index rank() const
+    {
+      using std::abs;
+      eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
+      if(m_singularValues.size()==0) return 0;
+      RealScalar premultiplied_threshold = m_singularValues.coeff(0) * threshold();
+      Index i = m_nonzeroSingularValues-1;
+      while(i>=0 && m_singularValues.coeff(i) < premultiplied_threshold) --i;
+      return i+1;
+    }
+    
+    /** Allows to prescribe a threshold to be used by certain methods, such as rank() and solve(),
+      * which need to determine when singular values are to be considered nonzero.
+      * This is not used for the SVD decomposition itself.
+      *
+      * When it needs to get the threshold value, Eigen calls threshold().
+      * The default is \c NumTraits<Scalar>::epsilon()
+      *
+      * \param threshold The new value to use as the threshold.
+      *
+      * A singular value will be considered nonzero if its value is strictly greater than
+      *  \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
+      *
+      * If you want to come back to the default behavior, call setThreshold(Default_t)
+      */
+    JacobiSVD& setThreshold(const RealScalar& threshold)
+    {
+      m_usePrescribedThreshold = true;
+      m_prescribedThreshold = threshold;
+      return *this;
+    }
+
+    /** Allows to come back to the default behavior, letting Eigen use its default formula for
+      * determining the threshold.
+      *
+      * You should pass the special object Eigen::Default as parameter here.
+      * \code svd.setThreshold(Eigen::Default); \endcode
+      *
+      * See the documentation of setThreshold(const RealScalar&).
+      */
+    JacobiSVD& setThreshold(Default_t)
+    {
+      m_usePrescribedThreshold = false;
+      return *this;
+    }
+
+    /** Returns the threshold that will be used by certain methods such as rank().
+      *
+      * See the documentation of setThreshold(const RealScalar&).
+      */
+    RealScalar threshold() const
+    {
+      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+      return m_usePrescribedThreshold ? m_prescribedThreshold
+                                      : NumTraits<Scalar>::epsilon();
+    }
 
     inline Index rows() const { return m_rows; }
     inline Index cols() const { return m_cols; }
@@ -674,11 +740,12 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     MatrixVType m_matrixV;
     SingularValuesType m_singularValues;
     WorkMatrixType m_workMatrix;
-    bool m_isInitialized, m_isAllocated;
+    bool m_isInitialized, m_isAllocated, m_usePrescribedThreshold;
     bool m_computeFullU, m_computeThinU;
     bool m_computeFullV, m_computeThinV;
     unsigned int m_computationOptions;
     Index m_nonzeroSingularValues, m_rows, m_cols, m_diagSize;
+    RealScalar m_prescribedThreshold;
 
     template<typename __MatrixType, int _QRPreconditioner, bool _IsComplex>
     friend struct internal::svd_precondition_2x2_block_to_be_real;
@@ -851,7 +918,7 @@ struct solve_retval<JacobiSVD<_MatrixType, QRPreconditioner>, Rhs>
     // So A^{-1} = V S^{-1} U^*
 
     Matrix<Scalar, Dynamic, Rhs::ColsAtCompileTime, 0, _MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime> tmp;
-    Index nonzeroSingVals = dec().nonzeroSingularValues();
+    Index nonzeroSingVals = dec().rank();
     
     tmp.noalias() = dec().matrixU().leftCols(nonzeroSingVals).adjoint() * rhs();
     tmp = dec().singularValues().head(nonzeroSingVals).asDiagonal().inverse() * tmp;