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


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:58:04