Allow ComputeIterate to test maximum absolute change
[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 ComputeNR::compute(FitContext *fc)
274 {
275         // complain if there are non-linear constraints TODO
276
277         size_t numParam = varGroup->vars.size();
278         if (numParam <= 0) {
279                 error("Model has no free parameters");
280                 return;
281         }
282
283         std::vector<Ramsay1975*> ramsay;
284         if (fitMatrix->fitFunction->parametersHaveFlavor) {
285                 for (size_t px=0; px < numParam; ++px) {
286                         fc->flavor[px] = -1;
287                 }
288                 omxFitFunctionCompute(fitMatrix->fitFunction, FF_COMPUTE_PARAMFLAVOR, fc);
289         } else {
290                 OMXZERO(fc->flavor, numParam);
291         }
292
293         for (size_t px=0; px < numParam; ++px) {
294                 // global namespace for flavors? TODO
295                 if (fc->flavor[px] < 0 || fc->flavor[px] > 100) {  // max flavor? TODO
296                         error("Invalid parameter flavor %d", fc->flavor[px]);
297                 }
298                 while (int(ramsay.size()) < fc->flavor[px]+1) {
299                         ramsay.push_back(new Ramsay1975(fc, fc->flavor[px], verbose));
300                 }
301         }
302
303         omxFitFunctionCompute(fitMatrix->fitFunction, FF_COMPUTE_PREOPTIMIZE, fc);
304         fc->maybeCopyParamToModel(globalState);
305
306         iter = 0;
307         int sinceRestart = 0;
308         bool converged = false;
309         bool approaching = false;
310         bool restarted = carefully;
311         double maxAdj = 0;
312         double maxAdjSigned = 0;
313         int maxAdjFlavor = 0;
314         int maxAdjParam = -1;
315         double bestLL = 0;
316
317         std::valarray<double> startBackup(fc->est, numParam);
318         std::vector<double> bestBackup(numParam);
319
320         fitMatrix->data[0] = 0;  // may not recompute it, don't leave stale data
321
322         if (verbose >= 2) {
323                 mxLog("Welcome to Newton-Raphson (tolerance %.3g, max iter %d, %ld flavors)",
324                       tolerance, maxIter, ramsay.size());
325         }
326         while (1) {
327                 if (verbose >= 2) {
328                         mxLog("Begin %d/%d iter of Newton-Raphson (prev maxAdj %.4f for %d, flavor %d)",
329                               iter+1, maxIter, maxAdjSigned, maxAdjParam, maxAdjFlavor);
330                 }
331
332                 int want = FF_COMPUTE_GRADIENT|FF_COMPUTE_IHESSIAN;
333                 if (restarted) want |= FF_COMPUTE_FIT;
334
335                 OMXZERO(fc->grad, numParam);
336                 OMXZERO(fc->ihess, numParam * numParam);
337
338                 omxFitFunctionCompute(fitMatrix->fitFunction, want, fc);
339                 if (isErrorRaised(globalState)) break;
340
341                 if (verbose >= 5) {
342                         int show = FF_COMPUTE_ESTIMATE;
343                         if (verbose >= 6) show |= FF_COMPUTE_GRADIENT|FF_COMPUTE_IHESSIAN;
344                         fc->log("Newton-Raphson", show);
345                 }
346
347                 double LL = fitMatrix->data[0];
348                 if (bestLL == 0 || bestLL > LL) {
349                         bestLL = LL;
350                         memcpy(bestBackup.data(), fc->est, sizeof(double) * numParam);
351                 }
352                 if (bestLL > 0) {
353                         if (verbose >= 3) mxLog("Newton-Raphson cantankerous fit %.5f (best %.5f)", LL, bestLL);
354                         if (approaching && (LL > bestLL + 0.5 || !std::isfinite(LL))) {  // arbitrary threshold
355                                 memcpy(fc->est, bestBackup.data(), sizeof(double) * numParam);
356                                 fc->copyParamToModel(globalState);
357                                 break;
358                         }
359                 }
360
361                 if (verbose >= 1 || OMX_DEBUG) {
362                         for (size_t h1=1; h1 < numParam; h1++) {
363                                 for (size_t h2=0; h2 < h1; h2++) {
364                                         if (fc->ihess[h2 * numParam + h1] == 0) continue;
365                                         omxRaiseErrorf(globalState, "Inverse Hessian is not upper triangular");
366                                 }
367                         }
368                 }
369
370                 if (0) {
371                         const double maxDiag = 4;
372                         for (size_t dx=0; dx < numParam; dx++) {
373                                 double mag = fabs(fc->ihess[dx * numParam + dx]);
374                                 if (maxDiag < mag) {
375                                         double old = fc->grad[dx];
376                                         double logBad = log(1 + mag - maxDiag);
377                                         fc->grad[dx] /= (1 + logBad);  // arbitrary guess
378                                         mxLog("ihess bad at diag %lu grad %.8g -> %.8g", dx, old, fc->grad[dx]);
379                                 }
380                         }
381                         //fc->log("bad", FF_COMPUTE_IHESSIAN);
382                 }
383
384                 bool restart = false;
385                 if ((++sinceRestart) % 3 == 0) {
386                         for (size_t rx=0; rx < ramsay.size(); ++rx) {
387                                 ramsay[rx]->recalibrate(&restart);
388                         }
389                 }
390                 for (size_t px=0; px < numParam; ++px) {
391                         if (!std::isfinite(fc->grad[px])) {
392                                 if (!restart) {
393                                         if (verbose >= 3) {
394                                                 mxLog("Newton-Raphson: grad[%lu] not finite, restart recommended", px);
395                                         }
396                                         restart = true;
397                                         break;
398                                 }
399                         }
400                 }
401                 if ((sinceRestart % 3) == 0 && !restart) {
402                         // 3 iterations without extreme oscillation or NaN gradients
403                         if (!approaching && verbose >= 2) {
404                                 mxLog("Newton-Raphson: Probably approaching solution");
405                         }
406                         approaching = true;
407                 }
408
409                 maxAdjParam = -1;
410                 maxAdj = 0;
411                 if (restart && (!approaching || !restarted)) {
412                         approaching = false;
413                         restarted = true;
414                         bestLL = 0;
415                         sinceRestart = 0;
416
417                         if (verbose >= 2) {
418                                 mxLog("Newton-Raphson: Increasing damping ...");
419                         }
420                         for (size_t px=0; px < numParam; ++px) {
421                                 fc->est[px] = startBackup[px];
422                         }
423                         for (size_t rx=0; rx < ramsay.size(); ++rx) {
424                                 ramsay[rx]->restart();
425                         }
426                 } else {
427                         double *grad = fc->grad;
428                         double *ihess = fc->ihess;
429                         for (size_t px=0; px < numParam; ++px) {
430                                 Ramsay1975 *ramsay1 = ramsay[ fc->flavor[px] ];
431                                 double oldEst = fc->est[px];
432                                 double move = 0;
433                                 for (size_t h1=0; h1 <= px; ++h1) {
434                                         move += ihess[px * numParam + h1] * grad[h1];
435                                 }
436                                 double *strip=ihess + (px+1) * numParam + px;
437                                 for (size_t h1=px+1; h1 < numParam; ++h1, strip += numParam) {
438                                         move += *strip * grad[h1];
439                                 }
440                                 double speed = 1 - ramsay1->caution;
441
442                                 ramsay1->recordEstimate(px, oldEst - speed * move);
443
444                                 double badj = fabs(oldEst - fc->est[px]);
445                                 if (maxAdj < badj) {
446                                         maxAdj = badj;
447                                         maxAdjSigned = oldEst - fc->est[px];
448                                         maxAdjParam = px;
449                                         maxAdjFlavor = fc->flavor[px];
450                                 }
451                         }
452                         converged = maxAdj < tolerance;
453                 }
454
455                 fc->copyParamToModel(globalState);
456
457                 R_CheckUserInterrupt();
458
459                 if (converged || ++iter >= maxIter) break;
460         }
461
462         if (verbose >= 1) {
463                 if (converged) {
464                         mxLog("Newton-Raphson converged in %d cycles (max change %.12f)", iter, maxAdj);
465                 } else if (iter < maxIter) {
466                         mxLog("Newton-Raphson not improving on %.6f after %d cycles", bestLL, iter);
467                 } else if (iter == maxIter) {
468                         mxLog("Newton-Raphson failed to converge after %d cycles", iter);
469                 }
470         }
471
472         // better status reporting TODO
473         if (!converged && iter == maxIter) {
474                 if (bestLL > 0) {
475                         memcpy(fc->est, bestBackup.data(), sizeof(double) * numParam);
476                         fc->copyParamToModel(globalState);
477                 } else {
478                         omxRaiseErrorf(globalState, "Newton-Raphson failed to converge after %d cycles", iter);
479                 }
480         }
481
482         for (size_t rx=0; rx < ramsay.size(); ++rx) {
483                 delete ramsay[rx];
484         }
485 }
486
487 void ComputeNR::reportResults(FitContext *fc, MxRList *out)
488 {
489         if (Global->numIntervals) {
490                 warning("Confidence intervals are not implemented for Newton-Raphson");
491         }  
492
493         omxPopulateFitFunction(fitMatrix, out);
494
495         size_t numFree = varGroup->vars.size();
496
497         SEXP estimate;
498         PROTECT(estimate = allocVector(REALSXP, numFree));
499         memcpy(REAL(estimate), fc->est, sizeof(double) * numFree);
500
501         out->push_back(std::make_pair(mkChar("minimum"), ScalarReal(fc->fit)));
502         out->push_back(std::make_pair(mkChar("Minus2LogLikelihood"), ScalarReal(fc->fit)));
503         out->push_back(std::make_pair(mkChar("estimate"), estimate));
504
505         SEXP iterations;
506
507         // SEXP code;
508         // PROTECT(code = NEW_NUMERIC(1));
509         // REAL(code)[0] = inform;
510         // out->push_back(std::make_pair(mkChar("nr.code"), code));
511
512         PROTECT(iterations = NEW_NUMERIC(1));
513         REAL(iterations)[0] = iter;
514         out->push_back(std::make_pair(mkChar("nr.iterations"), iterations));
515 }