Bug567 : Fix iterative solvers to immediately return when the initial guess is the true solution and for trivial solution

2 files changed, 21 insertions, 8 deletions

diff --git a/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
index 5a822e0ea..fbefb696f 100644
--- a/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
+++ b/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
@@ -44,6 +44,11 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
   VectorType r0 = r;
   
   RealScalar r0_sqnorm = rhs.squaredNorm();
+  if(r0_sqnorm == 0)
+  {
+    x.setZero();
+    return true;
+  }
   Scalar rho    = 1;
   Scalar alpha  = 1;
   Scalar w      = 1;
diff --git a/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
index eadf711e5..00b5647c6 100644
--- a/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
+++ b/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
@@ -41,21 +41,29 @@ void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
   int n = mat.cols();
 
   VectorType residual = rhs - mat * x; //initial residual
-  VectorType p(n);
-
-  p = precond.solve(residual);      //initial search direction
 
-  VectorType z(n), tmp(n);
-  RealScalar absNew = internal::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
   RealScalar rhsNorm2 = rhs.squaredNorm();
-   // Check Zero right hand side
-  if(!rhsNorm2) 
+  if(rhsNorm2 == 0) 
   {
     x.setZero();
+    iters = 0;
+    tol_error = 0;
     return;
   }
-  RealScalar residualNorm2 = 0;
   RealScalar threshold = tol*tol*rhsNorm2;
+  RealScalar residualNorm2 = residual.squaredNorm();
+  if (residualNorm2 < threshold)
+  {
+    iters = 0;
+    tol_error = sqrt(residualNorm2 / rhsNorm2);
+    return;
+  }
+  
+  VectorType p(n);
+  p = precond.solve(residual);      //initial search direction
+
+  VectorType z(n), tmp(n);
+  RealScalar absNew = internal::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
   int i = 0;
   while(i < maxIters)
   {