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 (at) 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> 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> 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
