omxComputeEstimateHessian reorg 10/11
[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 "omxHessianCalculation.h"
46 #include "omxGlobalState.h"
47 #include "omxExportBackendState.h"
48
49 struct hess_struct {
50         double* freeParams;
51         double* Haprox;
52         double* Gaprox;
53         omxMatrix* fitMatrix;
54         double  f0;
55 };
56
57 void omxComputeEstimateHessian::omxPopulateHessianWork(struct hess_struct *hess_work, omxState* state)
58 {
59         omxFitFunction* oo = state->fitMatrix->fitFunction;
60
61         double *freeParams = (double*) Calloc(numParams, double);
62
63         hess_work->Haprox = (double*) Calloc(numIter, double);          // Hessian Workspace
64         hess_work->Gaprox = (double*) Calloc(numIter, double);          // Gradient Workspace
65         hess_work->freeParams = freeParams;
66         for(int i = 0; i < numParams; i++) {
67                 freeParams[i] = optima[i];
68         }
69
70         omxMatrix *fitMatrix = oo->matrix;
71         hess_work->fitMatrix = fitMatrix;
72
73         handleFreeVarListHelper(state, freeParams, numParams);
74
75         omxRecompute(fitMatrix);                // Initial recompute in case it matters.        
76         hess_work->f0 = omxMatrixElement(fitMatrix, 0, 0);
77 }
78
79 /**
80   @params i              parameter number
81   @params hess_work      local copy
82   @params optima         shared read-only variable
83   @params gradient       shared write-only variable
84   @params hessian        shared write-only variable
85  */
86 void omxComputeEstimateHessian::omxEstimateHessianOnDiagonal(int i, struct hess_struct* hess_work)
87 {
88         static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
89         static const double eps = 1E-4; // Kept here for access purposes.
90
91         double *Haprox             = hess_work->Haprox;
92         double *Gaprox             = hess_work->Gaprox;
93         double *freeParams         = hess_work->freeParams;
94         omxMatrix* fitMatrix = hess_work->fitMatrix; 
95         double f0                  = hess_work->f0;                // read-only
96
97         /* Part the first: Gradient and diagonal */
98         double iOffset = fabs(stepSize * optima[i]);
99         if(fabs(iOffset) < eps) iOffset += eps;
100         if(OMX_DEBUG) {Rprintf("Hessian estimation: iOffset: %f.\n", iOffset);}
101         for(int k = 0; k < numIter; k++) {                      // Decreasing step size, starting at k == 0
102                 if(OMX_DEBUG) {Rprintf("Hessian estimation: Parameter %d at refinement level %d (%f). One Step Forward.\n", i, k, iOffset);}
103                 freeParams[i] = optima[i] + iOffset;
104
105                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
106
107                 omxRecompute(fitMatrix);
108                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
109
110                 if(OMX_DEBUG) {Rprintf("Hessian estimation: One Step Back.\n");}
111
112                 freeParams[i] = optima[i] - iOffset;
113
114                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
115
116                 omxRecompute(fitMatrix);
117                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
118
119                 Gaprox[k] = (f1 - f2) / (2.0*iOffset);                                          // This is for the gradient
120                 Haprox[k] = (f1 - 2.0 * f0 + f2) / (iOffset * iOffset);         // This is second derivative
121                 freeParams[i] = optima[i];                                                                      // Reset parameter value
122                 iOffset /= v;
123                 if(OMX_DEBUG) {Rprintf("Hessian estimation: (%d, %d)--Calculating F1: %f F2: %f, Haprox: %f...\n", i, i, f1, f2, Haprox[k]);}
124         }
125
126         for(int m = 1; m < numIter; m++) {                                              // Richardson Step
127                 for(int k = 0; k < (numIter - m); k++) {
128                         Gaprox[k] = (Gaprox[k+1] * pow(4.0, m) - Gaprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
129                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
130                 }
131         }
132
133         if(OMX_DEBUG) { Rprintf("Hessian estimation: Populating Hessian (0x%x) at ([%d, %d] = %d) with value %f...\n", hessian, i, i, i*numParams+i, Haprox[0]); }
134         gradient[i] = Gaprox[0];                                                // NPSOL reports a gradient that's fine.  Why report two?
135         hessian[i*numParams + i] = Haprox[0];
136
137         if(OMX_DEBUG) {Rprintf("Done with parameter %d.\n", i);}
138
139 }
140
141 void omxComputeEstimateHessian::omxEstimateHessianOffDiagonal(int i, int l, struct hess_struct* hess_work)
142 {
143     static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
144     static const double eps = 1E-4; // Kept here for access purposes.
145
146         double *Haprox             = hess_work->Haprox;
147         double *freeParams         = hess_work->freeParams;
148         omxMatrix* fitMatrix = hess_work->fitMatrix; 
149         double f0                  = hess_work->f0;                // read-only
150
151         double iOffset = fabs(stepSize*optima[i]);
152         if(fabs(iOffset) < eps) iOffset += eps;
153         double lOffset = fabs(stepSize*optima[l]);
154         if(fabs(lOffset) < eps) lOffset += eps;
155
156         for(int k = 0; k < numIter; k++) {
157                 freeParams[i] = optima[i] + iOffset;
158                 freeParams[l] = optima[l] + lOffset;
159
160                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
161
162                 omxRecompute(fitMatrix);
163                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
164
165                 if(OMX_DEBUG) {Rprintf("Hessian estimation: One Step Back.\n");}
166
167                 freeParams[i] = optima[i] - iOffset;
168                 freeParams[l] = optima[l] - lOffset;
169
170                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
171
172                 omxRecompute(fitMatrix);
173                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
174
175                 Haprox[k] = (f1 - 2.0 * f0 + f2 - hessian[i*numParams+i]*iOffset*iOffset -
176                                                 hessian[l*numParams+l]*lOffset*lOffset)/(2.0*iOffset*lOffset);
177                 if(OMX_DEBUG) {Rprintf("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).\n", Haprox[k], iOffset, lOffset, f1, hessian[i*numParams+i], hessian[l*numParams+l], v, k, pow(v, k), stepSize*optima[i], stepSize*optima[l]);}
178
179                 freeParams[i] = optima[i];                              // Reset parameter values
180                 freeParams[l] = optima[l];
181
182                 iOffset = iOffset / v;                                  //  And shrink step
183                 lOffset = lOffset / v;
184         }
185
186         for(int m = 1; m < numIter; m++) {                                              // Richardson Step
187                 for(int k = 0; k < (numIter - m); k++) {
188                         if(OMX_DEBUG) {Rprintf("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).\n", Haprox[k], iOffset, lOffset, stepSize, optima[i], optima[l], v, m, pow(4.0, m), stepSize*optima[i], stepSize*optima[l], k);}
189                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k]) / (pow(4.0, m)-1);
190                 }
191         }
192
193         if(OMX_DEBUG) {Rprintf("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]);}
194         hessian[i*numParams+l] = Haprox[0];
195         hessian[l*numParams+i] = Haprox[0];
196
197 }
198
199 void omxComputeEstimateHessian::doHessianCalculation(int numChildren, struct hess_struct *hess_work)
200 {
201         int i,j;
202
203         int numOffDiagonal = (numParams * (numParams - 1)) / 2;
204         int *diags = Calloc(numOffDiagonal, int);
205         int *offDiags = Calloc(numOffDiagonal, int);
206         int offset = 0;
207         // gcc does not detect the usage of the following variable
208         // in the omp parallel pragma, and marks the variable as
209         // unused, so the attribute is placed to silence the warning.
210     int __attribute__((unused)) parallelism = (numChildren == 0) ? 1 : numChildren;
211
212         // There must be a way to avoid constructing the
213         // diags and offDiags arrays and replace them with functions
214         // that produce these values given the input
215         /// {0, 1, ..., numOffDiagonal - 1} -- M. Spiegel
216         for(i = 0; i < numParams; i++) {
217                 for(j = i - 1; j >= 0; j--) {
218                         diags[offset] = i;
219                         offDiags[offset] = j;
220                         offset++;
221                 }
222         }
223
224         #pragma omp parallel for num_threads(parallelism) 
225         for(i = 0; i < numParams; i++) {
226                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
227                 omxEstimateHessianOnDiagonal(i, hess_work + threadId);
228         }
229
230         #pragma omp parallel for num_threads(parallelism) 
231         for(offset = 0; offset < numOffDiagonal; offset++) {
232                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
233                 omxEstimateHessianOffDiagonal(diags[offset], offDiags[offset],
234                         hess_work + threadId);
235         }
236
237         Free(diags);
238         Free(offDiags);
239 }
240
241 omxComputeEstimateHessian::omxComputeEstimateHessian() :
242         stepSize(.0001),
243         numIter(4),
244         stdErrors(NULL)
245 {
246 }
247
248 omxComputeEstimateHessian::~omxComputeEstimateHessian()
249 {
250 }
251
252 class omxCompute *newComputeEstimateHessian()
253 {
254         return new omxComputeEstimateHessian;
255 }
256
257 void omxComputeEstimateHessian::compute()
258 {
259         optima = globalState->optimalValues;
260         numParams = globalState->numFreeParams;
261
262         PROTECT(calculatedHessian = allocMatrix(REALSXP, numParams, numParams));
263
264         // TODO: Check for nonlinear constraints and adjust algorithm accordingly.
265         // TODO: Allow more than one hessian value for calculation
266
267         int numChildren = globalState->numChildren;
268
269         struct hess_struct* hess_work;
270         if (numChildren < 2) {
271                 hess_work = Calloc(1, struct hess_struct);
272                 omxPopulateHessianWork(hess_work, globalState);
273         } else {
274                 hess_work = Calloc(numChildren, struct hess_struct);
275                 for(int i = 0; i < numChildren; i++) {
276                         omxPopulateHessianWork(hess_work + i, globalState->childList[i]);
277                 }
278         }
279         if(OMX_DEBUG) Rprintf("Hessian Calculation using %d children\n", numChildren);
280
281         hessian = REAL(calculatedHessian);
282
283         gradient = (double*) R_alloc(numParams, sizeof(double));
284   
285         doHessianCalculation(numChildren, hess_work);
286
287         if(OMX_DEBUG) {Rprintf("Hessian Computation complete.\n");}
288
289         if (numChildren < 2) {
290                 Free(hess_work->Haprox);
291                 Free(hess_work->Gaprox);
292                 Free(hess_work->freeParams);
293             Free(hess_work);
294         } else {
295                 for(int i = 0; i < numChildren; i++) {
296                         Free((hess_work + i)->Haprox);
297                         Free((hess_work + i)->Gaprox);
298                         Free((hess_work + i)->freeParams);
299                 }
300                 Free(hess_work);
301         }
302
303         if (globalState->calculateStdErrors) {
304                 // This function calculates the standard errors from the hessian matrix
305                 // sqrt(diag(solve(hessian)))
306
307                 const double scale = 2;
308                 double* workspace = (double *) Calloc(numParams * numParams, double);
309         
310                 for(int i = 0; i < numParams; i++)
311                         for(int j = 0; j <= i; j++)
312                                 workspace[i*numParams+j] = hessian[i*numParams+j];              // Populate upper triangle
313         
314                 char u = 'U';
315                 int *ipiv = new int[numParams];
316                 int lwork = -1;
317                 double temp;
318                 int info = 0;
319         
320                 F77_CALL(dsytrf)(&u, &numParams, workspace, &numParams, ipiv, &temp, &lwork, &info);
321         
322                 lwork = (temp > numParams?temp:numParams);
323         
324                 double* work = (double*) Calloc(lwork, double);
325         
326                 F77_CALL(dsytrf)(&u, &numParams, workspace, &numParams, ipiv, work, &lwork, &info);
327         
328                 if(info != 0) {
329                         // report error TODO
330                 } else {
331                         F77_CALL(dsytri)(&u, &numParams, workspace, &numParams, ipiv, work, &info);
332         
333                         if(info != 0) {
334                                 // report error TODO
335                         } else {
336                                 PROTECT(stdErrors = allocMatrix(REALSXP, numParams, 1));
337                                 double* stdErr = REAL(stdErrors);
338                                 for(int i = 0; i < numParams; i++) {
339                                         stdErr[i] = sqrt(scale) * sqrt(workspace[i * numParams + i]);
340                                 }
341                         }
342                 }
343         
344                 delete ipiv;
345                 Free(workspace);
346                 Free(work);
347         }
348 }
349
350 void omxComputeEstimateHessian::reportResults(MxRList *result)
351 {
352         result->push_back(std::make_pair(mkChar("calculatedHessian"), calculatedHessian));
353
354         if (stdErrors) {
355                 result->push_back(std::make_pair(mkChar("standardErrors"), stdErrors));
356         }
357 }