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