src/IterativeSolvers/ConstrainedConjGrad.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
0005
0006 /* NOTE The functions of this file have been adapted from the GMM++ library */
0007
0008 //========================================================================
0009 //
0010 // Copyright (C) 2002-2007 Yves Renard
0011 //
0012 // This file is a part of GETFEM++
0013 //
0014 // Getfem++ is free software; you can redistribute it and/or modify
0015 // it under the terms of the GNU Lesser General Public License as
0016 // published by the Free Software Foundation; version 2.1 of the License.
0017 //
0018 // This program is distributed in the hope that it will be useful,
0019 // but WITHOUT ANY WARRANTY; without even the implied warranty of
0020 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0021 // GNU Lesser General Public License for more details.
0022 // You should have received a copy of the GNU Lesser General Public
0023 // License along with this program; if not, write to the Free Software
0024 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301,
0025 // USA.
0026 //
0027 //========================================================================
0028
0029 #include "../../../../Eigen/src/Core/util/NonMPL2.h"
0030
0031 #ifndef EIGEN_CONSTRAINEDCG_H
0032 #define EIGEN_CONSTRAINEDCG_H
0033
0034 #include "../../../../Eigen/Core"
0035
0036 namespace Eigen {
0037
0038 namespace internal {
0039
0040 /** \ingroup IterativeLinearSolvers_Module
0041   * Compute the pseudo inverse of the non-square matrix C such that
0042   * \f$ CINV = (C * C^T)^{-1} * C \f$ based on a conjugate gradient method.
0043   *
0044   * This function is internally used by constrained_cg.
0045   */
0046 template <typename CMatrix, typename CINVMatrix>
0047 void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
0048 {
0049   // optimisable : copie de la ligne, precalcul de C * trans(C).
0050   typedef typename CMatrix::Scalar Scalar;
0051   typedef typename CMatrix::Index Index;
0052   // FIXME use sparse vectors ?
0053   typedef Matrix<Scalar,Dynamic,1> TmpVec;
0054
0055   Index rows = C.rows(), cols = C.cols();
0056
0057   TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
0058   Scalar rho, rho_1, alpha;
0059   d.setZero();
0060
0061   typedef Triplet<double> T;
0062   std::vector<T> tripletList;
0063
0064   for (Index i = 0; i < rows; ++i)
0065   {
0066     d[i] = 1.0;
0067     rho = 1.0;
0068     e.setZero();
0069     r = d;
0070     p = d;
0071
0072     while (rho >= 1e-38)
0073     { /* conjugate gradient to compute e             */
0074       /* which is the i-th row of inv(C * trans(C))  */
0075       l = C.transpose() * p;
0076       q = C * l;
0077       alpha = rho / p.dot(q);
0078       e +=  alpha * p;
0079       r += -alpha * q;
0080       rho_1 = rho;
0081       rho = r.dot(r);
0082       p = (rho/rho_1) * p + r;
0083     }
0084
0085     l = C.transpose() * e; // l is the i-th row of CINV
0086     // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
0087     for (Index j=0; j<l.size(); ++j)
0088       if (l[j]<1e-15)
0089     tripletList.push_back(T(i,j,l(j)));
0090
0091
0092     d[i] = 0.0;
0093   }
0094   CINV.setFromTriplets(tripletList.begin(), tripletList.end());
0095 }
0096
0097
0098
0099 /** \ingroup IterativeLinearSolvers_Module
0100   * Constrained conjugate gradient
0101   *
0102   * Computes the minimum of \f$ 1/2((Ax).x) - bx \f$ under the constraint \f$ Cx \le f \f$
0103   */
0104 template<typename TMatrix, typename CMatrix,
0105          typename VectorX, typename VectorB, typename VectorF>
0106 void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x,
0107                        const VectorB& b, const VectorF& f, IterationController &iter)
0108 {
0109   using std::sqrt;
0110   typedef typename TMatrix::Scalar Scalar;
0111   typedef typename TMatrix::Index Index;
0112   typedef Matrix<Scalar,Dynamic,1>  TmpVec;
0113
0114   Scalar rho = 1.0, rho_1, lambda, gamma;
0115   Index xSize = x.size();
0116   TmpVec  p(xSize), q(xSize), q2(xSize),
0117           r(xSize), old_z(xSize), z(xSize),
0118           memox(xSize);
0119   std::vector<bool> satured(C.rows());
0120   p.setZero();
0121   iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b)
0122   if (iter.rhsNorm() == 0.0) iter.setRhsNorm(1.0);
0123
0124   SparseMatrix<Scalar,RowMajor> CINV(C.rows(), C.cols());
0125   pseudo_inverse(C, CINV);
0126
0127   while(true)
0128   {
0129     // computation of residual
0130     old_z = z;
0131     memox = x;
0132     r = b;
0133     r += A * -x;
0134     z = r;
0135     bool transition = false;
0136     for (Index i = 0; i < C.rows(); ++i)
0137     {
0138       Scalar al = C.row(i).dot(x) - f.coeff(i);
0139       if (al >= -1.0E-15)
0140       {
0141         if (!satured[i])
0142         {
0143           satured[i] = true;
0144           transition = true;
0145         }
0146         Scalar bb = CINV.row(i).dot(z);
0147         if (bb > 0.0)
0148           // FIXME: we should allow that: z += -bb * C.row(i);
0149           for (typename CMatrix::InnerIterator it(C,i); it; ++it)
0150             z.coeffRef(it.index()) -= bb*it.value();
0151       }
0152       else
0153         satured[i] = false;
0154     }
0155
0156     // descent direction
0157     rho_1 = rho;
0158     rho = r.dot(z);
0159
0160     if (iter.finished(rho)) break;
0161     if (transition || iter.first()) gamma = 0.0;
0162     else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
0163     p = z + gamma*p;
0164
0165     ++iter;
0166     // one dimensionnal optimization
0167     q = A * p;
0168     lambda = rho / q.dot(p);
0169     for (Index i = 0; i < C.rows(); ++i)
0170     {
0171       if (!satured[i])
0172       {
0173         Scalar bb = C.row(i).dot(p) - f[i];
0174         if (bb > 0.0)
0175           lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
0176       }
0177     }
0178     x += lambda * p;
0179     memox -= x;
0180   }
0181 }
0182
0183 } // end namespace internal
0184
0185 } // end namespace Eigen
0186
0187 #endif // EIGEN_CONSTRAINEDCG_H