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 
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
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::SparseMatrix< Scalar, RowMajor >
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
b
Scalar * b
Definition: benchVecAdd.cpp:17
Eigen::IterationController
Controls the iterations of the iterative solvers.
Definition: IterationController.h:71
Eigen::internal::pseudo_inverse
void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
Definition: ConstrainedConjGrad.h:47
x
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
Definition: gnuplot_common_settings.hh:12
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
Eigen::internal::constrained_cg
void constrained_cg(const TMatrix &A, const CMatrix &C, VectorX &x, const VectorB &b, const VectorF &f, IterationController &iter)
Definition: ConstrainedConjGrad.h:106
A
Definition: test_numpy_dtypes.cpp:298
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
l
static const Line3 l(Rot3(), 1, 1)
Eigen::PlainObjectBase::rows
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:143
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
Eigen::Triplet< double >
gamma
#define gamma
Definition: mconf.h:85
anyset::size
size_t size() const
Definition: pytypes.h:2220
lambda
static double lambda[]
Definition: jv.c:524
E
DiscreteKey E(5, 2)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
Eigen::PlainObjectBase::cols
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:145
iter
iterator iter(handle obj)
Definition: pytypes.h:2475
p
float * p
Definition: Tutorial_Map_using.cpp:9
min
#define min(a, b)
Definition: datatypes.h:19
VectorX
Matrix< Scalar, Dynamic, 1 > VectorX
Definition: sparse_lu.cpp:41
Eigen::Matrix< Scalar, Dynamic, 1 >
internal
Definition: BandTriangularSolver.h:13
cols
int cols
Definition: Tutorial_commainit_02.cpp:1
max
#define max(a, b)
Definition: datatypes.h:20
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:11