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
74 struct 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 */
LMBLEC_FUNC(LM_REAL * p,LM_REAL * hx,int m,int n,void * adata)83 static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)
84 {
85 struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
86 int nn;
87 register int i, j, *typ;
88 register 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 */
LMBLEC_JACF(LM_REAL * p,LM_REAL * jac,int m,int n,void * adata)116 static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)
117 {
118 struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
119 int nn, *typ;
120 register int i, j;
121 register 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 */
LEVMAR_BLEC_DER(void (* func)(LM_REAL * p,LM_REAL * hx,int m,int n,void * adata),void (* jacf)(LM_REAL * p,LM_REAL * j,int m,int n,void * adata),LM_REAL * p,LM_REAL * x,int m,int n,LM_REAL * lb,LM_REAL * ub,LM_REAL * A,LM_REAL * b,int k,LM_REAL * wghts,int itmax,LM_REAL opts[4],LM_REAL info[LM_INFO_SZ],LM_REAL * work,LM_REAL * covar,void * adata)185 int 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 */
LEVMAR_BLEC_DIF(void (* func)(LM_REAL * p,LM_REAL * hx,int m,int n,void * adata),LM_REAL * p,LM_REAL * x,int m,int n,LM_REAL * lb,LM_REAL * ub,LM_REAL * A,LM_REAL * b,int k,LM_REAL * wghts,int itmax,LM_REAL opts[5],LM_REAL info[LM_INFO_SZ],LM_REAL * work,LM_REAL * covar,void * adata)298 int 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
414