source: branches/2929_PrioritizedGrammarEnumeration/HeuristicLab.Algorithms.DataAnalysis.PGE/3.3/go-code/go-levmar/levmar-2.6/lmblec_core.c @ 16080

Last change on this file since 16080 was 16080, checked in by hmaislin, 15 months ago

#2929 initial commit of working PGE version

File size: 17.7 KB
Line 
1/////////////////////////////////////////////////////////////////////////////////
2//
3//  Levenberg - Marquardt non-linear minimization algorithm
4//  Copyright (C) 2004-06  Manolis Lourakis (lourakis at ics forth gr)
5//  Institute of Computer Science, Foundation for Research & Technology - Hellas
6//  Heraklion, Crete, Greece.
7//
8//  This program is free software; you can redistribute it and/or modify
9//  it under the terms of the GNU General Public License as published by
10//  the Free Software Foundation; either version 2 of the License, or
11//  (at your option) any later version.
12//
13//  This program is distributed in the hope that it will be useful,
14//  but WITHOUT ANY WARRANTY; without even the implied warranty of
15//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16//  GNU General Public License for more details.
17//
18/////////////////////////////////////////////////////////////////////////////////
19
20/*******************************************************************************
21 * This file implements combined box and linear equation constraints.
22 *
23 * Note that the algorithm implementing linearly constrained minimization does
24 * so by a change in parameters that transforms the original program into an
25 * unconstrained one. To employ the same idea for implementing box & linear
26 * constraints would require the transformation of box constraints on the
27 * original parameters to box constraints for the new parameter set. This
28 * being impossible, a different approach is used here for finding the minimum.
29 * The trick is to remove the box constraints by augmenting the function to
30 * be fitted with penalty terms and then solve the resulting problem (which
31 * involves linear constrains only) with the functions in lmlec.c
32 *
33 * More specifically, for the constraint a<=x[i]<=b to hold, the term C[i]=
34 * (2*x[i]-(a+b))/(b-a) should be within [-1, 1]. This is enforced by adding
35 * the penalty term w[i]*max((C[i])^2-1, 0) to the objective function, where
36 * w[i] is a large weight. In the case of constraints of the form a<=x[i],
37 * the term C[i]=a-x[i] has to be non positive, thus the penalty term is
38 * w[i]*max(C[i], 0). If x[i]<=b, C[i]=x[i]-b has to be non negative and
39 * the penalty is w[i]*max(C[i], 0). The derivatives needed for the Jacobian
40 * are as follows:
41 * For the constraint a<=x[i]<=b: 4*(2*x[i]-(a+b))/(b-a)^2 if x[i] not in [a, b],
42 *                                0 otherwise
43 * For the constraint a<=x[i]: -1 if x[i]<=a, 0 otherwise
44 * For the constraint x[i]<=b: 1 if b<=x[i], 0 otherwise
45 *
46 * Note that for the above to work, the weights w[i] should be large enough;
47 * depending on your minimization problem, the default values might need some
48 * tweaking (see arg "wghts" below).
49 *******************************************************************************/
50
51#ifndef LM_REAL // not included by lmblec.c
52#error This file should not be compiled directly!
53#endif
54
55
56#define __MAX__(x, y)   (((x)>=(y))? (x) : (y))
57#define __BC_WEIGHT__   LM_CNST(1E+04)
58
59#define __BC_INTERVAL__ 0
60#define __BC_LOW__      1
61#define __BC_HIGH__     2
62
63/* precision-specific definitions */
64#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
65#define LMBLEC_DATA LM_ADD_PREFIX(lmblec_data)
66#define LMBLEC_FUNC LM_ADD_PREFIX(lmblec_func)
67#define LMBLEC_JACF LM_ADD_PREFIX(lmblec_jacf)
68#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
69#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
70#define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der)
71#define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif)
72#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
73
74struct LMBLEC_DATA{
75  LM_REAL *x, *lb, *ub, *w;
76  int *bctype;
77  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
78  void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
79  void *adata;
80};
81
82/* augmented measurements */
83static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)
84{
85struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
86int nn;
87register int i, j, *typ;
88register LM_REAL *lb, *ub, *w, tmp;
89
90  nn=n-m;
91  lb=data->lb;
92  ub=data->ub;
93  w=data->w;
94  typ=data->bctype;
95  (*(data->func))(p, hx, m, nn, data->adata);
96
97  for(i=nn, j=0; i<n; ++i, ++j){
98    switch(typ[j]){
99      case __BC_INTERVAL__:
100        tmp=(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(ub[j]-lb[j]);
101        hx[i]=w[j]*__MAX__(tmp*tmp-LM_CNST(1.0), LM_CNST(0.0));
102      break;
103
104      case __BC_LOW__:
105        hx[i]=w[j]*__MAX__(lb[j]-p[j], LM_CNST(0.0));
106      break;
107
108      case __BC_HIGH__:
109        hx[i]=w[j]*__MAX__(p[j]-ub[j], LM_CNST(0.0));
110      break;
111    }
112  }
113}
114
115/* augmented Jacobian */
116static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)
117{
118struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
119int nn, *typ;
120register int i, j;
121register LM_REAL *lb, *ub, *w, tmp;
122
123  nn=n-m;
124  lb=data->lb;
125  ub=data->ub;
126  w=data->w;
127  typ=data->bctype;
128  (*(data->jacf))(p, jac, m, nn, data->adata);
129
130  /* clear all extra rows */
131  for(i=nn*m; i<n*m; ++i)
132    jac[i]=0.0;
133
134  for(i=nn, j=0; i<n; ++i, ++j){
135    switch(typ[j]){
136      case __BC_INTERVAL__:
137        if(lb[j]<=p[j] && p[j]<=ub[j])
138          continue; // corresp. jac element already 0
139
140        /* out of interval */
141        tmp=ub[j]-lb[j];
142        tmp=LM_CNST(4.0)*(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(tmp*tmp);
143        jac[i*m+j]=w[j]*tmp;
144      break;
145
146      case __BC_LOW__: // (lb[j]<=p[j])? 0.0 : -1.0;
147        if(lb[j]<=p[j])
148          continue; // corresp. jac element already 0
149
150        /* smaller than lower bound */
151        jac[i*m+j]=-w[j];
152      break;
153
154      case __BC_HIGH__: // (p[j]<=ub[j])? 0.0 : 1.0;
155        if(p[j]<=ub[j])
156          continue; // corresp. jac element already 0
157
158        /* greater than upper bound */
159        jac[i*m+j]=w[j];
160      break;
161    }
162  }
163}
164
165/*
166 * This function seeks the parameter vector p that best describes the measurements
167 * vector x under box & linear constraints.
168 * More precisely, given a vector function  func : R^m --> R^n with n>=m,
169 * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
170 * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i] and A p=b;
171 * A is kxm, b kx1. Note that this function DOES NOT check the satisfiability of
172 * the specified box and linear equation constraints.
173 * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
174 * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
175 *
176 * This function requires an analytic Jacobian. In case the latter is unavailable,
177 * use LEVMAR_BLEC_DIF() bellow
178 *
179 * Returns the number of iterations (>=0) if successful, LM_ERROR if failed
180 *
181 * For more details on the algorithm implemented by this function, please refer to
182 * the comments in the top of this file.
183 *
184 */
185int LEVMAR_BLEC_DER(
186  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
187  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ 
188  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
189  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
190  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
191  int n,              /* I: measurement vector dimension */
192  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
193  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
194  LM_REAL *A,         /* I: constraints matrix, kxm */
195  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
196  int k,              /* I: number of constraints (i.e. A's #rows) */
197  LM_REAL *wghts,     /* mx1 weights for penalty terms, defaults used if NULL */
198  int itmax,          /* I: maximum number of iterations */
199  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
200                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
201                       */
202  LM_REAL info[LM_INFO_SZ],
203                     /* O: information regarding the minimization. Set to NULL if don't care
204                      * info[0]= ||e||_2 at initial p.
205                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
206                      * info[5]= # iterations,
207                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
208                      *                                 2 - stopped by small Dp
209                      *                                 3 - stopped by itmax
210                      *                                 4 - singular matrix. Restart from current p with increased mu
211                      *                                 5 - no further error reduction is possible. Restart with increased mu
212                      *                                 6 - stopped by small ||e||_2
213                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
214                      * info[7]= # function evaluations
215                      * info[8]= # Jacobian evaluations
216                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
217                      */
218  LM_REAL *work,     /* working memory at least LM_BLEC_DER_WORKSZ() reals large, allocated if NULL */
219  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
220  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
221                      * Set to NULL if not needed
222                      */
223{
224  struct LMBLEC_DATA data;
225  int ret;
226  LM_REAL locinfo[LM_INFO_SZ];
227  register int i;
228
229  if(!jacf){
230    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEC_DER)
231      RCAT("().\nIf no such function is available, use ", LEVMAR_BLEC_DIF) RCAT("() rather than ", LEVMAR_BLEC_DER) "()\n");
232    return LM_ERROR;
233  }
234
235  if(!lb && !ub){
236    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DER, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
237          LEVMAR_LEC_DER) "() in this case!\n");
238    return LM_ERROR;
239  }
240
241  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
242    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
243    return LM_ERROR;
244  }
245
246  /* measurement vector needs to be extended by m */
247  if(x){ /* nonzero x */
248    data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
249    if(!data.x){
250      fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
251      return LM_ERROR;
252    }
253
254    for(i=0; i<n; ++i)
255      data.x[i]=x[i];
256    for(i=n; i<n+m; ++i)
257      data.x[i]=0.0;
258  }
259  else
260    data.x=NULL;
261
262  data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
263  if(!data.w){
264    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
265    if(data.x) free(data.x);
266    return LM_ERROR;
267  }
268  data.bctype=(int *)(data.w+m);
269
270  /* note: at this point, one of lb, ub are not NULL */
271  for(i=0; i<m; ++i){
272    data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
273    if(!lb) data.bctype[i]=__BC_HIGH__;
274    else if(!ub) data.bctype[i]=__BC_LOW__;
275    else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
276    else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
277    else data.bctype[i]=__BC_HIGH__;
278  }
279
280  data.lb=lb;
281  data.ub=ub;
282  data.func=func;
283  data.jacf=jacf;
284  data.adata=adata;
285
286  if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */
287  ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
288
289  if(data.x) free(data.x);
290  free(data.w);
291
292  return ret;
293}
294
295/* Similar to the LEVMAR_BLEC_DER() function above, except that the Jacobian is approximated
296 * with the aid of finite differences (forward or central, see the comment for the opts argument)
297 */
298int LEVMAR_BLEC_DIF(
299  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
300  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
301  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
302  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
303  int n,              /* I: measurement vector dimension */
304  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
305  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
306  LM_REAL *A,         /* I: constraints matrix, kxm */
307  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
308  int k,              /* I: number of constraints (i.e. A's #rows) */
309  LM_REAL *wghts,     /* mx1 weights for penalty terms, defaults used if NULL */
310  int itmax,          /* I: maximum number of iterations */
311  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
312                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
313                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
314                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
315                       * (but slower!) compared to the forward differences employed by default.
316                       */
317  LM_REAL info[LM_INFO_SZ],
318                     /* O: information regarding the minimization. Set to NULL if don't care
319                      * info[0]= ||e||_2 at initial p.
320                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
321                      * info[5]= # iterations,
322                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
323                      *                                 2 - stopped by small Dp
324                      *                                 3 - stopped by itmax
325                      *                                 4 - singular matrix. Restart from current p with increased mu
326                      *                                 5 - no further error reduction is possible. Restart with increased mu
327                      *                                 6 - stopped by small ||e||_2
328                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
329                      * info[7]= # function evaluations
330                      * info[8]= # Jacobian evaluations
331                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
332                      */
333  LM_REAL *work,     /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated if NULL */
334  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
335  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
336                      * Set to NULL if not needed
337                      */
338{
339  struct LMBLEC_DATA data;
340  int ret;
341  register int i;
342  LM_REAL locinfo[LM_INFO_SZ];
343
344  if(!lb && !ub){
345    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DIF, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
346          LEVMAR_LEC_DIF) "() in this case!\n");
347    return LM_ERROR;
348  }
349
350  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
351    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
352    return LM_ERROR;
353  }
354
355  /* measurement vector needs to be extended by m */
356  if(x){ /* nonzero x */
357    data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
358    if(!data.x){
359      fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
360      return LM_ERROR;
361    }
362
363    for(i=0; i<n; ++i)
364      data.x[i]=x[i];
365    for(i=n; i<n+m; ++i)
366      data.x[i]=0.0;
367  }
368  else
369    data.x=NULL;
370
371  data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
372  if(!data.w){
373    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
374    if(data.x) free(data.x);
375    return LM_ERROR;
376  }
377  data.bctype=(int *)(data.w+m);
378
379  /* note: at this point, one of lb, ub are not NULL */
380  for(i=0; i<m; ++i){
381    data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
382    if(!lb) data.bctype[i]=__BC_HIGH__;
383    else if(!ub) data.bctype[i]=__BC_LOW__;
384    else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
385    else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
386    else data.bctype[i]=__BC_HIGH__;
387  }
388
389  data.lb=lb;
390  data.ub=ub;
391  data.func=func;
392  data.jacf=NULL;
393  data.adata=adata;
394
395  if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */
396  ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
397
398  if(data.x) free(data.x);
399  free(data.w);
400
401  return ret;
402}
403
404/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
405#undef LEVMAR_BOX_CHECK
406#undef LMBLEC_DATA
407#undef LMBLEC_FUNC
408#undef LMBLEC_JACF
409#undef LEVMAR_COVAR
410#undef LEVMAR_LEC_DER
411#undef LEVMAR_LEC_DIF
412#undef LEVMAR_BLEC_DER
413#undef LEVMAR_BLEC_DIF
Note: See TracBrowser for help on using the repository browser.