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