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