[65ba95]: optimize.c Maximize Restore History

Download this file

optimize.c    489 lines (382 with data), 13.4 kB

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
#include "filter.h"
#include "math.h"
#include "float.h"
lmfunc fcn;
static int AllocateLMStruct( struct LMStruct *LM );
static void FreeLMStruct ( struct LMStruct *LM );
void bracket( struct LMStruct *LM );
double sumSquared( double *a, int n );
#define FUNCS_PER_CP getFcnPanoNperCP() // number of functions per control point
// Call Levenberg-Marquard optimizer
#if 1
void RunLMOptimizer( OptInfo *o)
{
struct LMStruct LM;
int iflag;
char *warning;
char *infmsg[] = {
"improper input parameters",
"the relative error in the sum of squares is at most tol",
"the relative error between x and the solution is at most tol",
"conditions for info = 1 and info = 2 both hold",
"fvec is orthogonal to the columns of the jacobian to machine precision",
"number of calls to fcn has reached or exceeded 200*(n+1)",
"tol is too small. no further reduction in the sum of squares is possible",
"tol too small. no further improvement in approximate solution x possible",
"Interrupted"
};
int istrat; // strategy
int totalfev; // total function evaluations
int numconstraints; // number of constraints imposed by control points
int i;
int lmInfo;
AlignInfo *g; // obtained from adjust.c
AlignInfo *GetGlobalPtr(); // likewise
// PrintError("RunLMOptimizer");
// The method used here is a hybrid of two optimization strategies.
// In the first strategy, fcnPano is configured to return one function per
// control point, that function being total distance without regard for
// direction. In the second strategy, fcnPano is configured to return two
// functions per control point, those functions being distance in two
// directions, typically longitude and latitude. The second strategy
// converges much faster, but may be less stable with poor initial estimates.
// So, we use the first method as long as it makes significant progress
// (currently 5% reduction in error per iteration), and then switch to
// the second method to rapidly polish the estimate. Final result
// returned to the user is that of the second method.
//
// Older versions of Panorama Tools used just the first strategy,
// with error tolerances set to make it run to full convergence,
// which often took hundreds or thousands of iterations. The hybrid
// approach typically converges much faster (a few tens of iterations)
// and appears to be equally robust in testing to date. Full convergence
// (to am lmdif ftol of 1.0e-14) is not always achieved faster than the old
// version. However the convergence rate (error reduction per wall-clock
// second) has been significantly better in all cases tested, and the final
// accuracy has been equal or improved.
//
// So, in the interest of behavior that is friendlier to the user, I have
// set an ftol convergence criterion that is looser than before, 1.0e-6
// instead of 1.0e-14. By this point, it is very unlikely that
// significant reductions can be achieved by more iterating, since
// even 10,000 more iterations would be predicted to make at most 1%
// improvement in the total error.
//
// I have also made the diagnosis of too few control points more precise
// and more obvious to the user. The old version complained if the
// number of control points was less than the number of parameters,
// although in fact each normal control point contributes two independent
// constraints (x and y) so the actual critical number is
// 2*controlpoints >= parameters. As a result, the old version often
// complained even when things were fine, and never complained more loudly
// even when things were awful. This version does not complain
// at all unless there are not enough actual constraints, and then it puts
// out an error dialog that must be dismissed by the user.
//
// Rik Littlefield (rj.littlefield@computer.org), May 2004.
LM.n = o->numVars;
g = GetGlobalPtr();
numconstraints = 0;
for(i=0; i < g->numPts; i++) {
if (g->cpt[i].type == 0)
numconstraints += 2;
else numconstraints += 1;
}
warning = "";
if( numconstraints < LM.n )
{
char msgx[200];
warning = "Warning: Number of Data Points is smaller than Number of Variables to fit.\n";
sprintf (msgx,"You have too few control points (%d) or too many parameters (%d). Strange values may result!",o->numData,LM.n);
PrintError(msgx);
}
totalfev = 0;
for (istrat=1; istrat <= 2; istrat++) {
setFcnPanoNperCP(istrat);
LM.m = o->numData*FUNCS_PER_CP;
if( LM.m < LM.n ) LM.m = LM.n; // in strategy #1, fcnpano will pad fvec if needed
fcn = o->fcn;
if( AllocateLMStruct( &LM ) != 0 )
{
PrintError( "Not enough Memory" );
return;
}
// Initialize optimization params
if( o->SetVarsToX( LM.x ) != 0)
{
PrintError("Internal Error");
return;
}
iflag = -100; // reset counter and initialize dialog
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
if (istrat == 2) setFcnPanoDoNotInitAvgFov();
// infoDlg ( _initProgress, "Optimizing Variables" );
/* Call lmdif. */
LM.ldfjac = LM.m;
LM.mode = 1;
LM.nprint = 1; // 10
LM.info = 0;
LM.factor = 100.0;
LM.ftol = 1.0e-6; // used to be DBL_EPSILON; //1.0e-14;
if (istrat == 1) {
LM.ftol = 0.05; // for distance-only strategy, bail out when convergence slows
}
lmdif( LM.m, LM.n, LM.x, LM.fvec, LM.ftol, LM.xtol,
LM.gtol, LM.maxfev, LM.epsfcn, LM.diag, LM.mode, LM.factor,
LM.nprint, &LM.info, &LM.nfev, LM.fjac, LM.ldfjac, LM.ipvt,
LM.qtf, LM.wa1, LM.wa2, LM.wa3, LM.wa4);
lmInfo = LM.info;
// At end, one final evaluation to get errors that do not have fov stabilization applied,
// for reporting purposes.
if (istrat == 2) {
forceFcnPanoReinitAvgFov();
iflag = 1;
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
}
o->SetXToVars( LM.x );
iflag = -99; // reset counter and dispose dialog
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
// infoDlg ( _disposeProgress, "" );
// Display solver info
if(LM.info >= 8)
LM.info = 4;
if(LM.info < 0)
LM.info = 8;
totalfev += LM.nfev;
sprintf( (char*) o->message, "# %s%d function evaluations\n# %s\n# final rms error %g units\n",
warning, totalfev, infmsg[LM.info],
sqrt(sumSquared(LM.fvec,LM.m)/LM.m) * sqrt((double)FUNCS_PER_CP));
FreeLMStruct( &LM );
if (lmInfo < 0) break; // to honor user cancel in strategy 1
}
setFcnPanoNperCP(1); // Force back to startegy 1 for backwards compatability
}
#endif
#if 0
void RunLMOptimizer( OptInfo *o){
RunBROptimizer ( o, 1.0e-9);
}
#endif
// Call Bracketing optimizer
void RunBROptimizer ( OptInfo *o, double minStepWidth)
{
struct LMStruct LM;
int iflag;
char *infmsg[] = {
"improper input parameters",
"the relative error in the sum of squares is at most tol",
"the relative error between x and the solution is at most tol",
"conditions for info = 1 and info = 2 both hold",
"fvec is orthogonal to the columns of the jacobian to machine precision",
"number of calls to fcn has reached or exceeded 200*(n+1)",
"tol is too small. no further reduction in the sum of squares is possible",
"tol too small. no further improvement in approximate solution x possible",
"Interrupted"
};
// PrintError("RunBROptimizer");
LM.n = o->numVars;
setFcnPanoNperCP(1); // This optimizer does not use direction, don't waste time computing it
if( o->numData*FUNCS_PER_CP < LM.n )
{
LM.m = LM.n;
}
else
{
LM.m = o->numData*FUNCS_PER_CP;
}
fcn = o->fcn;
if( AllocateLMStruct( &LM ) != 0 )
{
PrintError( "Not enough Memory to allocate Data for BR-solver" );
return;
}
// Initialize optimization params
if( o->SetVarsToX( LM.x ) != 0)
{
PrintError("Internal Error");
return;
}
iflag = -100; // reset counter
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
//infoDlg ( _initProgress, "Optimizing Params" );
/* Call lmdif. */
LM.ldfjac = LM.m;
LM.mode = 1;
LM.nprint = 1;
// Set stepwidth to angle corresponding to one pixel in final pano
LM.epsfcn = minStepWidth; // g->pano.hfov / (double)g->pano.width;
LM.info = 0;
LM.factor = 1.0;
#if 0
lmdif( LM.m, LM.n, LM.x, LM.fvec, LM.ftol, LM.xtol,
LM.gtol, LM.maxfev, LM.epsfcn, LM.diag, LM.mode, LM.factor,
LM.nprint, &LM.info, &LM.nfev, LM.fjac, LM.ldfjac, LM.ipvt,
LM.qtf, LM.wa1, LM.wa2, LM.wa3, LM.wa4);
#endif
bracket( &LM );
o->SetXToVars( LM.x );
iflag = -99; //
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
//infoDlg ( _disposeProgress, "" );
FreeLMStruct( &LM );
}
// Allocate Memory and set default values. n must be set!
int AllocateLMStruct( struct LMStruct *LM )
{
int i,k, result = 0 ;
if( LM->n <= 0 || LM->m <= 0 || LM->n > LM->m )
return -1;
LM->ftol = DBL_EPSILON;//1.0e-14;
LM->xtol = DBL_EPSILON;//1.0e-14;
LM->gtol = DBL_EPSILON;//1.0e-14;
LM->epsfcn = DBL_EPSILON * 10.0;//1.0e-15;
LM->maxfev = 100 * (LM->n+1) * 100;
LM->ipvt = NULL;
LM->x = LM->fvec = LM->diag = LM->qtf = LM->wa1 = LM->wa2 = LM->wa3 = LM->wa4 = LM->fjac = NULL;
LM->ipvt = (int*) malloc( LM->n * sizeof( int ));
LM->x = (double*) malloc( LM->n * sizeof( double ));
LM->fvec = (double*) malloc( LM->m * sizeof( double ));
LM->diag = (double*) malloc( LM->n * sizeof( double ));
LM->qtf = (double*) malloc( LM->n * sizeof( double ));
LM->wa1 = (double*) malloc( LM->n * sizeof( double ));
LM->wa2 = (double*) malloc( LM->n * sizeof( double ));
LM->wa3 = (double*) malloc( LM->n * sizeof( double ));
LM->wa4 = (double*) malloc( LM->m * sizeof( double ));
LM->fjac = (double*) malloc( LM->m * LM->n * sizeof( double ));
if( LM->ipvt == NULL || LM->x == NULL || LM->fvec == NULL || LM->diag == NULL ||
LM->qtf == NULL || LM->wa1 == NULL || LM->wa2 == NULL || LM->wa3 == NULL ||
LM->wa4 == NULL || LM->fjac == NULL )
{
FreeLMStruct( LM );
return -1;
}
// Initialize to zero
for(i=0; i<LM->n; i++)
{
LM->x[i] = LM->diag[i] = LM->qtf[i] = LM->wa1[i] = LM->wa2[i] = LM->wa3[i] = 0.0;
LM->ipvt[i] = 0;
}
for(i=0; i<LM->m; i++)
{
LM->fvec[i] = LM->wa4[i] = 0.0;
}
k = LM->m * LM->n;
for( i=0; i<k; i++)
LM->fjac[i] = 0.0;
return 0;
}
void FreeLMStruct( struct LMStruct *LM )
{
if(LM->x != NULL) free( LM->x );
if(LM->fvec != NULL) free( LM->fvec );
if(LM->diag != NULL) free( LM->diag );
if(LM->qtf != NULL) free( LM->qtf );
if(LM->wa1 != NULL) free( LM->wa1 );
if(LM->wa2 != NULL) free( LM->wa2 );
if(LM->wa3 != NULL) free( LM->wa3 );
if(LM->wa4 != NULL) free( LM->wa4 );
if(LM->fjac != NULL) free( LM->fjac );
if(LM->ipvt != NULL) free( LM->ipvt );
}
void bracket( struct LMStruct *LM )
{
int iflag = 1,i;
double eps, delta, delta_max;
int changed, c = 1;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
if( iflag < 0 ) return;
// and do a print
iflag = 0;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
if( iflag < 0 ) return;
iflag = 1;
eps = sumSquared( LM->fvec, LM->m );
// Choose delta_max to be between 1 and 2 degrees
if( LM->epsfcn <= 0.0 ) return; // This is an error
for( delta_max = LM->epsfcn; delta_max < 1.0; delta_max *= 2.0){}
for( delta = delta_max;
delta >= LM->epsfcn;
delta /= 2.0 )
{
c = 1;
// PrintError("delta = %lf", delta);
while( c )
{
c = 0;
for( i = 0; i < LM->n; i++ )
{
changed = 0;
LM->x[i] += delta;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag); if( iflag < 0 ) return;
if( delta == delta_max ) // search everywhere
{
while( sumSquared( LM->fvec, LM->m ) < eps )
{
changed = 1;
eps = sumSquared( LM->fvec, LM->m );
LM->x[i] += delta;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);if( iflag < 0 ) return;
}
LM->x[i] -= delta;
}
else // do just this one step
{
if( sumSquared( LM->fvec, LM->m ) < eps )
{
eps = sumSquared( LM->fvec, LM->m );
changed = 1;
}
else
LM->x[i] -= delta;
}
if( !changed ) // Try other direction
{
LM->x[i] -= delta;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);if( iflag < 0 ) return;
if( delta == delta_max ) // search everywhere
{
while( sumSquared( LM->fvec, LM->m ) < eps )
{
changed = 1;
eps = sumSquared( LM->fvec, LM->m );
LM->x[i] -= delta;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);if( iflag < 0 ) return;
}
LM->x[i] += delta;
}
else // do just this one step
{
if( sumSquared( LM->fvec, LM->m ) < eps )
{
eps = sumSquared( LM->fvec, LM->m );
changed = 1;
}
else
LM->x[i] += delta;
}
}
if( changed ) c = 1;
if (c) { // an improvement, let's see it (and give the user a chance to bail out)
iflag = 0;
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
if( iflag < 0 ) return;
iflag = 1;
}
}
}
// PrintError("%lf %ld %lf", delta, c, eps);
iflag = 0;
LM->fvec[0] = sqrt(eps/LM->m);
fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
if( iflag < 0 ) return;
iflag = 1;
}
}
double sumSquared( double *a, int n )
{
double result = 0.0;
int i;
for( i=0; i<n; i++ )
result += a[i] * a[i];
return result;
}