29 #include "../../../../Eigen/src/Core/util/NonMPL2.h" 31 #ifndef EIGEN_CONSTRAINEDCG_H 32 #define EIGEN_CONSTRAINEDCG_H 46 template <
typename CMatrix,
typename CINVMatrix>
50 typedef typename CMatrix::Scalar Scalar;
51 typedef typename CMatrix::Index Index;
55 Index rows = C.
rows(), cols = C.cols();
57 TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
58 Scalar
rho, rho_1, alpha;
62 for (Index i = 0; i < rows; ++i)
73 l = C.transpose() * p;
75 alpha = rho / p.dot(q);
80 p = (rho/rho_1) * p + r;
83 l = C.transpose() * e;
85 for (Index j=0; j<l.size(); ++j)
87 CINV.fill(i,j) = l[j];
101 template<
typename TMatrix,
typename CMatrix,
102 typename VectorX,
typename VectorB,
typename VectorF>
106 typedef typename TMatrix::Scalar Scalar;
107 typedef typename TMatrix::Index Index;
110 Scalar
rho = 1.0, rho_1, lambda, gamma;
111 Index xSize = x.size();
112 TmpVec p(xSize), q(xSize), q2(xSize),
113 r(xSize), old_z(xSize), z(xSize),
115 std::vector<bool> satured(C.rows());
131 bool transition =
false;
132 for (Index i = 0; i < C.rows(); ++i)
134 Scalar al = C.row(i).dot(x) - f.coeff(i);
142 Scalar bb = CINV.row(i).dot(z);
145 for (
typename CMatrix::InnerIterator it(C,i); it; ++it)
146 z.coeffRef(it.index()) -= bb*it.value();
158 if (iter.
noiseLevel() > 0 && transition) std::cerr <<
"CCG: transition\n";
159 if (transition || iter.
first()) gamma = 0.0;
160 else gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
166 lambda = rho / q.dot(p);
167 for (Index i = 0; i < C.rows(); ++i)
171 Scalar bb = C.row(i).dot(p) - f[i];
173 lambda = (std::min)(lambda, (f.coeff(i)-C.row(i).dot(x)) / bb);
185 #endif // EIGEN_CONSTRAINEDCG_H void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
IntermediateState sqrt(const Expression &arg)
iterative scaling algorithm to equilibrate rows and column norms in matrices
void setRhsNorm(double r)
EIGEN_STRONG_INLINE Index rows() const
void constrained_cg(const TMatrix &A, const CMatrix &C, VectorX &x, const VectorB &b, const VectorF &f, IterationController &iter)
Controls the iterations of the iterative solvers.