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  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 
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  if (transition || iter.first()) gamma = 0.0;
162  else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
163  p = z + gamma*p;
164 
165  ++iter;
166  // one dimensionnal optimization
167  q = A * p;
168  lambda = rho / q.dot(p);
169  for (Index i = 0; i < C.rows(); ++i)
170  {
171  if (!satured[i])
172  {
173  Scalar bb = C.row(i).dot(p) - f[i];
174  if (bb > 0.0)
175  lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
176  }
177  }
178  x += lambda * p;
179  memox -= x;
180  }
181 }
182 
183 } // end namespace internal
184 
185 } // end namespace Eigen
186 
187 #endif // EIGEN_CONSTRAINEDCG_H
SCALAR Scalar
Definition: bench_gemm.cpp:46
void constrained_cg(const TMatrix &A, const CMatrix &C, VectorX &x, const VectorB &b, const VectorF &f, IterationController &iter)
#define max(a, b)
Definition: datatypes.h:20
Scalar * b
Definition: benchVecAdd.cpp:17
#define min(a, b)
Definition: datatypes.h:19
Matrix< Scalar, Dynamic, 1 > VectorX
Definition: sparse_lu.cpp:41
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
iterator iter(handle obj)
Definition: pytypes.h:2273
static const Line3 l(Rot3(), 1, 1)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
Eigen::Triplet< double > T
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EIGEN_DEVICE_FUNC const Scalar & q
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
DiscreteKey E(5, 2)
float * p
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
Controls the iterations of the iterative solvers.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:03