00001
00019 #include <math.h>
00020 #include <string.h>
00021
00022 #include "ipc.h"
00023 #include "analyze.h"
00024 #include "lissom.h"
00025 #include "fit_gauss.h"
00026 #include "npsol.h"
00027 #include "globals.h"
00028
00029
00030
00031 #define DEFAULT_BOUND_r 1.0
00032 #define DEFAULT_NET_DER_LEVEL 1
00033 #define DEFAULT_NET_MIL 200
00034 #define DEFAULT_NET_FP -1.0
00035 #define DEFAULT_LIN_SRCH_TOL -1.0
00036 #define DEFAULT_NET_OPT_TOL -1.0
00037
00038 int npsol_na_error_message_printed = 0;
00039 int npopt_na_error_message_printed = 0;
00040
00041
00042
00043
00044
00045
00046
00047
00048 struct SOLVER_DATA {
00049 INTEGER N, INFORM, ITER, ISTATE[6], LENIW, IW[19], LENW;
00050 INTEGER NCLIN, NCNLN, NROWA, NROWJ, NROWR;
00051 REAL A[6], OBJF, *OBJGRD, C[1], CJAC[6], CLAMBDA[6], R[36], *X, WW[121];
00052 REAL *BL, *BU;
00053 }data;
00054
00055 Conf Configuration;
00056
00057
00058
00059 int NN_Configure_Solver(Conf *Config);
00060 int NN_solver_cleanup(void);
00061 int NN_solve(int *inform);
00062 void NN_solver_allocate(void);
00063 npsolfunreturn Objective_Function(INTEGER *mode, INTEGER *nwts,
00064 REAL *vars, REAL *objf,
00065 REAL *objgrd, INTEGER *nstate);
00066 npsolfunreturn confun(INTEGER *mode, INTEGER *ncnln, INTEGER *n,
00067 INTEGER *nrowj, INTEGER *needc,
00068 REAL *x, REAL *c, REAL *cjac, INTEGER *nstate);
00069 npsolfunreturn NN_OBJFUN(INTEGER *mode, INTEGER *n,
00070 REAL *x, REAL *objf, REAL *objgrd, INTEGER *nstate);
00071
00072
00073
00074
00075
00076
00077 REAL Gradient[7], Variables[7],
00078 Lower_bounds[7], Upper_bounds[7];
00079
00080 INTEGER No_of_variables=6;
00081 INTEGER nPrints=0;
00082
00083 int lowk, highk, lowl, highl;
00084 INTEGER Solver_Status =0 ;
00085
00086
00087
00088
00089
00090
00091 void setup_variable_bounds(double rlow, double rhigh,
00092 double thetalow, double thetahigh,
00093 double cxlow, double cxhigh,
00094 double cylow, double cyhigh,
00095 double xsigmalow, double xsigmahigh,
00096 double ysigmalow, double ysigmahigh)
00097 {
00098
00099
00100 Lower_bounds[0] = rlow; Upper_bounds[0] = rhigh;
00101 Lower_bounds[1] = thetalow; Upper_bounds[1] = thetahigh;
00102 Lower_bounds[2] = cxlow; Upper_bounds[2] = cxhigh;
00103 Lower_bounds[3] = cylow; Upper_bounds[3] = cyhigh;
00104 Lower_bounds[4] = xsigmalow; Upper_bounds[4] = xsigmahigh;
00105 Lower_bounds[5] = ysigmalow; Upper_bounds[5] = ysigmahigh;
00106
00107 lowk = MAX((int)floor( Lower_bounds[2] * (double)RN), 0 );
00108 highk = MIN((int)floor(0.5+Upper_bounds[2] * (double)RN), RN-1);
00109 lowl = MAX((int)floor( Lower_bounds[3] * (double)RN), 0 );
00110 highl = MIN((int)floor(0.5+Upper_bounds[3] * (double)RN), RN-1);
00111 }
00112
00113
00114
00115 void setup_solver(void)
00116 {
00117 Configuration.Verify_Level = 0;
00118 Configuration.Der_Level = DEFAULT_NET_DER_LEVEL ;
00119 Configuration.Major_Iter_Limit = 200;
00120 #ifdef RELAXED_GAUSSFIT
00121
00122 Configuration.Function_Precision = 1e-3;
00123 #else
00124 Configuration.Function_Precision = -1.0;
00125 #endif
00126 Configuration.Lin_Srch_Tolerance = -1.0;
00127 Configuration.Optimality_Tolerance = -1.0;
00128 Configuration.nb_params = 0;
00129
00130 NN_solver_allocate() ;
00131 NN_Configure_Solver(&Configuration);
00132 }
00133
00134
00135
00136 void fit_gauss(double *r, double *theta,
00137 double *cx, double *cy,
00138 double *sigmax, double *sigmay)
00139 {
00140 int information;
00141
00142
00143
00144 Variables[0] = *r ; Variables[1] = *theta;
00145 Variables[2] = *cx; Variables[3] = *cy;
00146 Variables[4] = *sigmax; Variables[5] = *sigmay;
00147 Solver_Status = 0;
00148
00149 NN_solve(&information);
00150
00151 *r = Variables[0] ; *theta = Variables[1] ; *cx = Variables[2];
00152 *cy = Variables[3]; *sigmax = Variables[4]; *sigmay = Variables[5];
00153 }
00154
00155
00156
00157
00158
00159
00160 npsolfunreturn Objective_Function(INTEGER *mode, INTEGER *nwts,
00161 REAL *vars, REAL *objf,
00162 REAL *objgrd, INTEGER *nstate)
00163 {
00164 int x,y;
00165 double F_xy, the_exp, first, second, first_comp, second_comp;
00166 double error, sq_error;
00167 double xsigma_sq, ysigma_sq;
00168 double inv_xsigma_sq, inv_ysigma_sq;
00169 double actual_angle,cos_angle, sin_angle;
00170 double xcoord, ycoord;
00171
00172
00173 (void)mode; (void)nwts; (void)nstate;
00174
00175
00176
00177
00178 xsigma_sq = vars[4] * vars[4];
00179 ysigma_sq = vars[5] * vars[5];
00180 inv_xsigma_sq = 1.0/xsigma_sq;
00181 inv_ysigma_sq = 1.0/ysigma_sq;
00182 actual_angle = M_PI_2 * vars[1];
00183
00184 cos_angle = cos(actual_angle);
00185 sin_angle = sin(actual_angle);
00186 sq_error = 0.0;
00187 for (x=0; x <7; x++) objgrd[x] = 0.0;
00188
00189
00190
00191 for(x=lowk; x<=highk; x++)
00192 for(y=lowl; y<=highl; y++){
00193
00194 xcoord = (double)x/(double)RN; ycoord = (double)y/(double)RN;
00195
00196 first = (xcoord-vars[2]) * cos(actual_angle) +
00197 (ycoord-vars[3]) * sin(actual_angle) ;
00198
00199 second = -(xcoord-vars[2]) * sin(actual_angle) +
00200 (ycoord-vars[3]) * cos(actual_angle) ;
00201
00202 first_comp = first * first * inv_xsigma_sq;
00203 second_comp = second * second * inv_ysigma_sq;
00204 the_exp = exp( -first_comp -second_comp);
00205 F_xy = vars[0]*the_exp;
00206
00207 error = (F_xy - gaussfit_array[x][y]);
00208 00209
00210
00211 objgrd[0] += error * the_exp;
00212 objgrd[1] += error * F_xy * -2.0* (first * second *inv_xsigma_sq -
00213 second* first * inv_ysigma_sq);
00214 objgrd[2] += error * F_xy * 2.0 * (first *cos_angle * inv_xsigma_sq -
00215 second*sin_angle * inv_ysigma_sq);
00216 objgrd[3] += error * F_xy * 2.0 * (first*sin_angle * inv_xsigma_sq +
00217 second * cos_angle * inv_ysigma_sq);
00218 objgrd[4] += error * F_xy * 2.0 * first_comp/vars[4];
00219 objgrd[5] += error * F_xy * 2.0 * second_comp/vars[5];
00220 sq_error += error * error;
00221 gaussian[x][y] = F_xy;
00222 }
00223 for (x=0; x<6; x++) objgrd[x] *= (1.0/((double)(RN*RN)));
00224
00225 *objf = 0.5 * sq_error / ((double)(RN*RN));
00226
00227 return npsolfunreturnval;
00228 }
00229
00230
00231
00232
00233
00234
00235 npsolfunreturn confun(INTEGER *mode, INTEGER *ncnln, INTEGER *n,
00236 INTEGER *nrowj, INTEGER *needc,
00237 REAL *x, REAL *c, REAL *cjac, INTEGER *nstate)
00238 {
00239
00240 (void)mode; (void)ncnln; (void)n; (void)nrowj;
00241 (void)needc; (void)x; (void)nstate;
00242
00243 c[0] = 0.0;
00244 cjac[0] = 0.0;
00245
00246 return npsolfunreturnval;
00247 }
00248
00249
00250
00251
00252
00253
00254
00255 00256 00257 00258 00259 00260 00261 00262 00263 00264
00265 void NN_solver_allocate(void)
00266 {
00267 int n;
00268 int total_memory=0;
00269
00270
00271 n = No_of_variables;
00272 data.N = n;
00273 data.NCLIN = 0; data.NCNLN = 0; data.NROWA = 1;
00274 data.NROWJ = 1; data.NROWR = n;
00275 data.LENIW = 3 * n +1;
00276 data.LENW = 20 * n +1;
00277
00278 data.BL = Lower_bounds;
00279 data.BU = Upper_bounds;
00280
00281 if ( data.BL == (REAL *)NULL || data.BU == (REAL *)NULL ) {
00282 ipc_notify(IPC_ALL,IPC_ERROR,"NN_solver_allocate: BU or BL NULL ptr");
00283 return;
00284 }
00285
00286 data.OBJGRD = Gradient;
00287 data.X = Variables;
00288
00289 if ( data.OBJGRD == (REAL *)NULL
00290 || data.R == (REAL *)NULL
00291 || data.X == (REAL *)NULL
00292 || data.A == (REAL *)NULL
00293 || data.CJAC == (REAL *)NULL
00294 || data.ISTATE == (INTEGER *)NULL
00295 || data.WW == (REAL *)NULL
00296 || data.IW == (INTEGER *)NULL
00297 || data.CLAMBDA== (REAL *)NULL ) {
00298 ipc_notify(IPC_ALL,IPC_ERROR,"NN_solver_allocate: Out of Memory");
00299 return;
00300 }
00301 total_memory += 6*n + (data.LENIW) + (data.NROWR)*n +data.LENW;
00302
00303 ipc_notify(IPC_ALL,IPC_VERBOSE,"NPSOL memory allocations done. Solver requires: %d floats",
00304 total_memory);
00305
00306 }
00307
00308
00309
00310
00311 00312 00313 00314
00315 static void NP_Opt(char *str)
00316 {
00317
00318
00319 #ifndef USE_NPSOL
00320 (void)str;
00321 if (!npopt_na_error_message_printed) {
00322 ipc_notify(IPC_ALL,IPC_ERROR,"NP_Opt: NPSOL is not available; can't call NPOPTN");
00323 npopt_na_error_message_printed = 1;
00324 }
00325 #else
00326 NPOPTN_FUN(str,strlen(str));
00327 #endif
00328 }
00329
00330
00331
00332 00333 00334 00335 00336 00337 00338 00339 00340 00341 00342 00343 00344 00345 00346
00347 int NN_Configure_Solver(Conf *Config)
00348 {
00349 #define BUFSIZE 100
00350 char str[BUFSIZE];
00351 int i, mpl = 0;
00352
00353
00354
00355 snprintf(str,BUFSIZE,"Verify Level=%d",
00356 (int)(Config->Verify_Level));
00357 NP_Opt(str);
00358 snprintf(str,BUFSIZE,"Derivative Level = %d",
00359 (int)(Config->Der_Level));
00360 NP_Opt(str);
00361 snprintf(str,BUFSIZE,"Linesearch Tolerance = %f",
00362 (double)(Config->Lin_Srch_Tolerance));
00363 NP_Opt(str);
00364 snprintf(str,BUFSIZE,"Major Iteration Limit = %d",
00365 (int)(Config->Major_Iter_Limit));
00366 NP_Opt(str);
00367
00368 if (nPrints < 5) mpl = 0;
00369 if (nPrints >= 5 && nPrints < 15) mpl = 1;
00370 if (nPrints == 15 ) mpl = 5;
00371 if (nPrints >= 16 && nPrints < 20) mpl = 10;
00372 if (nPrints >= 20 && nPrints < 30) mpl = 20;
00373 if (nPrints >= 30 ) mpl = 30;
00374
00375 snprintf(str,BUFSIZE, "Major Print Level = %d", mpl);
00376 NP_Opt(str);
00377
00378 snprintf(str,BUFSIZE, "Minor Print Level = %d", 0);
00379 NP_Opt(str);
00380
00381 if (Config->Function_Precision != -1) {
00382 snprintf(str,BUFSIZE, "Function Precision = %g",
00383 (double)(Config->Function_Precision));
00384 NP_Opt(str);
00385 }
00386
00387 if (Config->Optimality_Tolerance != -1) {
00388 snprintf(str,BUFSIZE, "Optimality Tolerance = %f",
00389 (double)(Config->Optimality_Tolerance));
00390 }
00391
00392
00393 for (i=0; i< Config->nb_params; i++) {
00394 strncpy(str,Config->param_table[i],BUFSIZE);
00395 NP_Opt(str);
00396 }
00397
00398 return(0);
00399 }
00400 #undef BUFSIZE
00401
00402
00403
00404
00405 00406 00407 00408 00409 00410 00411 00412 00413 00414 00415 00416
00417 npsolfunreturn NN_OBJFUN(INTEGER *mode, INTEGER *n,
00418 REAL *x, REAL *objf, REAL *objgrd, INTEGER *nstate)
00419 {
00420 Objective_Function(mode, n, x, objf, objgrd, nstate);
00421
00422 if (*objf < Configuration.Optimality_Tolerance) {
00423 ipc_notify(IPC_ALL,IPC_VERBOSE,"Optimality_Tolerance reached: solver stopped");
00424 *mode = -1;
00425 }
00426
00427 return npsolfunreturnval;
00428 }
00429
00430
00431
00432 00433 00434 00435 00436 00437 00438 00439 00440 00441 00442 00443 00444 00445
00446 int NN_solve(int *inform)
00447 {
00448
00449 #ifndef USE_NPSOL
00450 if (!npsol_na_error_message_printed) {
00451 ipc_notify(IPC_ONE,IPC_ERROR,"NN_solve: NPSOL is not available; can't call NPSOL");
00452 npsol_na_error_message_printed = 1;
00453 }
00454 *inform = -1;
00455
00456 #else
00457 NPSOL_FUN(
00458 &(data.N),
00459 &(data.NCLIN),
00460 &(data.NCNLN),
00461 &(data.NROWA),
00462 &(data.NROWJ),
00463 &(data.NROWR),
00464 data.A,
00465 data.BL,
00466 data.BU,
00467 confun, NN_OBJFUN,
00468 &(data.INFORM), &(data.ITER), data.ISTATE,
00469 data.C, data.CJAC, data.CLAMBDA,
00470 &(data.OBJF), data.OBJGRD,
00471 data.R, data.X,
00472 data.IW, &(data.LENIW),
00473 data.WW, &(data.LENW) );
00474
00475 *inform = data.INFORM;
00476 #endif
00477
00478
00479
00480 return(0);
00481 }
00482