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 #define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
74
75 struct LMBLEC_DATA{
76 LM_REAL *x, *lb, *ub, *w;
77 int *bctype;
78 void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
79 void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
80 void *adata;
81 };
82
83 /* augmented measurements */
LMBLEC_FUNC(LM_REAL * p,LM_REAL * hx,int m,int n,void * adata)84 static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)
85 {
86 struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
87 int nn;
88 register int i, j, *typ;
89 register LM_REAL *lb, *ub, *w, tmp;
90
91 nn=n-m;
92 lb=data->lb;
93 ub=data->ub;
94 w=data->w;
95 typ=data->bctype;
96 (*(data->func))(p, hx, m, nn, data->adata);
97
98 for(i=nn, j=0; i<n; ++i, ++j){
99 switch(typ[j]){
100 case __BC_INTERVAL__:
101 tmp=(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(ub[j]-lb[j]);
102 hx[i]=w[j]*__MAX__(tmp*tmp-LM_CNST(1.0), LM_CNST(0.0));
103 break;
104
105 case __BC_LOW__:
106 hx[i]=w[j]*__MAX__(lb[j]-p[j], LM_CNST(0.0));
107 break;
108
109 case __BC_HIGH__:
110 hx[i]=w[j]*__MAX__(p[j]-ub[j], LM_CNST(0.0));
111 break;
112 }
113 }
114 }
115
116 /* augmented Jacobian */
LMBLEC_JACF(LM_REAL * p,LM_REAL * jac,int m,int n,void * adata)117 static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)
118 {
119 struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
120 int nn, *typ;
121 register int i, j;
122 register LM_REAL *lb, *ub, *w, tmp;
123
124 nn=n-m;
125 lb=data->lb;
126 ub=data->ub;
127 w=data->w;
128 typ=data->bctype;
129 (*(data->jacf))(p, jac, m, nn, data->adata);
130
131 /* clear all extra rows */
132 for(i=nn*m; i<n*m; ++i)
133 jac[i]=0.0;
134
135 for(i=nn, j=0; i<n; ++i, ++j){
136 switch(typ[j]){
137 case __BC_INTERVAL__:
138 if(lb[j]<=p[j] && p[j]<=ub[j])
139 continue; // corresp. jac element already 0
140
141 /* out of interval */
142 tmp=ub[j]-lb[j];
143 tmp=LM_CNST(4.0)*(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(tmp*tmp);
144 jac[i*m+j]=w[j]*tmp;
145 break;
146
147 case __BC_LOW__: // (lb[j]<=p[j])? 0.0 : -1.0;
148 if(lb[j]<=p[j])
149 continue; // corresp. jac element already 0
150
151 /* smaller than lower bound */
152 jac[i*m+j]=-w[j];
153 break;
154
155 case __BC_HIGH__: // (p[j]<=ub[j])? 0.0 : 1.0;
156 if(p[j]<=ub[j])
157 continue; // corresp. jac element already 0
158
159 /* greater than upper bound */
160 jac[i*m+j]=w[j];
161 break;
162 }
163 }
164 }
165
166 /*
167 * This function seeks the parameter vector p that best describes the measurements
168 * vector x under box & linear constraints.
169 * More precisely, given a vector function func : R^m --> R^n with n>=m,
170 * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
171 * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i] and A p=b;
172 * A is kxm, b kx1. Note that this function DOES NOT check the satisfiability of
173 * the specified box and linear equation constraints.
174 * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
175 * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
176 *
177 * This function requires an analytic Jacobian. In case the latter is unavailable,
178 * use LEVMAR_BLEC_DIF() bellow
179 *
180 * Returns the number of iterations (>=0) if successfull, LM_ERROR if failed
181 *
182 * For more details on the algorithm implemented by this function, please refer to
183 * the comments in the top of this file.
184 *
185 */
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)186 int LEVMAR_BLEC_DER(
187 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 */
188 void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */
189 LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
190 LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
191 int m, /* I: parameter vector dimension (i.e. #unknowns) */
192 int n, /* I: measurement vector dimension */
193 LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
194 LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
195 LM_REAL *A, /* I: constraints matrix, kxm */
196 LM_REAL *b, /* I: right hand constraints vector, kx1 */
197 int k, /* I: number of constraints (i.e. A's #rows) */
198 LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */
199 int itmax, /* I: maximum number of iterations */
200 LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
201 * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
202 */
203 LM_REAL info[LM_INFO_SZ],
204 /* O: information regarding the minimization. Set to NULL if don't care
205 * info[0]= ||e||_2 at initial p.
206 * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
207 * info[5]= # iterations,
208 * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
209 * 2 - stopped by small Dp
210 * 3 - stopped by itmax
211 * 4 - singular matrix. Restart from current p with increased mu
212 * 5 - no further error reduction is possible. Restart with increased mu
213 * 6 - stopped by small ||e||_2
214 * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
215 * info[7]= # function evaluations
216 * info[8]= # Jacobian evaluations
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(!LEVMAR_BOX_CHECK(lb, ub, m)){
236 fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
237 return LM_ERROR;
238 }
239
240 /* measurement vector needs to be extended by m */
241 if(x){ /* nonzero x */
242 data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
243 if(!data.x){
244 fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
245 exit(1);
246 }
247
248 for(i=0; i<n; ++i)
249 data.x[i]=x[i];
250 for(i=n; i<n+m; ++i)
251 data.x[i]=0.0;
252 }
253 else
254 data.x=NULL;
255
256 data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int));
257 if(!data.w){
258 fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
259 exit(1);
260 }
261 data.bctype=(int *)(data.w+m);
262
263 for(i=0; i<m; ++i){
264 data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
265 if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
266 else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
267 else data.bctype[i]=__BC_HIGH__;
268 }
269
270 data.lb=lb;
271 data.ub=ub;
272 data.func=func;
273 data.jacf=jacf;
274 data.adata=adata;
275
276 if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */
277 ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
278
279 if(data.x) free(data.x);
280 free(data.w);
281
282 return ret;
283 }
284
285 /* Similar to the LEVMAR_BLEC_DER() function above, except that the Jacobian is approximated
286 * with the aid of finite differences (forward or central, see the comment for the opts argument)
287 */
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)288 int LEVMAR_BLEC_DIF(
289 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 */
290 LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
291 LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
292 int m, /* I: parameter vector dimension (i.e. #unknowns) */
293 int n, /* I: measurement vector dimension */
294 LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
295 LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
296 LM_REAL *A, /* I: constraints matrix, kxm */
297 LM_REAL *b, /* I: right hand constraints vector, kx1 */
298 int k, /* I: number of constraints (i.e. A's #rows) */
299 LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */
300 int itmax, /* I: maximum number of iterations */
301 LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
302 * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
303 * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
304 * If \delta<0, the Jacobian is approximated with central differences which are more accurate
305 * (but slower!) compared to the forward differences employed by default.
306 */
307 LM_REAL info[LM_INFO_SZ],
308 /* O: information regarding the minimization. Set to NULL if don't care
309 * info[0]= ||e||_2 at initial p.
310 * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
311 * info[5]= # iterations,
312 * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
313 * 2 - stopped by small Dp
314 * 3 - stopped by itmax
315 * 4 - singular matrix. Restart from current p with increased mu
316 * 5 - no further error reduction is possible. Restart with increased mu
317 * 6 - stopped by small ||e||_2
318 * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
319 * info[7]= # function evaluations
320 * info[8]= # Jacobian evaluations
321 */
322 LM_REAL *work, /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated if NULL */
323 LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
324 void *adata) /* pointer to possibly additional data, passed uninterpreted to func.
325 * Set to NULL if not needed
326 */
327 {
328 struct LMBLEC_DATA data;
329 int ret;
330 register int i;
331 LM_REAL locinfo[LM_INFO_SZ];
332
333 if(!LEVMAR_BOX_CHECK(lb, ub, m)){
334 fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
335 return LM_ERROR;
336 }
337
338 /* measurement vector needs to be extended by m */
339 if(x){ /* nonzero x */
340 data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
341 if(!data.x){
342 fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
343 exit(1);
344 }
345
346 for(i=0; i<n; ++i)
347 data.x[i]=x[i];
348 for(i=n; i<n+m; ++i)
349 data.x[i]=0.0;
350 }
351 else
352 data.x=NULL;
353
354 data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int));
355 if(!data.w){
356 fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
357 exit(1);
358 }
359 data.bctype=(int *)(data.w+m);
360
361 for(i=0; i<m; ++i){
362 data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
363 if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
364 else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
365 else data.bctype[i]=__BC_HIGH__;
366 }
367
368 data.lb=lb;
369 data.ub=ub;
370 data.func=func;
371 data.jacf=NULL;
372 data.adata=adata;
373
374 if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */
375 ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
376
377 if(data.x) free(data.x);
378 free(data.w);
379
380 return ret;
381 }
382
383 /* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
384 #undef LEVMAR_BOX_CHECK
385 #undef LMBLEC_DATA
386 #undef LMBLEC_FUNC
387 #undef LMBLEC_JACF
388 #undef LEVMAR_FDIF_FORW_JAC_APPROX
389 #undef LEVMAR_COVAR
390 #undef LEVMAR_LEC_DER
391 #undef LEVMAR_LEC_DIF
392 #undef LEVMAR_BLEC_DER
393 #undef LEVMAR_BLEC_DIF
394