From e16e52d493faffd65ca023b6670772cf1b51ebfd Mon Sep 17 00:00:00 2001
From: Gael Guennebaud <g.gael@free.fr>
Date: Fri, 1 Nov 2013 18:21:46 +0100
Subject: [PATCH] Add a rank method with threshold control to JacobiSVD, and
 make solve uses it to return the minimal norm solution for rank-deficient
 problems (grafted from bbd49d194adc34e1a0e25cdfb5d488f2e0551ce0 )

---
 Eigen/src/SVD/JacobiSVD.h | 71 +++++++++++++++++++++++++++++++++++++--
 1 file changed, 69 insertions(+), 2 deletions(-)

diff --git a/Eigen/src/SVD/JacobiSVD.h b/Eigen/src/SVD/JacobiSVD.h
index f44995cd3..160c3feb6 100644
--- a/Eigen/src/SVD/JacobiSVD.h
+++ b/Eigen/src/SVD/JacobiSVD.h
@@ -531,6 +531,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)
     {}
@@ -545,6 +546,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)
     {
@@ -564,6 +566,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)
     {
@@ -665,6 +668,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; }
@@ -677,11 +743,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;
@@ -854,7 +921,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;