Home | History | Annotate | Download | only in IterativeSolvers 
      1 // This file is part of Eigen, a lightweight C++ template library
      2 // for linear algebra.
      3 //
      4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud (at) inria.fr>
      5 
      6 /* NOTE The functions of this file have been adapted from the GMM++ library */
      7 
      8 //========================================================================
      9 //
     10 // Copyright (C) 2002-2007 Yves Renard
     11 //
     12 // This file is a part of GETFEM++
     13 //
     14 // Getfem++ is free software; you can redistribute it and/or modify
     15 // it under the terms of the GNU Lesser General Public License as
     16 // published by the Free Software Foundation; version 2.1 of the License.
     17 //
     18 // This program is distributed in the hope that it will be useful,
     19 // but WITHOUT ANY WARRANTY; without even the implied warranty of
     20 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     21 // GNU Lesser General Public License for more details.
     22 // You should have received a copy of the GNU Lesser General Public
     23 // License along with this program; if not, write to the Free Software
     24 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301,
     25 // USA.
     26 //
     27 //========================================================================
     28 
     29 #include "../../../../Eigen/src/Core/util/NonMPL2.h"
     30 
     31 #ifndef EIGEN_CONSTRAINEDCG_H
     32 #define EIGEN_CONSTRAINEDCG_H
     33 
     34 #include <Eigen/Core>
     35 
     36 namespace Eigen {
     37 
     38 namespace internal {
     39 
     40 /** \ingroup IterativeSolvers_Module
     41   * Compute the pseudo inverse of the non-square matrix C such that
     42   * \f$ CINV = (C * C^T)^{-1} * C \f$ based on a conjugate gradient method.
     43   *
     44   * This function is internally used by constrained_cg.
     45   */
     46 template <typename CMatrix, typename CINVMatrix>
     47 void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
     48 {
     49   // optimisable : copie de la ligne, precalcul de C * trans(C).
     50   typedef typename CMatrix::Scalar Scalar;
     51   typedef typename CMatrix::Index Index;
     52   // FIXME use sparse vectors ?
     53   typedef Matrix<Scalar,Dynamic,1> TmpVec;
     54 
     55   Index rows = C.rows(), cols = C.cols();
     56 
     57   TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
     58   Scalar rho, rho_1, alpha;
     59   d.setZero();
     60 
     61   typedef Triplet<double> T;
     62   std::vector<T> tripletList;
     63 
     64   for (Index i = 0; i < rows; ++i)
     65   {
     66     d[i] = 1.0;
     67     rho = 1.0;
     68     e.setZero();
     69     r = d;
     70     p = d;
     71 
     72     while (rho >= 1e-38)
     73     { /* conjugate gradient to compute e             */
     74       /* which is the i-th row of inv(C * trans(C))  */
     75       l = C.transpose() * p;
     76       q = C * l;
     77       alpha = rho / p.dot(q);
     78       e +=  alpha * p;
     79       r += -alpha * q;
     80       rho_1 = rho;
     81       rho = r.dot(r);
     82       p = (rho/rho_1) * p + r;
     83     }
     84 
     85     l = C.transpose() * e; // l is the i-th row of CINV
     86     // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
     87     for (Index j=0; j<l.size(); ++j)
     88       if (l[j]<1e-15)
     89 	tripletList.push_back(T(i,j,l(j)));
     90 
     91 
     92     d[i] = 0.0;
     93   }
     94   CINV.setFromTriplets(tripletList.begin(), tripletList.end());
     95 }
     96 
     97 
     98 
     99 /** \ingroup IterativeSolvers_Module
    100   * Constrained conjugate gradient
    101   *
    102   * Computes the minimum of \f$ 1/2((Ax).x) - bx \f$ under the contraint \f$ Cx \le f \f$
    103   */
    104 template<typename TMatrix, typename CMatrix,
    105          typename VectorX, typename VectorB, typename VectorF>
    106 void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x,
    107                        const VectorB& b, const VectorF& f, IterationController &iter)
    108 {
    109   using std::sqrt;
    110   typedef typename TMatrix::Scalar Scalar;
    111   typedef typename TMatrix::Index Index;
    112   typedef Matrix<Scalar,Dynamic,1>  TmpVec;
    113 
    114   Scalar rho = 1.0, rho_1, lambda, gamma;
    115   Index xSize = x.size();
    116   TmpVec  p(xSize), q(xSize), q2(xSize),
    117           r(xSize), old_z(xSize), z(xSize),
    118           memox(xSize);
    119   std::vector<bool> satured(C.rows());
    120   p.setZero();
    121   iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b)
    122   if (iter.rhsNorm() == 0.0) iter.setRhsNorm(1.0);
    123 
    124   SparseMatrix<Scalar,RowMajor> CINV(C.rows(), C.cols());
    125   pseudo_inverse(C, CINV);
    126 
    127   while(true)
    128   {
    129     // computation of residual
    130     old_z = z;
    131     memox = x;
    132     r = b;
    133     r += A * -x;
    134     z = r;
    135     bool transition = false;
    136     for (Index i = 0; i < C.rows(); ++i)
    137     {
    138       Scalar al = C.row(i).dot(x) - f.coeff(i);
    139       if (al >= -1.0E-15)
    140       {
    141         if (!satured[i])
    142         {
    143           satured[i] = true;
    144           transition = true;
    145         }
    146         Scalar bb = CINV.row(i).dot(z);
    147         if (bb > 0.0)
    148           // FIXME: we should allow that: z += -bb * C.row(i);
    149           for (typename CMatrix::InnerIterator it(C,i); it; ++it)
    150             z.coeffRef(it.index()) -= bb*it.value();
    151       }
    152       else
    153         satured[i] = false;
    154     }
    155 
    156     // descent direction
    157     rho_1 = rho;
    158     rho = r.dot(z);
    159 
    160     if (iter.finished(rho)) break;
    161 
    162     if (iter.noiseLevel() > 0 && transition) std::cerr << "CCG: transition\n";
    163     if (transition || iter.first()) gamma = 0.0;
    164     else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
    165     p = z + gamma*p;
    166 
    167     ++iter;
    168     // one dimensionnal optimization
    169     q = A * p;
    170     lambda = rho / q.dot(p);
    171     for (Index i = 0; i < C.rows(); ++i)
    172     {
    173       if (!satured[i])
    174       {
    175         Scalar bb = C.row(i).dot(p) - f[i];
    176         if (bb > 0.0)
    177           lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
    178       }
    179     }
    180     x += lambda * p;
    181     memox -= x;
    182   }
    183 }
    184 
    185 } // end namespace internal
    186 
    187 } // end namespace Eigen
    188 
    189 #endif // EIGEN_CONSTRAINEDCG_H
    190