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 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10 /* NOTE The functions of this file have been adapted from the GMM++ library */
11
12 //========================================================================
13 //
14 // Copyright (C) 2002-2007 Yves Renard
15 //
16 // This file is a part of GETFEM++
17 //
18 // Getfem++ is free software; you can redistribute it and/or modify
19 // it under the terms of the GNU Lesser General Public License as
20 // published by the Free Software Foundation; version 2.1 of the License.
21 //
22 // This program is distributed in the hope that it will be useful,
23 // but WITHOUT ANY WARRANTY; without even the implied warranty of
24 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25 // GNU Lesser General Public License for more details.
26 // You should have received a copy of the GNU Lesser General Public
27 // License along with this program; if not, write to the Free Software
28 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301,
29 // USA.
30 //
31 //========================================================================
32
33 #include "../../../../Eigen/src/Core/util/NonMPL2.h"
34
35 #ifndef EIGEN_CONSTRAINEDCG_H
36 #define EIGEN_CONSTRAINEDCG_H
37
38 #include <Eigen/Core>
39
40 namespace Eigen {
41
42 namespace internal {
43
44 /** \ingroup IterativeSolvers_Module
45 * Compute the pseudo inverse of the non-square matrix C such that
46 * \f$ CINV = (C * C^T)^{-1} * C \f$ based on a conjugate gradient method.
47 *
48 * This function is internally used by constrained_cg.
49 */
50 template <typename CMatrix, typename CINVMatrix>
pseudo_inverse(const CMatrix & C,CINVMatrix & CINV)51 void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
52 {
53 // optimisable : copie de la ligne, precalcul de C * trans(C).
54 typedef typename CMatrix::Scalar Scalar;
55 typedef typename CMatrix::Index Index;
56 // FIXME use sparse vectors ?
57 typedef Matrix<Scalar,Dynamic,1> TmpVec;
58
59 Index rows = C.rows(), cols = C.cols();
60
61 TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
62 Scalar rho, rho_1, alpha;
63 d.setZero();
64
65 CINV.startFill(); // FIXME estimate the number of non-zeros
66 for (Index i = 0; i < rows; ++i)
67 {
68 d[i] = 1.0;
69 rho = 1.0;
70 e.setZero();
71 r = d;
72 p = d;
73
74 while (rho >= 1e-38)
75 { /* conjugate gradient to compute e */
76 /* which is the i-th row of inv(C * trans(C)) */
77 l = C.transpose() * p;
78 q = C * l;
79 alpha = rho / p.dot(q);
80 e += alpha * p;
81 r += -alpha * q;
82 rho_1 = rho;
83 rho = r.dot(r);
84 p = (rho/rho_1) * p + r;
85 }
86
87 l = C.transpose() * e; // l is the i-th row of CINV
88 // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
89 for (Index j=0; j<l.size(); ++j)
90 if (l[j]<1e-15)
91 CINV.fill(i,j) = l[j];
92
93 d[i] = 0.0;
94 }
95 CINV.endFill();
96 }
97
98
99
100 /** \ingroup IterativeSolvers_Module
101 * Constrained conjugate gradient
102 *
103 * Computes the minimum of \f$ 1/2((Ax).x) - bx \f$ under the contraint \f$ Cx \le f \f$
104 */
105 template<typename TMatrix, typename CMatrix,
106 typename VectorX, typename VectorB, typename VectorF>
constrained_cg(const TMatrix & A,const CMatrix & C,VectorX & x,const VectorB & b,const VectorF & f,IterationController & iter)107 void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x,
108 const VectorB& b, const VectorF& f, IterationController &iter)
109 {
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