Diff of /src/optimize.c [000000] .. [24dce1]  Maximize  Restore

  Switch to unified view

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

Get latest updates about Open Source Projects, Conferences and News.

Sign up for the SourceForge newsletter:





No, thanks