src/qlpsolve.cc

   1 #include "qlpsolve.hh"
   2 #include "const.hh"
   3 #include "debug.hh"
   4 #include "choleski.hh"
   5
   6 const Real TOL=1e-2;            // roughly 1/10 mm
   7
   8 String
   9 Active_constraints::status() const
  10 {
  11     String s("Active|Inactive [");
  12     for (int i=0; i< active.sz(); i++) {
  13         s += String(active[i]) + " ";
  14     }
  15
  16     s+="| ";
  17     for (int i=0; i< inactive.sz(); i++) {
  18         s += String(inactive[i]) + " ";
  19     }
  20     s+="]";
  21
  22     return s;
  23 }
  24
  25 void
  26 Active_constraints::OK() {
  27     H.OK();
  28     A.OK();
  29     assert(active.sz() +inactive.sz() == opt->cons.sz());
  30     assert(H.dim() == opt->dim());
  31     assert(active.sz() == A.rows());
  32     svec<int> allcons;
  33
  34     for (int i=0; i < opt->cons.sz(); i++)
  35         allcons.add(0);
  36     for (int i=0; i < active.sz(); i++) {
  37         int j = active[i];
  38         allcons[j]++;
  39     }
  40     for (int i=0; i < inactive.sz(); i++) {
  41         int j = inactive[i];
  42         allcons[j]++;
  43     }
  44     for (int i=0; i < allcons.sz(); i++)
  45         assert(allcons[i] == 1);
  46 }
  47
  48 Vector
  49 Active_constraints::get_lagrange(Vector gradient)
  50 {
  51     Vector l(A*gradient);
  52
  53     return l;
  54 }
  55
  56 void
  57 Active_constraints::add(int k)
  58 {
  59     // add indices
  60     int cidx=inactive[k];
  61     active.add(cidx);
  62
  63     inactive.swap(k,inactive.sz()-1);
  64     inactive.pop();
  65
  66     Vector a( opt->cons[cidx] );
  67     // update of matrices
  68     Vector Ha = H*a;
  69     Real aHa = a*Ha;
  70     Vector addrow(Ha.dim());
  71     if (abs(aHa) > EPS) {
  72         /*
  73           a != 0, so if Ha = O(EPS), then
  74           Ha * aH / aHa = O(EPS^2/EPS)
  75
  76           if H*a == 0, the constraints are dependent.
  77           */
  78         H -= Matrix(Ha/aHa , Ha);
  79
  80
  81         /*
  82           sorry, don't know how to justify this. ..
  83           */
  84         addrow=Ha;
  85         addrow/= aHa;
  86         A -= Matrix(A*a, addrow);
  87         A.insert_row(addrow,A.rows());
  88     }else
  89         WARN << "degenerate constraints";
  90 }
  91
  92 void
  93 Active_constraints::drop(int k)
  94 {
  95     int q=active.sz()-1;
  96
  97         // drop indices
  98     inactive.add(active[k]);
  99     active.swap(k,q);
 100     A.swap_rows(k,q);
 101     active.pop();
 102
 103     Vector a(A.row(q));
 104     if (a.norm() > EPS) {
 105         /*
 106
 107          */
 108         Real q = a*opt->quad*a;
 109         H += Matrix(a,a/q);
 110         A -= A*opt->quad*Matrix(a,a/q);
 111     }else
 112         WARN << "degenerate constraints";
 113    #ifndef NDEBUG
 114     Vector rem_row(A.row(q));
 115     assert(rem_row.norm() < EPS);
 116    #endif
 117
 118     A.delete_row(q);
 119 }
 120
 121
 122 Active_constraints::Active_constraints(Ineq_constrained_qp const *op)
 123     :       A(0,op->dim()),
 124             H(op->dim()),
 125             opt(op)
 126 {
 127     for (int i=0; i < op->cons.sz(); i++)
 128         inactive.add(i);
 129     Choleski_decomposition chol(op->quad);
 130     H=chol.inverse();
 131 }
 132
 133 /* Find the optimum which is in the planes generated by the active
 134     constraints.
 135     */
 136 Vector
 137 Active_constraints::find_active_optimum(Vector g)
 138 {
 139     return H*g;
 140 }
 141
 142 /****************************************************************/
 143
 144 int
 145 min_elt_index(Vector v)
 146 {
 147     Real m=INFTY; int idx=-1;
 148     for (int i = 0; i < v.dim(); i++){
 149         if (v(i) < m) {
 150             idx = i;
 151             m = v(i);
 152         }
 153         assert(v(i) <= INFTY);
 154     }
 155     return idx;
 156 }
 157
 158 ///the numerical solving
 159 Vector
 160 Ineq_constrained_qp::solve(Vector start) const
 161 {
 162     Active_constraints act(this);
 163
 164
 165     act.OK();
 166
 167
 168     Vector x(start);
 169     Vector gradient=quad*x+lin;
 170
 171
 172     Vector last_gradient(gradient);
 173     int iterations=0;
 174
 175     while (iterations++ < MAXITER) {
 176         Vector direction= - act.find_active_optimum(gradient);
 177
 178         mtor << "gradient "<< gradient<< "\ndirection " << direction<<"\n";
 179
 180         if (direction.norm() > EPS) {
 181             mtor << act.status() << '\n';
 182
 183             Real minalf = INFTY;
 184
 185             Inactive_iter minidx(act);
 186
 187
 188             /*
 189     we know the optimum on this "hyperplane". Check if we
 190     bump into the edges of the simplex
 191     */
 192
 193             for (Inactive_iter ia(act); ia.ok(); ia++) {
 194
 195                 if (ia.vec() * direction >= 0)
 196                     continue;
 197                 Real alfa= - (ia.vec()*x - ia.rhs())/
 198                     (ia.vec()*direction);
 199
 200                 if (minalf > alfa) {
 201                     minidx = ia;
 202                     minalf = alfa;
 203                 }
 204             }
 205             Real unbounded_alfa = 1.0;
 206             Real optimal_step = min(minalf, unbounded_alfa);
 207
 208             Vector deltax=direction * optimal_step;
 209             x += deltax;
 210             gradient += optimal_step * (quad * deltax);
 211
 212             mtor << "step = " << optimal_step<< " (|dx| = " <<
 213                 deltax.norm() << ")\n";
 214
 215             if (minalf < unbounded_alfa) {
 216                 /* bumped into an edge. try again, in smaller space. */
 217                 act.add(minidx.idx());
 218                 mtor << "adding cons "<< minidx.idx()<<'\n';
 219                 continue;
 220             }
 221             /*ASSERT: we are at optimal solution for this "plane"*/
 222
 223
 224         }
 225
 226         Vector lagrange_mult=act.get_lagrange(gradient);
 227         int m= min_elt_index(lagrange_mult);
 228
 229         if (m>=0 && lagrange_mult(m) > 0) {
 230             break;              // optimal sol.
 231         } else if (m<0) {
 232             assert(gradient.norm() < EPS) ;
 233
 234             break;
 235         }
 236
 237         mtor << "dropping cons " << m<<'\n';
 238         act.drop(m);
 239     }
 240     if (iterations >= MAXITER)
 241         WARN<<"didn't converge!\n";
 242
 243     mtor <<  ": found " << x<<" in " << iterations <<" iterations\n";
 244     assert_solution(x);
 245     return x;
 246 }
 247
 248 /** Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
 249     Prentice Hall.
 250
 251     Section 13.3
 252
 253     This is a "projected gradient" algorithm. Starting from a point x
 254     the next point is found in a direction determined by projecting
 255     the gradient onto the active constraints.  (well, not really the
 256     gradient. The optimal solution obeying the active constraints is
 257     tried. This is why H = Q^-1 in initialisation) )
 258
 259
 260     */
 261