cquad/cquad.c

   1 #include <stdlib.h>
   2 #include <math.h>
   3 #include <string.h>
   4 #include <stdio.h>
   5
   6 #include "cquad.h"
   7
   8 #define MIN(a,b) ((a) < (b) ? (a) : (b))
   9 #define MAX(a,b) ((a) > (b) ? (a) : (b))
  10
  11 /**************************************************************************/
  12 /* a quadratic model for n-dimensional data.  in Matlab notation:
  13
  14         model(x) = q0 + q' (x-x0) + 0.5 * (x-x0)' * Q * (x-x0)
  15
  16    where x0 is an origin (array of length n), q is the gradient vector
  17    (length n), and Q is the n x n Hessian matrix. */
  18 typedef struct {
  19      int n;
  20      const double *x0; /* length n vector of reference pt */
  21      double q0, *q, *Q; /* q is a length n vector, and Q is an n x n matrix */
  22 } quad_model;
  23
  24 static quad_model alloc_model(int n)
  25 {
  26      quad_model model;
  27      model.n = n;
  28      model.x0 = 0;
  29      model.q0 = 0;
  30      model.q = (double *) malloc(sizeof(double) * n);
  31      model.Q = (double *) malloc(sizeof(double) * n*n);
  32      return model;
  33 }
  34
  35 static void free_model(quad_model *model)
  36 {
  37      free(model->Q); model->Q = 0;
  38      free(model->q); model->q = 0;
  39 }
  40
  41 /* evaluate the model for the vector x (length x) */
  42 static double eval_model(const quad_model *model, const double *x)
  43 {
  44      double q0 = model->q0, *q = model->q, *Q = model->Q;
  45      const double *x0 = model->x0;
  46      int j, n = model->n;
  47      double val = q0;
  48      for (j = 0; j < n; ++j) {
  49           double sum = 0;
  50           int k;
  51           for (k = 0; k < n; ++k)
  52                sum += Q[j*n + k] * (x[k] - x0[k]);
  53           val += (q[j] + 0.5 * sum) * (x[j] - x0[j]);
  54      }
  55      return val;
  56 }
  57
  58 /* gradient of the model at x -> grad */
  59 static void eval_model_grad(const quad_model *model, const double *x,
  60                             double *grad)
  61 {
  62      double *q = model->q, *Q = model->Q;
  63      const double *x0 = model->x0;
  64      int j, n = model->n;
  65      for (j = 0; j < n; ++j) {
  66           double sum = 0;
  67           int k;
  68           for (k = 0; k < n; ++k)
  69                sum += Q[j*n + k] * (x[k] - x0[k]);
  70           grad[j] = q[j] + sum;
  71      }
  72 }
  73
  74 #define DSYSV_BLOCKSIZE 64
  75
  76 #define DSYSV dsysv_
  77
  78 /* LAPACK routine to solve a real-symmetric indefinite system A X = B */
  79 extern void DSYSV(const char *uplo, const int *N, const int *NRHS,
  80                   double *A, const int *LDA,
  81                   int *ipiv,
  82                   double *B, const int *LDB,
  83                   double *work, const int *lwork,
  84                   int *info);
  85
  86 /* update the model when one of the x vectors has been changed.
  87    W is an N by N array (N = M + n + 1), r is length N; both uninitialized.
  88    X is an M x n array of the input vectors, inew is the index (0 <= inew < M)
  89    of the changed vector, and fnew is the function value at the new point.
  90    iwork has length N, and work has length DSYSV_BLOCKSIZE * N.
  91
  92    See Powell (2004) for how this routine works.  Here, we use the
  93    simple O((M+n)^3) technique, rather than the fancy O((M+n)^2) method
  94    described by Powell, as explained in the README. */
  95 static void update_model(quad_model *model, double *W, double *r,
  96                          int M, double *X, int inew, double fnew,
  97                          int *iwork, double *work)
  98 {
  99      double *q = model->q, *Q = model->Q;
 100      const double *x0 = model->x0;
 101      double *lam = r, *c = r + M, *g = r + M+1;
 102      int i, j, k, n = model->n, N = M + n + 1, one = 1;
 103      int lwork = N * DSYSV_BLOCKSIZE, info;
 104
 105      /* set A matrix; A = 0.5 * (X * X').^2 */
 106      for (i = 0; i < M; ++i) {
 107           double *xi = X + i*n;
 108           for (j = 0; j < M; ++j) {
 109                double sum = 0, *xj = X + j*n;
 110                for (k = 0; k < n; ++k)
 111                     sum += (xi[k] - x0[k]) * (xj[k] - x0[k]);
 112                W[i * N + j] = W[j * N + i] = 0.5 * sum * sum;
 113           }
 114      }
 115      /* update X matrix: */
 116      for (i = 0; i < M; ++i) {
 117           double *xi = X + i*n;
 118           W[M * N + i] = W[i * N + M] = 1.0;
 119           for (k = 0; k < n; ++k)
 120                W[(M+1+k) * N + i] = W[i * N + (M+1+k)] = xi[k] - x0[k];
 121      }
 122
 123      memset(r, 0, sizeof(double) * N);
 124      r[inew] = fnew - eval_model(model, X + inew*n);
 125
 126      /* solve s = W \ r, via the LAPACK symmetric-indefinite solver */
 127      DSYSV("U", &N, &one, W, &N, iwork, r, &N, work, &lwork, &info);
 128      if (info != 0) {
 129           fprintf(stderr, "nlopt cquad: failure %d in dsysv", info);
 130           abort();
 131      }
 132
 133      /* update model */
 134      model->q0 += *c;
 135      for (k = 0; k < n; ++k) q[k] += g[k];
 136      for (i = 0; i < n; ++i)
 137           for (j = i; j < n; ++j) {
 138                double sum = 0;
 139                for (k = 0; k < M; ++k)
 140                     sum += lam[k] * (X[k*n+i] - x0[i]) * (X[k*n+j] - x0[j]);
 141                Q[i*n + j] = Q[j*n + i] = Q[i*n + j] + sum;
 142           }
 143 }
 144
 145 /* insert the new point xnew (length n) into the array of points X (M x n),
 146    given the current optimal point xopt (length n).   Returns index inew
 147    of replaced point in X. */
 148 static int insert_new_point(int n, const double *xnew, const double *xopt,
 149                             int M, double *X)
 150 {
 151      int inew = 0, i, j;
 152      double dist2max = 0;
 153      /* just use a simple algorithm: replace the point farthest from xopt */
 154      for (i = 0; i < M; ++i) {
 155           double *xi = X + i * n;
 156           double dist2 = 0;
 157           for (j = 0; j < n; ++j ) dist2 += (xi[j] - xopt[j]);
 158           if (dist2 > dist2max) {
 159                dist2max = dist2;
 160                inew = i;
 161           }
 162      }
 163      memcpy(X + inew * n, xnew, sizeof(double) * n);
 164      return inew;
 165 }
 166
 167 /**************************************************************************/
 168 /* a conservative quadratic model is the generic quadratic model above,
 169    plus 0.5 * |(x - xopt) / sigma|^2 * rho, where rho is nonnegative */
 170 typedef struct {
 171      const double *xopt, *sigma;
 172      quad_model model;
 173      double rho;
 174 } conservative_model;
 175
 176 static double cmodel_func(int n, const double *x, double *grad, void *data)
 177 {
 178      conservative_model *cmodel = (conservative_model *) data;
 179      double rho = cmodel->rho, val;
 180      const double *xopt = cmodel->xopt, *sigma = cmodel->sigma;
 181      int j;
 182
 183      val = eval_model(&cmodel->model, x);
 184      if (grad) eval_model_grad(&cmodel->model, x, grad);
 185      for (j = 0; j < n; ++j) {
 186           double siginv = 1.0 / sigma[j];
 187           double dx = (x[j] - xopt[j]) * siginv;
 188           val += (0.5 * rho) * dx*dx;
 189           if (grad) grad[j] += rho * dx * siginv;
 190      }
 191      return val;
 192 }
 193
 194 /* just the rho part of the model (what Svanberg calls the "w" function) */
 195 static double wfunc(int n, const double *x, const conservative_model *cmodel)
 196 {
 197      const double *xopt = cmodel->xopt, *sigma = cmodel->sigma;
 198      double val = 0;
 199      int j;
 200      for (j = 0; j < n; ++j) {
 201           double dx = (x[j] - xopt[j]) / sigma[j];
 202           val += 0.5 * dx*dx;
 203      }
 204      return val;
 205 }
 206
 207 /**************************************************************************/
 208
 209 /* magic minimum value for rho in MMA ... the 2002 paper says it should
 210    be a "fixed, strictly positive `small' number, e.g. 1e-5"
 211    ... grrr, I hate these magic numbers, which seem like they
 212    should depend on the objective function in some way ... in particular,
 213    note that rho is dimensionful (= dimensions of objective function) */
 214 #define MMA_RHOMIN 1e-5
 215
 216 int cquad_verbose = 1;
 217
 218 nlopt_result cquad_minimize(int n, nlopt_func f, void *f_data,
 219                             int m, nlopt_func fc,
 220                             void *fc_data_, ptrdiff_t fc_datum_size,
 221                             const double *lb, const double *ub, /* bounds */
 222                             double *x, /* in: initial guess, out: minimizer */
 223                             double *minf,
 224                             nlopt_stopping *stop,
 225                             nlopt_algorithm model_alg,
 226                             double model_tolrel, int model_maxeval)
 227 {
 228      nlopt_result ret = NLOPT_SUCCESS;
 229      double *x0, *xcur, *sigma, *xprev, *xprevprev, fcur, gval;
 230      double *fcval_cur, *gcval, *model_lb, *model_ub;
 231      double *W, *X, *r, *work;
 232      int *iwork;
 233      int i, j, k = 0, M = 2*n + 1, N = M + n + 1;
 234      char *fc_data = (char *) fc_data_;
 235      conservative_model model, *modelc;
 236      int feasible, feasible_cur;
 237
 238      sigma = (double *) malloc(sizeof(double) * (n*7 + m*2 + N*N + M*n + N
 239                                                  + DSYSV_BLOCKSIZE * N));
 240      if (!sigma) return NLOPT_OUT_OF_MEMORY;
 241      x0 = sigma + n;
 242      xcur = x0 + n;
 243      xprev = xcur + n;
 244      xprevprev = xprev + n;
 245      model_lb = xprevprev + n;
 246      model_ub = model_lb + n;
 247
 248      fcval_cur = model_ub + n;
 249      gcval = fcval_cur + m;
 250
 251      W = gcval + m;
 252      X = W + N*N;
 253      r = X + M*n;
 254      work = r + N;
 255
 256      model.model.q = model.model.Q = 0;
 257      modelc = 0;
 258
 259      iwork = (int *) malloc(sizeof(int) * N);
 260      if (!iwork) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
 261
 262      model.xopt = x; model.sigma = sigma;
 263      model.rho = 1;
 264      model.model = alloc_model(n);
 265      if (!model.model.q || !model.model.Q) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
 266      model.model.x0 = x0;
 267
 268      modelc = (conservative_model *) malloc(sizeof(conservative_model) * m);
 269      if (m > 0 && !modelc) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
 270      for (i = 0; i < m; ++i) modelc[i].model.q = modelc[i].model.Q = 0;
 271      for (i = 0; i < m; ++i) {
 272           modelc[i].xopt = x; modelc[i].sigma = sigma;
 273           modelc[i].rho = 1;
 274           modelc[i].model = alloc_model(n);
 275           if (!modelc[i].model.q || !modelc[i].model.Q) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
 276           modelc[i].model.x0 = x0;
 277      }
 278
 279      feasible = 0;
 280      *minf = HUGE_VAL;
 281
 282      {
 283           int iM = 0;
 284           double *dx = xprev; /* initial step to construct quad. model */
 285
 286           for (j = 0; j < n; ++j) {
 287                if (nlopt_isinf(ub[j]) || nlopt_isinf(lb[j]))
 288                     sigma[j] = 1.0; /* arbitrary default */
 289                else
 290                     sigma[j] = 0.25 * (ub[j] - lb[j]);
 291                dx[j] = sigma[j];
 292                if (x[j] + dx[j] > ub[j])
 293                     x0[j] = x[j] - dx[j];
 294                else if (x[j] - dx[j] < lb[j])
 295                     x0[j] = x[j] + dx[j];
 296                else
 297                     x0[j] = x[j];
 298           }
 299
 300           /* initialize quadratic model via simple finite differences
 301              around x0, as suggested by Powell */
 302
 303           model.model.q0 = f(n, x0, NULL, f_data);
 304           memcpy(X + (iM++ * n), x0, n * sizeof(double));
 305           memset(model.model.Q, 0, sizeof(double) * n*n);
 306           stop->nevals++;
 307           feasible_cur = 1;
 308           for (i = 0; i < m; ++i) {
 309                modelc[i].model.q0 = fc(n, x0, NULL, fc_data + fc_datum_size*i);
 310                memset(modelc[i].model.Q, 0, sizeof(double) * n*n);
 311                feasible_cur = feasible_cur && (modelc[i].model.q0 <= 0);
 312           }
 313           if (feasible_cur) {
 314                *minf = model.model.q0;
 315                memcpy(x, x0, sizeof(double) * n);
 316                feasible = 1;
 317           }
 318           if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
 319           else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
 320           else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
 321           else if (*minf < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
 322           if (ret != NLOPT_SUCCESS) goto done;
 323
 324           memcpy(xcur, x0, sizeof(double) * n);
 325           for (j = 0; j < n; ++j) {
 326                double fmp[2], *fcmp[2];
 327                int s;
 328                fcmp[0] = work; fcmp[1] = work + m;
 329                for (s = 0; s < 2; ++s) {
 330                     xcur[j] = x0[j] + (2*s - 1) * dx[j];
 331                     fmp[s] = fcur = f(n, xcur, NULL, f_data);
 332                     memcpy(X + (iM++ * n), xcur, n * sizeof(double));
 333                     stop->nevals++;
 334                     feasible_cur = 1;
 335                     for (i = 0; i < m; ++i) {
 336                          fcmp[s][i] = fcval_cur[i] =
 337                               fc(n, xcur, NULL, fc_data + fc_datum_size*i);
 338                          feasible_cur = feasible_cur && (fcval_cur[i] <= 0);
 339                     }
 340                     if (feasible_cur && fcur < *minf) {
 341                          *minf = fcur;
 342                          memcpy(x, xcur, sizeof(double) * n);
 343                          feasible = 1;
 344                     }
 345                     if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
 346                     else if (nlopt_stop_evals(stop))
 347                          ret = NLOPT_MAXEVAL_REACHED;
 348                     else if (nlopt_stop_time(stop))
 349                          ret = NLOPT_MAXTIME_REACHED;
 350                     else if (*minf < stop->minf_max)
 351                          ret = NLOPT_MINF_MAX_REACHED;
 352                     if (ret != NLOPT_SUCCESS) goto done;
 353                }
 354                xcur[j] = x0[j];
 355                model.model.q[j] = (fmp[1] - fmp[0]) / (2 * dx[j]);
 356                model.model.Q[j*n+j] = (fmp[1] + fmp[0]
 357                                        - 2*model.model.q0) / (dx[j]*dx[j]);
 358                for (i = 0; i < m; ++i) {
 359                     modelc[i].model.q[j] =
 360                          (fcmp[1][i] - fcmp[0][i]) / (2 * dx[j]);
 361                     modelc[i].model.Q[j*n+j] = (fcmp[1][i] + fcmp[0][i]
 362                                       - 2*modelc[i].model.q0) / (dx[j]*dx[j]);
 363                }
 364           }
 365      }
 366
 367      fcur = *minf;
 368      memcpy(xcur, x, sizeof(double) * n);
 369
 370      while (1) { /* outer iterations */
 371           double fprev = fcur;
 372           if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
 373           else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
 374           else if (*minf < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
 375           if (ret != NLOPT_SUCCESS) goto done;
 376           if (++k > 1) memcpy(xprevprev, xprev, sizeof(double) * n);
 377           memcpy(xprev, xcur, sizeof(double) * n);
 378
 379           while (1) { /* inner iterations */
 380                double min_model;
 381                int inner_done, iMnew;
 382                nlopt_result reti;
 383
 384                /* solve model problem */
 385                for (j = 0; j < n; ++j) {
 386                     model_lb[j] = MAX(lb[j], x[j] - 0.9 * sigma[j]);
 387                     model_ub[j] = MIN(ub[j], x[j] + 0.9 * sigma[j]);
 388                }
 389           model_solution:
 390                memcpy(xcur, x, sizeof(double) * n);
 391                reti = nlopt_minimize_constrained(
 392                     model_alg, n, cmodel_func, &model,
 393                     m, cmodel_func, modelc, sizeof(conservative_model),
 394                     model_lb, model_ub, xcur, &min_model,
 395                     -HUGE_VAL, model_tolrel,0., 0.,NULL, model_maxeval,
 396                     stop->maxtime - (nlopt_seconds() - stop->start));
 397                if (reti == NLOPT_FAILURE && model_alg != NLOPT_LD_MMA) {
 398                     /* LBFGS etc. converge quickly but are sometimes
 399                        very finicky if there are any rounding errors in
 400                        the gradient, etcetera; if it fails, try again
 401                        with MMA called recursively for the model */
 402                     model_alg = NLOPT_LD_MMA;
 403                     if (cquad_verbose)
 404                          printf("cquad: switching to MMA for model\n");
 405                     goto model_solution;
 406                }
 407                if (reti < 0 || reti == NLOPT_MAXTIME_REACHED) {
 408                     ret = reti;
 409                     goto done;
 410                }
 411
 412           got_new_xcur:
 413                /* evaluate final xcur etc. */
 414                gval = cmodel_func(n, xcur, NULL, &model);
 415                for (i = 0; i < m; ++i)
 416                     gcval[i] = cmodel_func(n, xcur, NULL, modelc + i);
 417
 418                fcur = f(n, xcur, NULL, f_data);
 419                stop->nevals++;
 420                feasible_cur = 1;
 421                inner_done = gval >= fcur;
 422                for (i = 0; i < m; ++i) {
 423                     fcval_cur[i] = fc(n, xcur, NULL,
 424                                       fc_data + fc_datum_size * i);
 425                     feasible_cur = feasible_cur && (fcval_cur[i] <= 0);
 426                     inner_done = inner_done && (gcval[i] >= fcval_cur[i]);
 427                }
 428
 429                if (cquad_verbose) {
 430                     printf("cquad model converged to g=%g vs. f=%g:\n",
 431                            gval, fcur);
 432                     for (i = 0; i < MIN(cquad_verbose, m); ++i)
 433                          printf("    cquad gc[%d]=%g vs. fc[%d]=%g\n",
 434                                 i, gcval[i], i, fcval_cur[i]);
 435                }
 436
 437                /* update the quadratic models */
 438                iMnew = insert_new_point(n, xcur, x, M, X);
 439                update_model(&model.model, W, r, M, X, iMnew, fcur, iwork,work);
 440                for (i = 0; i < m; ++i)
 441                     update_model(&modelc[i].model, W, r, M, X, iMnew,
 442                                  fcval_cur[i], iwork,work);
 443
 444                /* once we have reached a feasible solution, the
 445                   algorithm should never make the solution infeasible
 446                   again (if inner_done), although the constraints may
 447                   be violated slightly by rounding errors etc. so we
 448                   must be a little careful about checking feasibility */
 449                if (feasible_cur) feasible = 1;
 450
 451                if (fcur < *minf && (inner_done || feasible_cur || !feasible)) {
 452                     if (cquad_verbose && !feasible_cur)
 453                          printf("cquad - using infeasible point?\n");
 454                     *minf = fcur;
 455                     memcpy(x, xcur, sizeof(double)*n);
 456                }
 457                if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
 458                else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
 459                else if (*minf < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
 460                if (ret != NLOPT_SUCCESS) goto done;
 461
 462                /* check for ill-conditioned model matrix, as indicated
 463                   by a model that doesn't match f(xcur)=fcur as required */
 464                if (0 && fabs(eval_model(&model.model, xcur) - fcur)
 465                    > 1e-6 * fabs(fcur)) {
 466                     if (cquad_verbose)
 467                          printf("cquad conditioning problem, diff = %g\n",
 468                                 eval_model(&model.model, xcur) - fcur);
 469                     /* pick a random point to insert, using X diameter */
 470                     for (j = 0; j < n; ++j) {
 471                          double diam = 1e-3 * sigma[j]; /* minimum diam */
 472                          for (i = 0; i < M; ++i) {
 473                               double diami = fabs(X[i*n+j] - x[j]);
 474                               if (diami > diam) diam = diami;
 475                          }
 476                          diam *= 0.1;
 477                          xcur[j] = x[j] + nlopt_urand(-diam, diam);
 478                     }
 479                     goto got_new_xcur;
 480                }
 481
 482                if (inner_done) break;
 483
 484                if (fcur > gval)
 485                     model.rho = MIN(10*model.rho,
 486                                     1.1 * (model.rho + (fcur-gval)
 487                                            / wfunc(n, xcur, &model)));
 488                for (i = 0; i < m; ++i)
 489                     if (fcval_cur[i] > gcval[i])
 490                          modelc[i].rho =
 491                               MIN(10*modelc[i].rho,
 492                                   1.1 * (modelc[i].rho
 493                                          + (fcval_cur[i]-gcval[i])
 494                                          / wfunc(n, xcur, &modelc[i])));
 495
 496                if (cquad_verbose)
 497                     printf("cquad inner iteration: rho -> %g\n", model.rho);
 498                for (i = 0; i < MIN(cquad_verbose, m); ++i)
 499                     printf("                 cquad rhoc[%d] -> %g\n",
 500                            i, modelc[i].rho);
 501           }
 502
 503           if (nlopt_stop_ftol(stop, fcur, fprev))
 504                ret = NLOPT_FTOL_REACHED;
 505           if (nlopt_stop_x(stop, xcur, xprev))
 506                ret = NLOPT_XTOL_REACHED;
 507           if (ret != NLOPT_SUCCESS) goto done;
 508
 509           /* update rho and sigma for iteration k+1 */
 510           model.rho = MAX(0.1 * model.rho, MMA_RHOMIN);
 511           if (cquad_verbose)
 512                printf("cquad outer iteration: rho -> %g\n", model.rho);
 513           for (i = 0; i < m; ++i)
 514                modelc[i].rho = MAX(0.1 * modelc[i].rho, MMA_RHOMIN);
 515           for (i = 0; i < MIN(cquad_verbose, m); ++i)
 516                printf("                 cquad rhoc[%d] -> %g\n",
 517                       i, modelc[i].rho);
 518           if (k > 1) {
 519                for (j = 0; j < n; ++j) {
 520                     double dx2 = (xcur[j]-xprev[j]) * (xprev[j]-xprevprev[j]);
 521                     double gam = dx2 < 0 ? 0.7 : (dx2 > 0 ? 1.2 : 1);
 522                     sigma[j] *= gam;
 523                     if (!nlopt_isinf(ub[j]) && !nlopt_isinf(lb[j])) {
 524                          sigma[j] = MIN(sigma[j], 10*(ub[j]-lb[j]));
 525                          sigma[j] = MAX(sigma[j], 0.01*(ub[j]-lb[j]));
 526                     }
 527                }
 528                for (j = 0; j < MIN(cquad_verbose, n); ++j)
 529                     printf("                 cquad sigma[%d] -> %g\n",
 530                            j, sigma[j]);
 531           }
 532      }
 533
 534  done:
 535      free(modelc);
 536      for (i = 0; i < m; ++i)
 537           free_model(&modelc[i].model);
 538      free_model(&model.model);
 539      free(iwork);
 540      free(sigma);
 541      return ret;
 542 }