Report free variable name in N-R diagnostics instead of just #
[openmx:openmx.git] / src / ComputeNR.cpp
1 /*
2  *  Copyright 2013 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *   Unless required by applicable law or agreed to in writing, software
11  *   distributed under the License is distributed on an "AS IS" BASIS,
12  *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  */
16
17 #include <valarray>
18
19 #include "omxState.h"
20 #include "omxFitFunction.h"
21 #include "omxExportBackendState.h"
22 #include "Compute.h"
23
24 class ComputeNR : public omxCompute {
25         typedef omxCompute super;
26         omxMatrix *fitMatrix;
27
28         int maxIter;
29         double tolerance;
30         int inform, iter;
31         int verbose;
32         bool carefully;
33
34 public:
35         ComputeNR();
36         virtual void initFromFrontend(SEXP rObj);
37         virtual void compute(FitContext *fc);
38         virtual void reportResults(FitContext *fc, MxRList *out);
39         virtual double getOptimizerStatus() { return inform; }  // backward compatibility
40 };
41
42 class omxCompute *newComputeNewtonRaphson()
43 {
44         return new ComputeNR();
45 }
46
47 ComputeNR::ComputeNR()
48 {
49         inform = 0;
50         iter = 0;
51 }
52
53 void ComputeNR::initFromFrontend(SEXP rObj)
54 {
55         super::initFromFrontend(rObj);
56
57         fitMatrix = omxNewMatrixFromSlot(rObj, globalState, "fitfunction");
58         setFreeVarGroup(fitMatrix->fitFunction, varGroup);
59         omxCompleteFitFunction(fitMatrix);
60
61         if (!fitMatrix->fitFunction->hessianAvailable ||
62             !fitMatrix->fitFunction->gradientAvailable) {
63                 error("Newton-Raphson requires derivatives");
64         }
65
66         SEXP slotValue;
67         PROTECT(slotValue = GET_SLOT(rObj, install("maxIter")));
68         maxIter = INTEGER(slotValue)[0];
69
70         PROTECT(slotValue = GET_SLOT(rObj, install("tolerance")));
71         tolerance = REAL(slotValue)[0];
72         if (tolerance <= 0) error("tolerance must be positive");
73
74         PROTECT(slotValue = GET_SLOT(rObj, install("verbose")));
75         verbose = asInteger(slotValue);
76
77         PROTECT(slotValue = GET_SLOT(rObj, install("carefully")));
78         carefully = asLogical(slotValue);
79 }
80
81 void omxApproxInvertPosDefTriangular(int dim, double *hess, double *ihess, double *stress)
82 {
83         const char uplo = 'L';
84         int info;
85         int retries = 0;
86         const int maxRetries = 31; // assume >=32 bit integers
87         double adj = 0;
88         do {
89                 memcpy(ihess, hess, sizeof(double) * dim * dim);
90
91                 if (retries >= 1) {
92                         int th = maxRetries - retries;
93                         if (th > 0) {
94                                 adj = 1.0/(1 << th);
95                         } else {
96                                 adj = (1 << -th);
97                         }
98                         for (int px=0; px < dim; ++px) {
99                                 ihess[px * dim + px] += adj;
100                         }
101                 }
102
103                 F77_CALL(dpotrf)(&uplo, &dim, ihess, &dim, &info);
104                 if (info < 0) error("Arg %d is invalid", -info);
105                 if (info == 0) break;
106         } while (++retries < maxRetries * 1.5);
107
108         if (info > 0) {
109                 omxRaiseErrorf(globalState, "Hessian is not even close to positive definite (order %d)", info);
110                 return;
111         }
112         F77_CALL(dpotri)(&uplo, &dim, ihess, &dim, &info);
113         if (info < 0) error("Arg %d is invalid", -info);
114         if (info > 0) {
115                 // Impossible to fail if dpotrf worked?
116                 omxRaiseErrorf(globalState, "Hessian is not of full rank");
117         }
118
119         if (stress) *stress = adj;
120 }
121
122 void omxApproxInvertPackedPosDefTriangular(int dim, int *mask, double *packedHess, double *stress)
123 {
124         int mdim = 0;
125         for (int dx=0; dx < dim; ++dx) if (mask[dx]) mdim += 1;
126
127         std::vector<double> hess(mdim * mdim, 0.0);
128         for (int d1=0, px=0, m1=-1; d1 < dim; ++d1) {
129                 if (mask[d1]) ++m1;
130                 for (int d2=0, m2=-1; d2 <= d1; ++d2) {
131                         if (mask[d2]) ++m2;
132                         if (mask[d1] && mask[d2]) {
133                                 hess[m2 * mdim + m1] = packedHess[px];
134                         }
135                         ++px;
136                 }
137         }
138
139         std::vector<double> ihess(mdim * mdim);
140         omxApproxInvertPosDefTriangular(mdim, hess.data(), ihess.data(), stress);
141
142         for (int d1=0, px=0, m1=-1; d1 < dim; ++d1) {
143                 if (mask[d1]) ++m1;
144                 for (int d2=0, m2=-1; d2 <= d1; ++d2) {
145                         if (mask[d2]) ++m2;
146                         if (mask[d1] && mask[d2]) {
147                                 packedHess[px] = *stress? 0 : ihess[m2 * mdim + m1];
148                         }
149                         ++px;
150                 }
151         }
152 }
153
154 class Ramsay1975 {
155         // Ramsay, J. O. (1975). Solving Implicit Equations in
156         // Psychometric Data Analysis.  Psychometrika, 40(3), 337-360.
157
158         FitContext *fc;
159         size_t numParam;
160         int flavor;
161         int verbose;
162         double highWatermark;
163         std::vector<double> prevAdj1;
164         std::vector<double> prevAdj2;
165
166 public:
167         double caution;
168
169         Ramsay1975(FitContext *fc, int flavor, int verbose);
170         void recordEstimate(int px, double newEst);
171         void recalibrate(bool *restart);
172         void restart();
173 };
174
175 Ramsay1975::Ramsay1975(FitContext *fc, int flavor, int verbose)
176 {
177         this->fc = fc;
178         this->flavor = flavor;
179         this->verbose = verbose;
180         caution = 0.0;
181         highWatermark = 0.5;  // arbitrary guess
182
183         numParam = fc->varGroup->vars.size();
184         prevAdj1.assign(numParam, 0);
185         prevAdj2.resize(numParam);
186 }
187
188 void Ramsay1975::recordEstimate(int px, double newEst)
189 {
190         omxFreeVar *fv = fc->varGroup->vars[px];
191         bool hitBound=false;
192         double param = newEst;
193         if (param < fv->lbound) {
194                 hitBound=true;
195                 param = fc->est[px] - (fc->est[px] - fv->lbound) / 2;
196         }
197         if (param > fv->ubound) {
198                 hitBound=true;
199                 param = fc->est[px] + (fv->ubound - fc->est[px]) / 2;
200         }
201         
202         prevAdj2[px] = prevAdj1[px];
203         prevAdj1[px] = param - fc->est[px];
204         
205         if (verbose >= 4) {
206                 std::string buf;
207                 buf += string_snprintf("~%d~%s: %.4f -> %.4f", px, fv->name, fc->est[px], param);
208                 if (hitBound) {
209                         buf += string_snprintf(" wanted %.4f but hit bound", newEst);
210                 }
211                 if (prevAdj1[px] * prevAdj2[px] < 0) {
212                         buf += " *OSC*";
213                 }
214                 buf += "\n";
215                 mxLogBig(buf);
216         }
217
218         fc->est[px] = param;
219 }
220
221 void Ramsay1975::recalibrate(bool *restart)
222 {
223         double normPrevAdj2 = 0;
224         double normAdjDiff = 0;
225         std::vector<double> adjDiff(numParam);
226
227         // The choice of norm is also arbitrary. Other norms might work better.
228         for (size_t px=0; px < numParam; ++px) {
229                 if (fc->flavor[px] != flavor) continue;
230                 adjDiff[px] = prevAdj1[px] - prevAdj2[px];
231                 normPrevAdj2 += prevAdj2[px] * prevAdj2[px];
232         }
233
234         for (size_t px=0; px < numParam; ++px) {
235                 if (fc->flavor[px] != flavor) continue;
236                 normAdjDiff += adjDiff[px] * adjDiff[px];
237         }
238         double ratio = sqrt(normPrevAdj2 / normAdjDiff);
239         //if (verbose >= 3) mxLog("Ramsay[%d]: sqrt(%.5f/%.5f) = %.5f",
240         // flavor, normPrevAdj2, normAdjDiff, ratio);
241
242         double newCaution = 1 - (1-caution) * ratio;
243         if (newCaution < 0) newCaution = 0;  // doesn't make sense to go faster than full speed
244         if (newCaution > .95) newCaution = .95;  // arbitrary guess
245         if (newCaution < caution) {
246                 caution = newCaution/3 + 2*caution/3;  // don't speed up too fast, arbitrary ratio
247         } else {
248                 caution = newCaution;
249         }
250         if (caution < highWatermark || (normPrevAdj2 < 1e-3 && normAdjDiff < 1e-3)) {
251                 if (verbose >= 3) mxLog("Ramsay[%d]: %.2f caution", flavor, caution);
252         } else {
253                 if (verbose >= 3) mxLog("Ramsay[%d]: caution %.2f > %.2f, extreme oscillation, restart recommended",
254                                         flavor, caution, highWatermark);
255                 *restart = TRUE;
256         }
257         highWatermark += .02; // arbitrary guess
258 }
259
260 void Ramsay1975::restart()
261 {
262         prevAdj1.assign(numParam, 0);
263         prevAdj2.assign(numParam, 0);
264         highWatermark = 1 - (1 - highWatermark) * .5; // arbitrary guess
265         caution = std::max(caution, highWatermark);   // arbitrary guess
266         highWatermark = caution;
267         if (verbose >= 3) {
268                 mxLog("Ramsay[%d]: restart with %.2f caution %.2f highWatermark",
269                       flavor, caution, highWatermark);
270         }
271 }
272
273 void pda(const double *ar, int rows, int cols);
274
275 void ComputeNR::compute(FitContext *fc)
276 {
277         // complain if there are non-linear constraints TODO
278
279         size_t numParam = varGroup->vars.size();
280         if (numParam <= 0) {
281                 error("Model has no free parameters");
282                 return;
283         }
284
285         std::vector<Ramsay1975*> ramsay;
286         if (fitMatrix->fitFunction->parametersHaveFlavor) {
287                 for (size_t px=0; px < numParam; ++px) {
288                         fc->flavor[px] = -1;
289                 }
290                 omxFitFunctionCompute(fitMatrix->fitFunction, FF_COMPUTE_PARAMFLAVOR, fc);
291         } else {
292                 OMXZERO(fc->flavor, numParam);
293         }
294
295         if (1) { // add conditions to disable TODO
296                 fc->hgProd.resize(0);
297                 omxFitFunctionCompute(fitMatrix->fitFunction, FF_COMPUTE_HGPROD, fc);
298                 std::sort(fc->hgProd.begin(), fc->hgProd.end());
299                 std::vector<matrixVectorProdTerm>::iterator iter = std::unique(fc->hgProd.begin(), fc->hgProd.end());
300                 fc->hgProd.resize( std::distance(fc->hgProd.begin(), iter) );
301                 //fc->log("NR", FF_COMPUTE_HGPROD);
302         }
303
304         for (size_t px=0; px < numParam; ++px) {
305                 // global namespace for flavors? TODO
306                 if (fc->flavor[px] < 0 || fc->flavor[px] > 100) {  // max flavor? TODO
307                         error("Invalid parameter flavor %d", fc->flavor[px]);
308                 }
309                 while (int(ramsay.size()) < fc->flavor[px]+1) {
310                         ramsay.push_back(new Ramsay1975(fc, fc->flavor[px], verbose));
311                 }
312         }
313
314         omxFitFunctionCompute(fitMatrix->fitFunction, FF_COMPUTE_PREOPTIMIZE, fc);
315         fc->maybeCopyParamToModel(globalState);
316
317         iter = 0;
318         int sinceRestart = 0;
319         bool converged = false;
320         bool approaching = false;
321         bool restarted = carefully;
322         double maxAdj = 0;
323         double maxAdjSigned = 0;
324         int maxAdjFlavor = 0;
325         int maxAdjParam = -1;
326         double bestLL = 0;
327
328         std::valarray<double> startBackup(fc->est, numParam);
329         std::vector<double> bestBackup(numParam);
330
331         fitMatrix->data[0] = 0;  // may not recompute it, don't leave stale data
332
333         if (verbose >= 2) {
334                 mxLog("Welcome to Newton-Raphson (tolerance %.3g, max iter %d, %ld flavors)",
335                       tolerance, maxIter, ramsay.size());
336         }
337         while (1) {
338                 if (verbose >= 2) {
339                         const char *pname = "none";
340                         if (maxAdjParam >= 0) pname = fc->varGroup->vars[maxAdjParam]->name;
341                         mxLog("Begin %d/%d iter of Newton-Raphson (prev maxAdj %.4f for %s, flavor %d)",
342                               iter+1, maxIter, maxAdjSigned, pname, maxAdjFlavor);
343                 }
344
345                 int want = FF_COMPUTE_GRADIENT|FF_COMPUTE_IHESSIAN;
346                 if (restarted) want |= FF_COMPUTE_FIT;
347
348                 OMXZERO(fc->grad, numParam);
349                 OMXZERO(fc->ihess, numParam * numParam);
350
351                 omxFitFunctionCompute(fitMatrix->fitFunction, want, fc);
352                 if (isErrorRaised(globalState)) break;
353
354                 if (verbose >= 5) {
355                         int show = FF_COMPUTE_ESTIMATE;
356                         if (verbose >= 6) show |= FF_COMPUTE_GRADIENT|FF_COMPUTE_IHESSIAN;
357                         fc->log("Newton-Raphson", show);
358                 }
359
360                 double LL = fitMatrix->data[0];
361                 if (bestLL == 0 || bestLL > LL) {
362                         bestLL = LL;
363                         memcpy(bestBackup.data(), fc->est, sizeof(double) * numParam);
364                 }
365                 if (bestLL > 0) {
366                         if (verbose >= 3) mxLog("Newton-Raphson cantankerous fit %.5f (best %.5f)", LL, bestLL);
367                         if (approaching && (LL > bestLL + 0.5 || !std::isfinite(LL))) {  // arbitrary threshold
368                                 memcpy(fc->est, bestBackup.data(), sizeof(double) * numParam);
369                                 fc->copyParamToModel(globalState);
370                                 break;
371                         }
372                 }
373
374                 if (verbose >= 1 || OMX_DEBUG) {
375                         for (size_t h1=1; h1 < numParam; h1++) {
376                                 for (size_t h2=0; h2 < h1; h2++) {
377                                         if (fc->ihess[h2 * numParam + h1] == 0) continue;
378                                         omxRaiseErrorf(globalState, "Inverse Hessian is not upper triangular");
379                                 }
380                         }
381                 }
382
383                 if (0) {
384                         const double maxDiag = 4;
385                         for (size_t dx=0; dx < numParam; dx++) {
386                                 double mag = fabs(fc->ihess[dx * numParam + dx]);
387                                 if (maxDiag < mag) {
388                                         double old = fc->grad[dx];
389                                         double logBad = log(1 + mag - maxDiag);
390                                         fc->grad[dx] /= (1 + logBad);  // arbitrary guess
391                                         mxLog("ihess bad at diag %lu grad %.8g -> %.8g", dx, old, fc->grad[dx]);
392                                 }
393                         }
394                         //fc->log("bad", FF_COMPUTE_IHESSIAN);
395                 }
396
397                 bool restart = false;
398                 if ((++sinceRestart) % 3 == 0) {
399                         for (size_t rx=0; rx < ramsay.size(); ++rx) {
400                                 ramsay[rx]->recalibrate(&restart);
401                         }
402                 }
403                 for (size_t px=0; px < numParam; ++px) {
404                         if (!std::isfinite(fc->grad[px])) {
405                                 if (!restart) {
406                                         if (verbose >= 3) {
407                                                 mxLog("Newton-Raphson: grad[%lu] not finite, restart recommended", px);
408                                         }
409                                         restart = true;
410                                         break;
411                                 }
412                         }
413                 }
414                 if ((sinceRestart % 3) == 0 && !restart) {
415                         // 3 iterations without extreme oscillation or NaN gradients
416                         if (!approaching && verbose >= 2) {
417                                 mxLog("Newton-Raphson: Probably approaching solution");
418                         }
419                         approaching = true;
420                 }
421
422                 maxAdjParam = -1;
423                 maxAdj = 0;
424                 if (restart && (!approaching || !restarted)) {
425                         approaching = false;
426                         restarted = true;
427                         bestLL = 0;
428                         sinceRestart = 0;
429
430                         if (verbose >= 2) {
431                                 mxLog("Newton-Raphson: Increasing damping ...");
432                         }
433                         for (size_t px=0; px < numParam; ++px) {
434                                 fc->est[px] = startBackup[px];
435                         }
436                         for (size_t rx=0; rx < ramsay.size(); ++rx) {
437                                 ramsay[rx]->restart();
438                         }
439                 } else {
440                         double *grad = fc->grad;
441                         double *ihess = fc->ihess;
442
443                         std::vector<double> move(numParam, 0.0);
444                         for (size_t px=0; px < fc->hgProd.size(); ++px) {
445                                 matrixVectorProdTerm &mvp = fc->hgProd[px];
446                                 move[mvp.dest] += ihess[mvp.hentry] * grad[mvp.gentry];
447                         }
448
449                         if (0) {
450                                 std::vector<double> blasMove(numParam); // uninit
451                                 const char uplo = 'U';
452                                 int dim = int(numParam);
453                                 int incx = 1;
454                                 double alpha = 1;
455                                 double beta = 0;
456                                 F77_CALL(dsymv)(&uplo, &dim, &alpha, fc->ihess, &dim,
457                                                 fc->grad, &incx, &beta, blasMove.data(), &incx);
458
459                                 double blasDiff = 0;
460                                 for (size_t px=0; px < numParam; ++px) {
461                                         blasDiff += fabs(move[px] - blasMove[px]);
462                                 }
463                                 mxLog("blasdiff %.10g", blasDiff);
464                                 if (blasDiff > 1) {
465                                         fc->log("N-R", FF_COMPUTE_GRADIENT|FF_COMPUTE_IHESSIAN|FF_COMPUTE_HGPROD);
466                                         pda(move.data(), 1, numParam);
467                                         pda(blasMove.data(), 1, numParam);
468                                         abort();
469                                 }
470                         }
471
472                         for (size_t px=0; px < numParam; ++px) {
473                                 Ramsay1975 *ramsay1 = ramsay[ fc->flavor[px] ];
474                                 double oldEst = fc->est[px];
475                                 double speed = 1 - ramsay1->caution;
476
477                                 ramsay1->recordEstimate(px, oldEst - speed * move[px]);
478
479                                 double badj = fabs(oldEst - fc->est[px]);
480                                 if (maxAdj < badj) {
481                                         maxAdj = badj;
482                                         maxAdjSigned = oldEst - fc->est[px];
483                                         maxAdjParam = px;
484                                         maxAdjFlavor = fc->flavor[px];
485                                 }
486                         }
487                         converged = maxAdj < tolerance;
488                 }
489
490                 fc->copyParamToModel(globalState);
491
492                 R_CheckUserInterrupt();
493
494                 if (converged || ++iter >= maxIter) break;
495         }
496
497         if (verbose >= 1) {
498                 if (converged) {
499                         mxLog("Newton-Raphson converged in %d cycles (max change %.12f)", iter, maxAdj);
500                 } else if (iter < maxIter) {
501                         mxLog("Newton-Raphson not improving on %.6f after %d cycles", bestLL, iter);
502                 } else if (iter == maxIter) {
503                         mxLog("Newton-Raphson failed to converge after %d cycles", iter);
504                 }
505         }
506
507         // better status reporting TODO
508         if (!converged && iter == maxIter) {
509                 if (bestLL > 0) {
510                         memcpy(fc->est, bestBackup.data(), sizeof(double) * numParam);
511                         fc->copyParamToModel(globalState);
512                 } else {
513                         omxRaiseErrorf(globalState, "Newton-Raphson failed to converge after %d cycles", iter);
514                 }
515         }
516
517         for (size_t rx=0; rx < ramsay.size(); ++rx) {
518                 delete ramsay[rx];
519         }
520 }
521
522 void ComputeNR::reportResults(FitContext *fc, MxRList *out)
523 {
524         if (Global->numIntervals) {
525                 warning("Confidence intervals are not implemented for Newton-Raphson");
526         }  
527
528         omxPopulateFitFunction(fitMatrix, out);
529
530         size_t numFree = varGroup->vars.size();
531
532         SEXP estimate;
533         PROTECT(estimate = allocVector(REALSXP, numFree));
534         memcpy(REAL(estimate), fc->est, sizeof(double) * numFree);
535
536         out->push_back(std::make_pair(mkChar("minimum"), ScalarReal(fc->fit)));
537         out->push_back(std::make_pair(mkChar("Minus2LogLikelihood"), ScalarReal(fc->fit)));
538         out->push_back(std::make_pair(mkChar("estimate"), estimate));
539
540         SEXP iterations;
541
542         // SEXP code;
543         // PROTECT(code = NEW_NUMERIC(1));
544         // REAL(code)[0] = inform;
545         // out->push_back(std::make_pair(mkChar("nr.code"), code));
546
547         PROTECT(iterations = NEW_NUMERIC(1));
548         REAL(iterations)[0] = iter;
549         out->push_back(std::make_pair(mkChar("nr.iterations"), iterations));
550 }