Enable R_NO_REMAP for a cleaner namespace
[openmx:openmx.git] / src / omxHessianCalculation.cpp
1 /*
2  *  Copyright 2007-2014 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 "omxDefines.h"
30 #include "glue.h"
31 #include "omxState.h"
32 #include "omxMatrix.h"
33 #include "omxAlgebra.h"
34 #include "omxFitFunction.h"
35 #include "omxNPSOLSpecific.h"
36 #include "omxOpenmpWrap.h"
37 #include "omxExportBackendState.h"
38 #include "Compute.h"
39 #include "matrix.h"
40 #include "omxBuffer.h"
41
42 class omxComputeNumericDeriv : public omxCompute {
43         typedef omxCompute super;
44         double stepSize;
45         int numIter;
46         bool parallel;
47         int totalProbeCount;
48
49         FitContext *fitContext;
50         omxMatrix *fitMat;
51         double minimum;
52         int numParams;
53         double *optima;
54         double *gradient;
55         double *hessian;
56
57         void init();
58         void omxPopulateHessianWork(struct hess_struct *hess_work, omxState* state);
59         void omxEstimateHessianOnDiagonal(int i, struct hess_struct* hess_work);
60         void omxEstimateHessianOffDiagonal(int i, int l, struct hess_struct* hess_work);
61         void doHessianCalculation(int numChildren, struct hess_struct *hess_work);
62
63  public:
64         omxComputeNumericDeriv();
65         virtual void initFromFrontend(SEXP rObj);
66         virtual void computeImpl(FitContext *fc);
67         virtual void reportResults(FitContext *fc, MxRList *slots, MxRList *out);
68 };
69
70 struct hess_struct {
71         int probeCount;
72         double* freeParams;
73         double* Haprox;
74         double* Gaprox;
75         omxMatrix* fitMatrix;
76 };
77
78 void omxComputeNumericDeriv::omxPopulateHessianWork(struct hess_struct *hess_work, omxState* state)
79 {
80         double *freeParams = (double*) Calloc(numParams, double);
81
82         hess_work->Haprox = (double*) Calloc(numIter, double);          // Hessian Workspace
83         hess_work->Gaprox = (double*) Calloc(numIter, double);          // Gradient Workspace
84         hess_work->freeParams = freeParams;
85         for(int i = 0; i < numParams; i++) {
86                 freeParams[i] = optima[i];
87         }
88
89         hess_work->fitMatrix = omxLookupDuplicateElement(state, fitMat);
90 }
91
92 /**
93   @params i              parameter number
94   @params hess_work      local copy
95   @params optima         shared read-only variable
96   @params gradient       shared write-only variable
97   @params hessian        shared write-only variable
98  */
99 void omxComputeNumericDeriv::omxEstimateHessianOnDiagonal(int i, struct hess_struct* hess_work)
100 {
101         static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
102         static const double eps = 1E-4; // Kept here for access purposes.
103
104         double *Haprox             = hess_work->Haprox;
105         double *Gaprox             = hess_work->Gaprox;
106         double *freeParams         = hess_work->freeParams;
107         omxMatrix* fitMatrix = hess_work->fitMatrix; 
108
109         /* Part the first: Gradient and diagonal */
110         double iOffset = fabs(stepSize * optima[i]);
111         if(fabs(iOffset) < eps) iOffset += eps;
112         if(OMX_DEBUG) {mxLog("Hessian estimation: iOffset: %f.", iOffset);}
113         for(int k = 0; k < numIter; k++) {                      // Decreasing step size, starting at k == 0
114                 if(OMX_DEBUG) {mxLog("Hessian estimation: Parameter %d at refinement level %d (%f). One Step Forward.", i, k, iOffset);}
115                 freeParams[i] = optima[i] + iOffset;
116
117                 
118                 fitContext->copyParamToModel(fitMatrix, freeParams);
119
120                 ++hess_work->probeCount;
121                 omxRecompute(fitMatrix);
122                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
123
124                 if(OMX_DEBUG) {mxLog("Hessian estimation: One Step Back.");}
125
126                 freeParams[i] = optima[i] - iOffset;
127
128                 fitContext->copyParamToModel(fitMatrix, freeParams);
129
130                 ++hess_work->probeCount;
131                 omxRecompute(fitMatrix);
132                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
133
134                 Gaprox[k] = (f1 - f2) / (2.0*iOffset);                                          // This is for the gradient
135                 Haprox[k] = (f1 - 2.0 * minimum + f2) / (iOffset * iOffset);            // This is second derivative
136                 freeParams[i] = optima[i];                                                                      // Reset parameter value
137                 iOffset /= v;
138                 if(OMX_DEBUG) {mxLog("Hessian estimation: (%d, %d)--Calculating F1: %f F2: %f, Haprox: %f...", i, i, f1, f2, Haprox[k]);}
139         }
140
141         for(int m = 1; m < numIter; m++) {                                              // Richardson Step
142                 for(int k = 0; k < (numIter - m); k++) {
143                         Gaprox[k] = (Gaprox[k+1] * pow(4.0, m) - Gaprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
144                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
145                 }
146         }
147
148         if(OMX_DEBUG) { mxLog("Hessian estimation: Populating Hessian ([%d, %d] = %d) with value %f...", i, i, i*numParams+i, Haprox[0]); }
149         gradient[i] = Gaprox[0];                                                // NPSOL reports a gradient that's fine.  Why report two?
150         hessian[i*numParams + i] = Haprox[0];
151
152         if(OMX_DEBUG) {mxLog("Done with parameter %d.", i);}
153
154 }
155
156 void omxComputeNumericDeriv::omxEstimateHessianOffDiagonal(int i, int l, struct hess_struct* hess_work)
157 {
158     static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
159     static const double eps = 1E-4; // Kept here for access purposes.
160
161         double *Haprox             = hess_work->Haprox;
162         double *freeParams         = hess_work->freeParams;
163         omxMatrix* fitMatrix = hess_work->fitMatrix; 
164
165         double iOffset = fabs(stepSize*optima[i]);
166         if(fabs(iOffset) < eps) iOffset += eps;
167         double lOffset = fabs(stepSize*optima[l]);
168         if(fabs(lOffset) < eps) lOffset += eps;
169
170         for(int k = 0; k < numIter; k++) {
171                 freeParams[i] = optima[i] + iOffset;
172                 freeParams[l] = optima[l] + lOffset;
173
174                 fitContext->copyParamToModel(fitMatrix, freeParams);
175
176                 ++hess_work->probeCount;
177                 omxRecompute(fitMatrix);
178                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
179
180                 if(OMX_DEBUG) {mxLog("Hessian estimation: One Step Back.");}
181
182                 freeParams[i] = optima[i] - iOffset;
183                 freeParams[l] = optima[l] - lOffset;
184
185                 fitContext->copyParamToModel(fitMatrix, freeParams);
186
187                 ++hess_work->probeCount;
188                 omxRecompute(fitMatrix);
189                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
190
191                 Haprox[k] = (f1 - 2.0 * minimum + f2 - hessian[i*numParams+i]*iOffset*iOffset -
192                                                 hessian[l*numParams+l]*lOffset*lOffset)/(2.0*iOffset*lOffset);
193                 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]);}
194
195                 freeParams[i] = optima[i];                              // Reset parameter values
196                 freeParams[l] = optima[l];
197
198                 iOffset = iOffset / v;                                  //  And shrink step
199                 lOffset = lOffset / v;
200         }
201
202         for(int m = 1; m < numIter; m++) {                                              // Richardson Step
203                 for(int k = 0; k < (numIter - m); k++) {
204                         //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);}
205                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k]) / (pow(4.0, m)-1);
206                 }
207         }
208
209         if(OMX_DEBUG) {mxLog("Hessian estimation: Populating Hessian ([%d, %d] = %d and %d) with value %f...", i, l, i*numParams+l, l*numParams+i, Haprox[0]);}
210         hessian[i*numParams+l] = Haprox[0];
211         hessian[l*numParams+i] = Haprox[0];
212
213 }
214
215 void omxComputeNumericDeriv::doHessianCalculation(int numChildren, struct hess_struct *hess_work)
216 {
217         int i,j;
218
219         int numOffDiagonal = (numParams * (numParams - 1)) / 2;
220         int *diags = Calloc(numOffDiagonal, int);
221         int *offDiags = Calloc(numOffDiagonal, int);
222         int offset = 0;
223         // gcc does not detect the usage of the following variable
224         // in the omp parallel pragma, and marks the variable as
225         // unused, so the attribute is placed to silence the Rf_warning.
226     int __attribute__((unused)) parallelism = (numChildren == 0) ? 1 : numChildren;
227
228         // There must be a way to avoid constructing the
229         // diags and offDiags arrays and replace them with functions
230         // that produce these values given the input
231         /// {0, 1, ..., numOffDiagonal - 1} -- M. Spiegel
232         for(i = 0; i < numParams; i++) {
233                 for(j = i - 1; j >= 0; j--) {
234                         diags[offset] = i;
235                         offDiags[offset] = j;
236                         offset++;
237                 }
238         }
239
240         #pragma omp parallel for num_threads(parallelism) 
241         for(i = 0; i < numParams; i++) {
242                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
243                 omxEstimateHessianOnDiagonal(i, hess_work + threadId);
244         }
245
246         #pragma omp parallel for num_threads(parallelism) 
247         for(offset = 0; offset < numOffDiagonal; offset++) {
248                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
249                 omxEstimateHessianOffDiagonal(diags[offset], offDiags[offset],
250                         hess_work + threadId);
251         }
252
253         Free(diags);
254         Free(offDiags);
255 }
256
257 void omxComputeNumericDeriv::init()
258 {
259         optima = NULL;
260 }
261
262 omxComputeNumericDeriv::omxComputeNumericDeriv()
263 {
264         init();
265 }
266
267 void omxComputeNumericDeriv::initFromFrontend(SEXP rObj)
268 {
269         super::initFromFrontend(rObj);
270
271         fitMat = omxNewMatrixFromSlot(rObj, globalState, "fitfunction");
272         setFreeVarGroup(fitMat->fitFunction, varGroup);
273
274         SEXP slotValue;
275
276         Rf_protect(slotValue = R_do_slot(rObj, Rf_install("iterations")));
277         numIter = INTEGER(slotValue)[0];
278         if (numIter < 2) Rf_error("stepSize must be 2 or greater");
279
280         Rf_protect(slotValue = R_do_slot(rObj, Rf_install("parallel")));
281         parallel = Rf_asLogical(slotValue);
282
283         Rf_protect(slotValue = R_do_slot(rObj, Rf_install("stepSize")));
284         stepSize = REAL(slotValue)[0];
285         if (stepSize <= 0) Rf_error("stepSize must be positive");
286 }
287
288 void omxComputeNumericDeriv::computeImpl(FitContext *fc)
289 {
290         fitContext = fc;
291         numParams = int(fc->varGroup->vars.size());
292         if (numParams <= 0) Rf_error("Model has no free parameters");
293
294         if (parallel) omxFitFunctionCreateChildren(globalState);
295
296         optima = fc->est;
297
298         // TODO: Check for nonlinear constraints and adjust algorithm accordingly.
299         // TODO: Allow more than one hessian value for calculation
300
301         int numChildren = 0;
302         if (parallel) numChildren = Global->numChildren;
303
304         omxRecompute(fitMat);
305         minimum = omxMatrixElement(fitMat, 0, 0);
306
307         struct hess_struct* hess_work;
308         if (numChildren < 2) {
309                 hess_work = Calloc(1, struct hess_struct);
310                 omxPopulateHessianWork(hess_work, globalState);
311         } else {
312                 hess_work = Calloc(numChildren, struct hess_struct);
313                 for(int i = 0; i < numChildren; i++) {
314                         omxPopulateHessianWork(hess_work + i, globalState->childList[i]);
315                 }
316         }
317         if(OMX_DEBUG) mxLog("Hessian Calculation using %d children", numChildren);
318
319         fc->wanted |= FF_COMPUTE_HESSIAN;
320         hessian = fc->hess;
321
322         gradient = (double*) R_alloc(numParams, sizeof(double));
323   
324         doHessianCalculation(numChildren, hess_work);
325
326         if(OMX_DEBUG) {mxLog("Hessian Computation complete.");}
327
328         totalProbeCount = 0;
329
330         if (numChildren < 2) {
331                 totalProbeCount = hess_work->probeCount;
332                 Free(hess_work->Haprox);
333                 Free(hess_work->Gaprox);
334                 Free(hess_work->freeParams);
335             Free(hess_work);
336         } else {
337                 for(int i = 0; i < numChildren; i++) {
338                         struct hess_struct *hw = hess_work + i;
339                         totalProbeCount += hw->probeCount;
340                         Free(hw->Haprox);
341                         Free(hw->Gaprox);
342                         Free(hw->freeParams);
343                 }
344                 Free(hess_work);
345         }
346
347         omxFreeChildStates(globalState);
348 }
349
350 void omxComputeNumericDeriv::reportResults(FitContext *fc, MxRList *slots, MxRList *result)
351 {
352         SEXP calculatedHessian;
353         Rf_protect(calculatedHessian = Rf_allocMatrix(REALSXP, numParams, numParams));
354         memcpy(REAL(calculatedHessian), fc->hess, sizeof(double) * numParams * numParams);
355
356         result->push_back(std::make_pair(Rf_mkChar("calculatedHessian"), calculatedHessian));
357
358         MxRList out;
359         out.push_back(std::make_pair(Rf_mkChar("probeCount"), Rf_ScalarInteger(totalProbeCount)));
360         slots->push_back(std::make_pair(Rf_mkChar("output"), out.asR()));
361 }
362
363 omxCompute *newComputeNumericDeriv()
364 {
365         if (globalState->numConstraints != 0) {
366                 Rf_error("Cannot compute estimated Hessian with constraints (%d constraints found)",
367                       globalState->numConstraints);
368         }
369         return new omxComputeNumericDeriv;
370 }
371