Move freeVars stuff out of omxState
[openmx:openmx.git] / src / omxHessianCalculation.cpp
1 /*
2  *  Copyright 2007-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 /**
18  * Based on:
19  *
20  * Paul Gilbert and Ravi Varadhan (2012). numDeriv: Accurate Numerical Derivatives. R package
21  * version 2012.9-1. http://CRAN.R-project.org/package=numDeriv
22  *
23  **/
24
25 #include <stdio.h>
26 #include <sys/types.h>
27 #include <errno.h>
28
29 #include <R.h>
30 #include <Rinternals.h>
31 #include <Rdefines.h>
32 #include <R_ext/Rdynload.h>
33 #include <R_ext/BLAS.h>
34 #include <R_ext/Lapack.h>
35
36 #include "omxDefines.h"
37 #include "npsolWrap.h"
38 #include "omxState.h"
39 #include "omxMatrix.h"
40 #include "omxAlgebra.h"
41 #include "omxFitFunction.h"
42 #include "omxNPSOLSpecific.h"
43 #include "omxOptimizer.h"
44 #include "omxOpenmpWrap.h"
45 #include "omxExportBackendState.h"
46 #include "Compute.h"
47
48 class omxComputeEstimatedHessian : public omxCompute {
49         double stepSize;
50         int numIter;
51         bool wantSE;
52
53         omxMatrix *fitMat;
54         double minimum;
55         int numParams;
56         double *optima;
57         double *gradient;
58         double *hessian;
59
60         SEXP calculatedHessian;
61         SEXP stdErrors;
62
63         void init();
64         void omxPopulateHessianWork(struct hess_struct *hess_work, omxState* state);
65         void omxEstimateHessianOnDiagonal(int i, struct hess_struct* hess_work);
66         void omxEstimateHessianOffDiagonal(int i, int l, struct hess_struct* hess_work);
67         void doHessianCalculation(int numChildren, struct hess_struct *hess_work);
68
69  public:
70         omxComputeEstimatedHessian();
71         virtual void initFromFrontend(SEXP rObj);
72         virtual void compute(double *at);
73         virtual void reportResults(MxRList *out);
74         virtual double getFit() { return 0; }
75         virtual double *getEstimate() { return optima; }
76 };
77
78 struct hess_struct {
79         double* freeParams;
80         double* Haprox;
81         double* Gaprox;
82         omxMatrix* fitMatrix;
83 };
84
85 void omxComputeEstimatedHessian::omxPopulateHessianWork(struct hess_struct *hess_work, omxState* state)
86 {
87         double *freeParams = (double*) Calloc(numParams, double);
88
89         hess_work->Haprox = (double*) Calloc(numIter, double);          // Hessian Workspace
90         hess_work->Gaprox = (double*) Calloc(numIter, double);          // Gradient Workspace
91         hess_work->freeParams = freeParams;
92         for(int i = 0; i < numParams; i++) {
93                 freeParams[i] = optima[i];
94         }
95
96         hess_work->fitMatrix = omxLookupDuplicateElement(state, fitMat);
97
98         handleFreeVarListHelper(state, freeParams, numParams);
99 }
100
101 /**
102   @params i              parameter number
103   @params hess_work      local copy
104   @params optima         shared read-only variable
105   @params gradient       shared write-only variable
106   @params hessian        shared write-only variable
107  */
108 void omxComputeEstimatedHessian::omxEstimateHessianOnDiagonal(int i, struct hess_struct* hess_work)
109 {
110         static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
111         static const double eps = 1E-4; // Kept here for access purposes.
112
113         double *Haprox             = hess_work->Haprox;
114         double *Gaprox             = hess_work->Gaprox;
115         double *freeParams         = hess_work->freeParams;
116         omxMatrix* fitMatrix = hess_work->fitMatrix; 
117
118         /* Part the first: Gradient and diagonal */
119         double iOffset = fabs(stepSize * optima[i]);
120         if(fabs(iOffset) < eps) iOffset += eps;
121         if(OMX_DEBUG) {mxLog("Hessian estimation: iOffset: %f.", iOffset);}
122         for(int k = 0; k < numIter; k++) {                      // Decreasing step size, starting at k == 0
123                 if(OMX_DEBUG) {mxLog("Hessian estimation: Parameter %d at refinement level %d (%f). One Step Forward.", i, k, iOffset);}
124                 freeParams[i] = optima[i] + iOffset;
125
126                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
127
128                 omxRecompute(fitMatrix);
129                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
130
131                 if(OMX_DEBUG) {mxLog("Hessian estimation: One Step Back.");}
132
133                 freeParams[i] = optima[i] - iOffset;
134
135                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
136
137                 omxRecompute(fitMatrix);
138                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
139
140                 Gaprox[k] = (f1 - f2) / (2.0*iOffset);                                          // This is for the gradient
141                 Haprox[k] = (f1 - 2.0 * minimum + f2) / (iOffset * iOffset);            // This is second derivative
142                 freeParams[i] = optima[i];                                                                      // Reset parameter value
143                 iOffset /= v;
144                 if(OMX_DEBUG) {mxLog("Hessian estimation: (%d, %d)--Calculating F1: %f F2: %f, Haprox: %f...", i, i, f1, f2, Haprox[k]);}
145         }
146
147         for(int m = 1; m < numIter; m++) {                                              // Richardson Step
148                 for(int k = 0; k < (numIter - m); k++) {
149                         Gaprox[k] = (Gaprox[k+1] * pow(4.0, m) - Gaprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
150                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
151                 }
152         }
153
154         if(OMX_DEBUG) { mxLog("Hessian estimation: Populating Hessian (0x%x) at ([%d, %d] = %d) with value %f...", hessian, i, i, i*numParams+i, Haprox[0]); }
155         gradient[i] = Gaprox[0];                                                // NPSOL reports a gradient that's fine.  Why report two?
156         hessian[i*numParams + i] = Haprox[0];
157
158         if(OMX_DEBUG) {mxLog("Done with parameter %d.", i);}
159
160 }
161
162 void omxComputeEstimatedHessian::omxEstimateHessianOffDiagonal(int i, int l, struct hess_struct* hess_work)
163 {
164     static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
165     static const double eps = 1E-4; // Kept here for access purposes.
166
167         double *Haprox             = hess_work->Haprox;
168         double *freeParams         = hess_work->freeParams;
169         omxMatrix* fitMatrix = hess_work->fitMatrix; 
170
171         double iOffset = fabs(stepSize*optima[i]);
172         if(fabs(iOffset) < eps) iOffset += eps;
173         double lOffset = fabs(stepSize*optima[l]);
174         if(fabs(lOffset) < eps) lOffset += eps;
175
176         for(int k = 0; k < numIter; k++) {
177                 freeParams[i] = optima[i] + iOffset;
178                 freeParams[l] = optima[l] + lOffset;
179
180                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
181
182                 omxRecompute(fitMatrix);
183                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
184
185                 if(OMX_DEBUG) {mxLog("Hessian estimation: One Step Back.");}
186
187                 freeParams[i] = optima[i] - iOffset;
188                 freeParams[l] = optima[l] - lOffset;
189
190                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
191
192                 omxRecompute(fitMatrix);
193                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
194
195                 Haprox[k] = (f1 - 2.0 * minimum + f2 - hessian[i*numParams+i]*iOffset*iOffset -
196                                                 hessian[l*numParams+l]*lOffset*lOffset)/(2.0*iOffset*lOffset);
197                 if(OMX_DEBUG) {mxLog("Hessian first off-diagonal calculation: Haprox = %f, iOffset = %f, lOffset=%f from params %f, %f and %f, %f and %d (also: %f, %f and %f).", Haprox[k], iOffset, lOffset, f1, hessian[i*numParams+i], hessian[l*numParams+l], v, k, pow(v, k), stepSize*optima[i], stepSize*optima[l]);}
198
199                 freeParams[i] = optima[i];                              // Reset parameter values
200                 freeParams[l] = optima[l];
201
202                 iOffset = iOffset / v;                                  //  And shrink step
203                 lOffset = lOffset / v;
204         }
205
206         for(int m = 1; m < numIter; m++) {                                              // Richardson Step
207                 for(int k = 0; k < (numIter - m); k++) {
208                         if(OMX_DEBUG) {mxLog("Hessian off-diagonal calculation: Haprox = %f, iOffset = %f, lOffset=%f from params %f, %f and %f, %f and %d (also: %f, %f and %f, and %f).", Haprox[k], iOffset, lOffset, stepSize, optima[i], optima[l], v, m, pow(4.0, m), stepSize*optima[i], stepSize*optima[l], k);}
209                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k]) / (pow(4.0, m)-1);
210                 }
211         }
212
213         if(OMX_DEBUG) {mxLog("Hessian estimation: Populating Hessian (0x%x) at ([%d, %d] = %d and %d) with value %f...", hessian, i, l, i*numParams+l, l*numParams+i, Haprox[0]);}
214         hessian[i*numParams+l] = Haprox[0];
215         hessian[l*numParams+i] = Haprox[0];
216
217 }
218
219 void omxComputeEstimatedHessian::doHessianCalculation(int numChildren, struct hess_struct *hess_work)
220 {
221         int i,j;
222
223         int numOffDiagonal = (numParams * (numParams - 1)) / 2;
224         int *diags = Calloc(numOffDiagonal, int);
225         int *offDiags = Calloc(numOffDiagonal, int);
226         int offset = 0;
227         // gcc does not detect the usage of the following variable
228         // in the omp parallel pragma, and marks the variable as
229         // unused, so the attribute is placed to silence the warning.
230     int __attribute__((unused)) parallelism = (numChildren == 0) ? 1 : numChildren;
231
232         // There must be a way to avoid constructing the
233         // diags and offDiags arrays and replace them with functions
234         // that produce these values given the input
235         /// {0, 1, ..., numOffDiagonal - 1} -- M. Spiegel
236         for(i = 0; i < numParams; i++) {
237                 for(j = i - 1; j >= 0; j--) {
238                         diags[offset] = i;
239                         offDiags[offset] = j;
240                         offset++;
241                 }
242         }
243
244         #pragma omp parallel for num_threads(parallelism) 
245         for(i = 0; i < numParams; i++) {
246                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
247                 omxEstimateHessianOnDiagonal(i, hess_work + threadId);
248         }
249
250         #pragma omp parallel for num_threads(parallelism) 
251         for(offset = 0; offset < numOffDiagonal; offset++) {
252                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
253                 omxEstimateHessianOffDiagonal(diags[offset], offDiags[offset],
254                         hess_work + threadId);
255         }
256
257         Free(diags);
258         Free(offDiags);
259 }
260
261 void omxComputeEstimatedHessian::init()
262 {
263         stepSize = .0001;
264         numIter = 4;
265         stdErrors = NULL;
266         optima = NULL;
267 }
268
269 omxComputeEstimatedHessian::omxComputeEstimatedHessian()
270 {
271         init();
272 }
273
274 void omxComputeEstimatedHessian::initFromFrontend(SEXP rObj)
275 {
276         numParams = Global.numFreeParams;
277         if (numParams <= 0) error("Model has no free parameters");
278
279         fitMat = omxNewMatrixFromSlot(rObj, globalState, "fitfunction");
280
281         SEXP slotValue;
282         PROTECT(slotValue = GET_SLOT(rObj, install("se")));
283         wantSE = asLogical(slotValue);
284         UNPROTECT(1);
285 }
286
287 void omxComputeEstimatedHessian::compute(double *at)
288 {
289         omxFitFunctionCreateChildren(globalState);
290
291         numParams = Global.numFreeParams;
292
293         optima = at;
294
295         PROTECT(calculatedHessian = allocMatrix(REALSXP, numParams, numParams));
296
297         // TODO: Check for nonlinear constraints and adjust algorithm accordingly.
298         // TODO: Allow more than one hessian value for calculation
299
300         int numChildren = Global.numChildren;
301
302         omxRecompute(fitMat);
303         minimum = omxMatrixElement(fitMat, 0, 0);
304
305         struct hess_struct* hess_work;
306         if (numChildren < 2) {
307                 hess_work = Calloc(1, struct hess_struct);
308                 omxPopulateHessianWork(hess_work, globalState);
309         } else {
310                 hess_work = Calloc(numChildren, struct hess_struct);
311                 for(int i = 0; i < numChildren; i++) {
312                         omxPopulateHessianWork(hess_work + i, globalState->childList[i]);
313                 }
314         }
315         if(OMX_DEBUG) mxLog("Hessian Calculation using %d children", numChildren);
316
317         hessian = REAL(calculatedHessian);
318
319         gradient = (double*) R_alloc(numParams, sizeof(double));
320   
321         doHessianCalculation(numChildren, hess_work);
322
323         if(OMX_DEBUG) {mxLog("Hessian Computation complete.");}
324
325         if (numChildren < 2) {
326                 Free(hess_work->Haprox);
327                 Free(hess_work->Gaprox);
328                 Free(hess_work->freeParams);
329             Free(hess_work);
330         } else {
331                 for(int i = 0; i < numChildren; i++) {
332                         Free((hess_work + i)->Haprox);
333                         Free((hess_work + i)->Gaprox);
334                         Free((hess_work + i)->freeParams);
335                 }
336                 Free(hess_work);
337         }
338
339         if (wantSE) {
340                 // This function calculates the standard errors from the hessian matrix
341                 // sqrt(diag(solve(hessian)))
342
343                 const double scale = 2;
344                 double* workspace = (double *) Calloc(numParams * numParams, double);
345         
346                 for(int i = 0; i < numParams; i++)
347                         for(int j = 0; j <= i; j++)
348                                 workspace[i*numParams+j] = hessian[i*numParams+j];              // Populate upper triangle
349         
350                 char u = 'U';
351                 std::vector<int> ipiv(numParams);
352                 int lwork = -1;
353                 double temp;
354                 int info = 0;
355         
356                 F77_CALL(dsytrf)(&u, &numParams, workspace, &numParams, ipiv.data(), &temp, &lwork, &info);
357         
358                 lwork = (temp > numParams?temp:numParams);
359         
360                 double* work = (double*) Calloc(lwork, double);
361         
362                 F77_CALL(dsytrf)(&u, &numParams, workspace, &numParams, ipiv.data(), work, &lwork, &info);
363         
364                 if(info != 0) {
365                         // report error TODO
366                 } else {
367                         F77_CALL(dsytri)(&u, &numParams, workspace, &numParams, ipiv.data(), work, &info);
368         
369                         if(info != 0) {
370                                 // report error TODO
371                         } else {
372                                 PROTECT(stdErrors = allocMatrix(REALSXP, numParams, 1));
373                                 double* stdErr = REAL(stdErrors);
374                                 for(int i = 0; i < numParams; i++) {
375                                         stdErr[i] = sqrt(scale) * sqrt(workspace[i * numParams + i]);
376                                 }
377                         }
378                 }
379         
380                 Free(workspace);
381                 Free(work);
382         }
383
384         omxFreeChildStates(globalState);
385 }
386
387 void omxComputeEstimatedHessian::reportResults(MxRList *result)
388 {
389         result->push_back(std::make_pair(mkChar("calculatedHessian"), calculatedHessian));
390
391         if (stdErrors) {
392                 result->push_back(std::make_pair(mkChar("standardErrors"), stdErrors));
393         }
394 }
395
396 omxCompute *newComputeEstimatedHessian()
397 {
398         if (globalState->numConstraints != 0) {
399                 error("Cannot compute estimated Hessian with constraints (%d constraints found)",
400                       globalState->numConstraints);
401         }
402         return new omxComputeEstimatedHessian;
403 }
404