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