MxFitFunctionMultigroup
[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 #include "R.h"
18 #include <Rinternals.h>
19 #include <Rdefines.h>
20 #include <R_ext/Rdynload.h>
21 #include <R_ext/BLAS.h>
22 #include <R_ext/Lapack.h>
23 #include "omxDefines.h"
24 #include "npsolWrap.h"
25
26 #include <stdio.h>
27 #include <sys/types.h>
28 #include <errno.h>
29 #include "omxState.h"
30 #include "omxMatrix.h"
31 #include "omxAlgebra.h"
32 #include "omxFitFunction.h"
33 #include "omxNPSOLSpecific.h"
34 #include "omxOptimizer.h"
35 #include "omxOpenmpWrap.h"
36
37 struct hess_struct {
38         int     numParams;
39         double* freeParams;
40         double* Haprox;
41         double* Gaprox;
42         omxMatrix* fitMatrix;
43         double  f0;
44         double  functionPrecision;
45         int r;
46 };
47
48 void omxPopulateHessianWork(struct hess_struct *hess_work, 
49         double functionPrecision, int r, omxState* state,
50         omxState *parentState) {
51
52         omxFitFunction* oo = state->fitMatrix->fitFunction;
53         int numParams = state->numFreeParams;
54
55         double* optima = parentState->optimalValues;
56         double *freeParams = (double*) Calloc(numParams, double);
57
58         hess_work->numParams = numParams;
59         hess_work->functionPrecision = functionPrecision;
60         hess_work->r = r;
61         hess_work->Haprox = (double*) Calloc(r, double);                // Hessian Workspace
62         hess_work->Gaprox = (double*) Calloc(r, double);                // Gradient Workspace
63         hess_work->freeParams = freeParams;
64         for(int i = 0; i < numParams; i++) {
65                 freeParams[i] = optima[i];
66         }
67
68         omxMatrix *fitMatrix = oo->matrix;
69         hess_work->fitMatrix = fitMatrix;
70
71         handleFreeVarListHelper(state, freeParams, numParams);
72
73         omxRecompute(fitMatrix);                // Initial recompute in case it matters.        
74         hess_work->f0 = omxMatrixElement(fitMatrix, 0, 0);
75 }
76
77 /**
78   @params i              parameter number
79   @params hess_work      local copy
80   @params optima         shared read-only variable
81   @params gradient       shared write-only variable
82   @params hessian        shared write-only variable
83  */
84 void omxEstimateHessianOnDiagonal(int i, struct hess_struct* hess_work, 
85         double *optima, double *gradient, double *hessian) {
86
87         static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
88         static const double eps = 1E-4; // Kept here for access purposes.
89
90         int     numParams          = hess_work->numParams;         // read-only
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 functionPrecision   = hess_work->functionPrecision; // read-only
96         double f0                  = hess_work->f0;                // read-only
97         int    r                   = hess_work->r;                 // read-only
98
99
100         /* Part the first: Gradient and diagonal */
101         double iOffset = fabs(functionPrecision * optima[i]);
102         if(fabs(iOffset) < eps) iOffset += eps;
103         if(OMX_DEBUG) {Rprintf("Hessian estimation: iOffset: %f.\n", iOffset);}
104         for(int k = 0; k < r; k++) {                    // Decreasing step size, starting at k == 0
105                 if(OMX_DEBUG) {Rprintf("Hessian estimation: Parameter %d at refinement level %d (%f). One Step Forward.\n", i, k, iOffset);}
106                 freeParams[i] = optima[i] + iOffset;
107
108                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
109
110                 omxRecompute(fitMatrix);
111                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
112
113                 if(OMX_DEBUG) {Rprintf("Hessian estimation: One Step Back.\n");}
114
115                 freeParams[i] = optima[i] - iOffset;
116
117                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
118
119                 omxRecompute(fitMatrix);
120                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
121
122                 Gaprox[k] = (f1 - f2) / (2.0*iOffset);                                          // This is for the gradient
123                 Haprox[k] = (f1 - 2.0 * f0 + f2) / (iOffset * iOffset);         // This is second derivative
124                 freeParams[i] = optima[i];                                                                      // Reset parameter value
125                 iOffset /= v;
126                 if(OMX_DEBUG) {Rprintf("Hessian estimation: (%d, %d)--Calculating F1: %f F2: %f, Haprox: %f...\n", i, i, f1, f2, Haprox[k]);}
127         }
128
129         for(int m = 1; m < r; m++) {                                            // Richardson Step
130                 for(int k = 0; k < (r - m); k++) {
131                         Gaprox[k] = (Gaprox[k+1] * pow(4.0, m) - Gaprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
132                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k])/(pow(4.0, m)-1); // NumDeriv Hard-wires 4s for r here. Why?
133                 }
134         }
135
136         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]); }
137         gradient[i] = Gaprox[0];                                                // NPSOL reports a gradient that's fine.  Why report two?
138         hessian[i*numParams + i] = Haprox[0];
139
140         if(OMX_DEBUG) {Rprintf("Done with parameter %d.\n", i);}
141
142 }
143
144 void omxEstimateHessianOffDiagonal(int i, int l, struct hess_struct* hess_work, 
145         double *optima, double *gradient, double *hessian) {
146
147     static const double v = 2.0; //Note: NumDeriv comments that this could be a parameter, but is hard-coded in the algorithm
148     static const double eps = 1E-4; // Kept here for access purposes.
149
150         int     numParams          = hess_work->numParams;         // read-only
151         double *Haprox             = hess_work->Haprox;
152         double *freeParams         = hess_work->freeParams;
153         omxMatrix* fitMatrix = hess_work->fitMatrix; 
154         double functionPrecision   = hess_work->functionPrecision; // read-only
155         double f0                  = hess_work->f0;                // read-only
156         int    r                   = hess_work->r;                 // read-only
157
158         double iOffset = fabs(functionPrecision*optima[i]);
159         if(fabs(iOffset) < eps) iOffset += eps;
160         double lOffset = fabs(functionPrecision*optima[l]);
161         if(fabs(lOffset) < eps) lOffset += eps;
162
163         for(int k = 0; k < r; k++) {
164                 freeParams[i] = optima[i] + iOffset;
165                 freeParams[l] = optima[l] + lOffset;
166
167                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
168
169                 omxRecompute(fitMatrix);
170                 double f1 = omxMatrixElement(fitMatrix, 0, 0);
171
172                 if(OMX_DEBUG) {Rprintf("Hessian estimation: One Step Back.\n");}
173
174                 freeParams[i] = optima[i] - iOffset;
175                 freeParams[l] = optima[l] - lOffset;
176
177                 fitMatrix->fitFunction->repopulateFun(fitMatrix->fitFunction, freeParams, numParams);
178
179                 omxRecompute(fitMatrix);
180                 double f2 = omxMatrixElement(fitMatrix, 0, 0);
181
182                 Haprox[k] = (f1 - 2.0 * f0 + f2 - hessian[i*numParams+i]*iOffset*iOffset -
183                                                 hessian[l*numParams+l]*lOffset*lOffset)/(2.0*iOffset*lOffset);
184                 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), functionPrecision*optima[i], functionPrecision*optima[l]);}
185
186                 freeParams[i] = optima[i];                              // Reset parameter values
187                 freeParams[l] = optima[l];
188
189                 iOffset = iOffset / v;                                  //  And shrink step
190                 lOffset = lOffset / v;
191         }
192
193         for(int m = 1; m < r; m++) {                                            // Richardson Step
194                 for(int k = 0; k < (r - m); k++) {
195                         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, functionPrecision, optima[i], optima[l], v, m, pow(4.0, m), functionPrecision*optima[i], functionPrecision*optima[l], k);}
196                         Haprox[k] = (Haprox[k+1] * pow(4.0, m) - Haprox[k]) / (pow(4.0, m)-1);
197                 }
198         }
199
200         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]);}
201         hessian[i*numParams+l] = Haprox[0];
202         hessian[l*numParams+i] = Haprox[0];
203
204 }
205
206 void doHessianCalculation(int numParams, int numChildren, 
207         struct hess_struct *hess_work, omxState* parentState) {
208
209         int i,j;
210
211         omxFitFunction* parent_oo = parentState->fitMatrix->fitFunction;
212         double* parent_gradient = parent_oo->gradient;
213         double* parent_hessian = parent_oo->hessian;
214         double* parent_optima = parentState->optimalValues;
215
216         int numOffDiagonal = (numParams * (numParams - 1)) / 2;
217         int *diags = Calloc(numOffDiagonal, int);
218         int *offDiags = Calloc(numOffDiagonal, int);
219         int offset = 0;
220         // gcc does not detect the usage of the following variable
221         // in the omp parallel pragma, and marks the variable as
222         // unused, so the attribute is placed to silence the warning.
223     int __attribute__((unused)) parallelism = (numChildren == 0) ? 1 : numChildren;
224
225         // There must be a way to avoid constructing the
226         // diags and offDiags arrays and replace them with functions
227         // that produce these values given the input
228         /// {0, 1, ..., numOffDiagonal - 1} -- M. Spiegel
229         for(i = 0; i < numParams; i++) {
230                 for(j = i - 1; j >= 0; j--) {
231                         diags[offset] = i;
232                         offDiags[offset] = j;
233                         offset++;
234                 }
235         }
236
237         #pragma omp parallel for num_threads(parallelism) 
238         for(i = 0; i < numParams; i++) {
239                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
240                 omxEstimateHessianOnDiagonal(i, hess_work + threadId, 
241                         parent_optima, parent_gradient, parent_hessian);
242         }
243
244         #pragma omp parallel for num_threads(parallelism) 
245         for(offset = 0; offset < numOffDiagonal; offset++) {
246                 int threadId = (numChildren < 2) ? 0 : omx_absolute_thread_num();
247                 omxEstimateHessianOffDiagonal(diags[offset], offDiags[offset],
248                         hess_work + threadId, parent_optima, parent_gradient,
249                         parent_hessian);
250         }
251
252         Free(diags);
253         Free(offDiags);
254
255 }
256
257 /*************************************************************************************
258  *
259  *   omxEstimateHessian
260  *
261  *  author: tbrick, 2010-02-04
262  *
263  *  Based on code in the numDeriv library for R <citation needed> // TODO: Find NumDeriv Citation
264  *
265  *  @params functionPrecision   functional precision for the calculation
266  *  @params r                                   number of repetitions for Richardson approximation
267  *  @params currentState                the current omxState
268  *
269  ************************************************************************************/
270 unsigned short omxEstimateHessian(int numHessians, double functionPrecision, int r, omxState* parentState) {
271
272         // TODO: Check for nonlinear constraints and adjust algorithm accordingly.
273         // TODO: Allow more than one hessian value for calculation
274
275         if(numHessians > 1) {
276                 error("NYI: Cannot yet calculate more than a single hessian per optimization.\n");
277         }
278
279         if(numHessians == 0) return FALSE;
280         int numChildren = parentState->numChildren;
281         int numParams = parentState->numFreeParams;
282         int i;
283
284     struct hess_struct* hess_work;
285         if (numChildren < 2) {
286                 hess_work = Calloc(1, struct hess_struct);
287                 omxPopulateHessianWork(hess_work, functionPrecision, r, parentState, parentState);
288         } else {
289                 hess_work = Calloc(numChildren, struct hess_struct);
290                 for(int i = 0; i < numChildren; i++) {
291                         omxPopulateHessianWork(hess_work + i, functionPrecision, r, parentState->childList[i], parentState);
292                 }
293         }
294         if(OMX_DEBUG) Rprintf("Hessian Calculation using %d children\n", numChildren);
295
296         omxFitFunction* parent_oo = parentState->fitMatrix->fitFunction;
297
298         if(parent_oo->hessian == NULL) {
299                 parent_oo->hessian = (double*) R_alloc(numParams * numParams, sizeof(double));
300                 if(OMX_DEBUG) {Rprintf("Generated hessian memory, (%d x %d), at 0x%x.\n", numParams, numParams, parent_oo->hessian);}
301         }
302
303         if(parent_oo->gradient == NULL) {
304                 parent_oo->gradient = (double*) R_alloc(numParams, sizeof(double));
305                 if(OMX_DEBUG) {Rprintf("Generated gradient memory, (%d), at 0x%x.\n", numParams, parent_oo->gradient);}
306         }
307   
308         doHessianCalculation(numParams, numChildren, hess_work, parentState);
309
310         if(OMX_DEBUG) {Rprintf("Hessian Computation complete.\n");}
311
312         if (numChildren < 2) {
313                 Free(hess_work->Haprox);
314                 Free(hess_work->Gaprox);
315                 Free(hess_work->freeParams);
316             Free(hess_work);
317         } else {
318                 for(i = 0; i < numChildren; i++) {
319                         Free((hess_work + i)->Haprox);
320                         Free((hess_work + i)->Gaprox);
321                         Free((hess_work + i)->freeParams);
322                 }
323                 Free(hess_work);
324         }
325         return TRUE;
326
327 }