1 files changed, 16 insertions, 8 deletions
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)
   {