ConstrainedConjGrad.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 /* NOTE The functions of this file have been adapted from the GMM++ library */
00026 
00027 //========================================================================
00028 //
00029 // Copyright (C) 2002-2007 Yves Renard
00030 //
00031 // This file is a part of GETFEM++
00032 //
00033 // Getfem++ is free software; you can redistribute it and/or modify
00034 // it under the terms of the GNU Lesser General Public License as
00035 // published by the Free Software Foundation; version 2.1 of the License.
00036 //
00037 // This program is distributed in the hope that it will be useful,
00038 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00039 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00040 // GNU Lesser General Public License for more details.
00041 // You should have received a copy of the GNU Lesser General Public
00042 // License along with this program; if not, write to the Free Software
00043 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301,
00044 // USA.
00045 //
00046 //========================================================================
00047 
00048 #ifndef EIGEN_CONSTRAINEDCG_H
00049 #define EIGEN_CONSTRAINEDCG_H
00050 
00051 #include <Eigen/Core>
00052 
00053 namespace Eigen { 
00054 
00055 namespace internal {
00056 
00063 template <typename CMatrix, typename CINVMatrix>
00064 void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
00065 {
00066   // optimisable : copie de la ligne, precalcul de C * trans(C).
00067   typedef typename CMatrix::Scalar Scalar;
00068   typedef typename CMatrix::Index Index;
00069   // FIXME use sparse vectors ?
00070   typedef Matrix<Scalar,Dynamic,1> TmpVec;
00071 
00072   Index rows = C.rows(), cols = C.cols();
00073 
00074   TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
00075   Scalar rho, rho_1, alpha;
00076   d.setZero();
00077 
00078   CINV.startFill(); // FIXME estimate the number of non-zeros
00079   for (Index i = 0; i < rows; ++i)
00080   {
00081     d[i] = 1.0;
00082     rho = 1.0;
00083     e.setZero();
00084     r = d;
00085     p = d;
00086 
00087     while (rho >= 1e-38)
00088     { /* conjugate gradient to compute e             */
00089       /* which is the i-th row of inv(C * trans(C))  */
00090       l = C.transpose() * p;
00091       q = C * l;
00092       alpha = rho / p.dot(q);
00093       e +=  alpha * p;
00094       r += -alpha * q;
00095       rho_1 = rho;
00096       rho = r.dot(r);
00097       p = (rho/rho_1) * p + r;
00098     }
00099 
00100     l = C.transpose() * e; // l is the i-th row of CINV
00101     // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
00102     for (Index j=0; j<l.size(); ++j)
00103       if (l[j]<1e-15)
00104         CINV.fill(i,j) = l[j];
00105 
00106     d[i] = 0.0;
00107   }
00108   CINV.endFill();
00109 }
00110 
00111 
00112 
00118 template<typename TMatrix, typename CMatrix,
00119          typename VectorX, typename VectorB, typename VectorF>
00120 void constrained_cg(const TMatrix& A, const CMatrix& C, VectorX& x,
00121                        const VectorB& b, const VectorF& f, IterationController &iter)
00122 {
00123   typedef typename TMatrix::Scalar Scalar;
00124   typedef typename TMatrix::Index Index;
00125   typedef Matrix<Scalar,Dynamic,1>  TmpVec;
00126 
00127   Scalar rho = 1.0, rho_1, lambda, gamma;
00128   Index xSize = x.size();
00129   TmpVec  p(xSize), q(xSize), q2(xSize),
00130           r(xSize), old_z(xSize), z(xSize),
00131           memox(xSize);
00132   std::vector<bool> satured(C.rows());
00133   p.setZero();
00134   iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b)
00135   if (iter.rhsNorm() == 0.0) iter.setRhsNorm(1.0);
00136 
00137   SparseMatrix<Scalar,RowMajor> CINV(C.rows(), C.cols());
00138   pseudo_inverse(C, CINV);
00139 
00140   while(true)
00141   {
00142     // computation of residual
00143     old_z = z;
00144     memox = x;
00145     r = b;
00146     r += A * -x;
00147     z = r;
00148     bool transition = false;
00149     for (Index i = 0; i < C.rows(); ++i)
00150     {
00151       Scalar al = C.row(i).dot(x) - f.coeff(i);
00152       if (al >= -1.0E-15)
00153       {
00154         if (!satured[i])
00155         {
00156           satured[i] = true;
00157           transition = true;
00158         }
00159         Scalar bb = CINV.row(i).dot(z);
00160         if (bb > 0.0)
00161           // FIXME: we should allow that: z += -bb * C.row(i);
00162           for (typename CMatrix::InnerIterator it(C,i); it; ++it)
00163             z.coeffRef(it.index()) -= bb*it.value();
00164       }
00165       else
00166         satured[i] = false;
00167     }
00168 
00169     // descent direction
00170     rho_1 = rho;
00171     rho = r.dot(z);
00172 
00173     if (iter.finished(rho)) break;
00174 
00175     if (iter.noiseLevel() > 0 && transition) std::cerr << "CCG: transition\n";
00176     if (transition || iter.first()) gamma = 0.0;
00177     else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
00178     p = z + gamma*p;
00179 
00180     ++iter;
00181     // one dimensionnal optimization
00182     q = A * p;
00183     lambda = rho / q.dot(p);
00184     for (Index i = 0; i < C.rows(); ++i)
00185     {
00186       if (!satured[i])
00187       {
00188         Scalar bb = C.row(i).dot(p) - f[i];
00189         if (bb > 0.0)
00190           lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
00191       }
00192     }
00193     x += lambda * p;
00194     memox -= x;
00195   }
00196 }
00197 
00198 } // end namespace internal
00199 
00200 } // end namespace Eigen
00201 
00202 #endif // EIGEN_CONSTRAINEDCG_H