ConstrainedConjGrad.h
Go to the documentation of this file.
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 
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  CINV.startFill(); // FIXME estimate the number of non-zeros
62  for (Index i = 0; i < rows; ++i)
63  {
64  d[i] = 1.0;
65  rho = 1.0;
66  e.setZero();
67  r = d;
68  p = d;
69 
70  while (rho >= 1e-38)
71  { /* conjugate gradient to compute e */
72  /* which is the i-th row of inv(C * trans(C)) */
73  l = C.transpose() * p;
74  q = C * l;
75  alpha = rho / p.dot(q);
76  e += alpha * p;
77  r += -alpha * q;
78  rho_1 = rho;
79  rho = r.dot(r);
80  p = (rho/rho_1) * p + r;
81  }
82 
83  l = C.transpose() * e; // l is the i-th row of CINV
84  // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
85  for (Index j=0; j<l.size(); ++j)
86  if (l[j]<1e-15)
87  CINV.fill(i,j) = l[j];
88 
89  d[i] = 0.0;
90  }
91  CINV.endFill();
92 }
93 
94 
95 
101 template<typename TMatrix, typename CMatrix,
102  typename VectorX, typename VectorB, typename VectorF>
103 void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x,
104  const VectorB& b, const VectorF& f, IterationController &iter)
105 {
106  typedef typename TMatrix::Scalar Scalar;
107  typedef typename TMatrix::Index Index;
108  typedef Matrix<Scalar,Dynamic,1> TmpVec;
109 
110  Scalar rho = 1.0, rho_1, lambda, gamma;
111  Index xSize = x.size();
112  TmpVec p(xSize), q(xSize), q2(xSize),
113  r(xSize), old_z(xSize), z(xSize),
114  memox(xSize);
115  std::vector<bool> satured(C.rows());
116  p.setZero();
117  iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b)
118  if (iter.rhsNorm() == 0.0) iter.setRhsNorm(1.0);
119 
120  SparseMatrix<Scalar,RowMajor> CINV(C.rows(), C.cols());
121  pseudo_inverse(C, CINV);
122 
123  while(true)
124  {
125  // computation of residual
126  old_z = z;
127  memox = x;
128  r = b;
129  r += A * -x;
130  z = r;
131  bool transition = false;
132  for (Index i = 0; i < C.rows(); ++i)
133  {
134  Scalar al = C.row(i).dot(x) - f.coeff(i);
135  if (al >= -1.0E-15)
136  {
137  if (!satured[i])
138  {
139  satured[i] = true;
140  transition = true;
141  }
142  Scalar bb = CINV.row(i).dot(z);
143  if (bb > 0.0)
144  // FIXME: we should allow that: z += -bb * C.row(i);
145  for (typename CMatrix::InnerIterator it(C,i); it; ++it)
146  z.coeffRef(it.index()) -= bb*it.value();
147  }
148  else
149  satured[i] = false;
150  }
151 
152  // descent direction
153  rho_1 = rho;
154  rho = r.dot(z);
155 
156  if (iter.finished(rho)) break;
157 
158  if (iter.noiseLevel() > 0 && transition) std::cerr << "CCG: transition\n";
159  if (transition || iter.first()) gamma = 0.0;
160  else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
161  p = z + gamma*p;
162 
163  ++iter;
164  // one dimensionnal optimization
165  q = A * p;
166  lambda = rho / q.dot(p);
167  for (Index i = 0; i < C.rows(); ++i)
168  {
169  if (!satured[i])
170  {
171  Scalar bb = C.row(i).dot(p) - f[i];
172  if (bb > 0.0)
173  lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
174  }
175  }
176  x += lambda * p;
177  memox -= x;
178  }
179 }
180 
181 } // end namespace internal
182 
183 } // end namespace Eigen
184 
185 #endif // EIGEN_CONSTRAINEDCG_H
void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
IntermediateState sqrt(const Expression &arg)
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
EIGEN_STRONG_INLINE Index rows() const
#define E
void constrained_cg(const TMatrix &A, const CMatrix &C, VectorX &x, const VectorB &b, const VectorF &f, IterationController &iter)
Controls the iterations of the iterative solvers.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:31