Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00050 typedef typename CMatrix::Scalar Scalar;
00051 typedef typename CMatrix::Index Index;
00052
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();
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 {
00072
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;
00084
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)));
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
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
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
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
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 }
00182
00183 }
00184
00185 #endif // EIGEN_CONSTRAINEDCG_H