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@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