ConjugateGradient.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_CONJUGATE_GRADIENT_H
00011 #define EIGEN_CONJUGATE_GRADIENT_H
00012 
00013 namespace Eigen { 
00014 
00015 namespace internal {
00016 
00026 template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
00027 EIGEN_DONT_INLINE
00028 void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
00029                         const Preconditioner& precond, int& iters,
00030                         typename Dest::RealScalar& tol_error)
00031 {
00032   using std::sqrt;
00033   using std::abs;
00034   typedef typename Dest::RealScalar RealScalar;
00035   typedef typename Dest::Scalar Scalar;
00036   typedef Matrix<Scalar,Dynamic,1> VectorType;
00037   
00038   RealScalar tol = tol_error;
00039   int maxIters = iters;
00040   
00041   int n = mat.cols();
00042 
00043   VectorType residual = rhs - mat * x; //initial residual
00044 
00045   RealScalar rhsNorm2 = rhs.squaredNorm();
00046   if(rhsNorm2 == 0) 
00047   {
00048     x.setZero();
00049     iters = 0;
00050     tol_error = 0;
00051     return;
00052   }
00053   RealScalar threshold = tol*tol*rhsNorm2;
00054   RealScalar residualNorm2 = residual.squaredNorm();
00055   if (residualNorm2 < threshold)
00056   {
00057     iters = 0;
00058     tol_error = sqrt(residualNorm2 / rhsNorm2);
00059     return;
00060   }
00061   
00062   VectorType p(n);
00063   p = precond.solve(residual);      //initial search direction
00064 
00065   VectorType z(n), tmp(n);
00066   RealScalar absNew = numext::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
00067   int i = 0;
00068   while(i < maxIters)
00069   {
00070     tmp.noalias() = mat * p;              // the bottleneck of the algorithm
00071 
00072     Scalar alpha = absNew / p.dot(tmp);   // the amount we travel on dir
00073     x += alpha * p;                       // update solution
00074     residual -= alpha * tmp;              // update residue
00075     
00076     residualNorm2 = residual.squaredNorm();
00077     if(residualNorm2 < threshold)
00078       break;
00079     
00080     z = precond.solve(residual);          // approximately solve for "A z = residual"
00081 
00082     RealScalar absOld = absNew;
00083     absNew = numext::real(residual.dot(z));     // update the absolute value of r
00084     RealScalar beta = absNew / absOld;            // calculate the Gram-Schmidt value used to create the new search direction
00085     p = z + beta * p;                             // update search direction
00086     i++;
00087   }
00088   tol_error = sqrt(residualNorm2 / rhsNorm2);
00089   iters = i;
00090 }
00091 
00092 }
00093 
00094 template< typename _MatrixType, int _UpLo=Lower,
00095           typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
00096 class ConjugateGradient;
00097 
00098 namespace internal {
00099 
00100 template< typename _MatrixType, int _UpLo, typename _Preconditioner>
00101 struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
00102 {
00103   typedef _MatrixType MatrixType;
00104   typedef _Preconditioner Preconditioner;
00105 };
00106 
00107 }
00108 
00157 template< typename _MatrixType, int _UpLo, typename _Preconditioner>
00158 class ConjugateGradient : public IterativeSolverBase<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
00159 {
00160   typedef IterativeSolverBase<ConjugateGradient> Base;
00161   using Base::mp_matrix;
00162   using Base::m_error;
00163   using Base::m_iterations;
00164   using Base::m_info;
00165   using Base::m_isInitialized;
00166 public:
00167   typedef _MatrixType MatrixType;
00168   typedef typename MatrixType::Scalar Scalar;
00169   typedef typename MatrixType::Index Index;
00170   typedef typename MatrixType::RealScalar RealScalar;
00171   typedef _Preconditioner Preconditioner;
00172 
00173   enum {
00174     UpLo = _UpLo
00175   };
00176 
00177 public:
00178 
00180   ConjugateGradient() : Base() {}
00181 
00192   ConjugateGradient(const MatrixType& A) : Base(A) {}
00193 
00194   ~ConjugateGradient() {}
00195   
00201   template<typename Rhs,typename Guess>
00202   inline const internal::solve_retval_with_guess<ConjugateGradient, Rhs, Guess>
00203   solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const
00204   {
00205     eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
00206     eigen_assert(Base::rows()==b.rows()
00207               && "ConjugateGradient::solve(): invalid number of rows of the right hand side matrix b");
00208     return internal::solve_retval_with_guess
00209             <ConjugateGradient, Rhs, Guess>(*this, b.derived(), x0);
00210   }
00211 
00213   template<typename Rhs,typename Dest>
00214   void _solveWithGuess(const Rhs& b, Dest& x) const
00215   {
00216     m_iterations = Base::maxIterations();
00217     m_error = Base::m_tolerance;
00218 
00219     for(int j=0; j<b.cols(); ++j)
00220     {
00221       m_iterations = Base::maxIterations();
00222       m_error = Base::m_tolerance;
00223 
00224       typename Dest::ColXpr xj(x,j);
00225       internal::conjugate_gradient(mp_matrix->template selfadjointView<UpLo>(), b.col(j), xj,
00226                                    Base::m_preconditioner, m_iterations, m_error);
00227     }
00228 
00229     m_isInitialized = true;
00230     m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
00231   }
00232   
00234   template<typename Rhs,typename Dest>
00235   void _solve(const Rhs& b, Dest& x) const
00236   {
00237     x.setOnes();
00238     _solveWithGuess(b,x);
00239   }
00240 
00241 protected:
00242 
00243 };
00244 
00245 
00246 namespace internal {
00247 
00248 template<typename _MatrixType, int _UpLo, typename _Preconditioner, typename Rhs>
00249 struct solve_retval<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner>, Rhs>
00250   : solve_retval_base<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner>, Rhs>
00251 {
00252   typedef ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> Dec;
00253   EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs)
00254 
00255   template<typename Dest> void evalTo(Dest& dst) const
00256   {
00257     dec()._solve(rhs(),dst);
00258   }
00259 };
00260 
00261 } // end namespace internal
00262 
00263 } // end namespace Eigen
00264 
00265 #endif // EIGEN_CONJUGATE_GRADIENT_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:58:03