1 /////////////////////////////////////////////////////////////////////////////////
2 //
3 // Levenberg - Marquardt non-linear minimization algorithm
4 // Copyright (C) 2004-05 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 #ifndef LM_REAL // not included by lmbc.c
21 #error This file should not be compiled directly!
22 #endif
23
24
25 /* precision-specific definitions */
26 #define FUNC_STATE LM_ADD_PREFIX(func_state)
27 #define LNSRCH LM_ADD_PREFIX(lnsrch)
28 #define BOXPROJECT LM_ADD_PREFIX(boxProject)
29 #define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
30 #define LEVMAR_BC_DER LM_ADD_PREFIX(levmar_bc_der)
31 #define LEVMAR_BC_DIF LM_ADD_PREFIX(levmar_bc_dif) //CHECKME
32 #define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
33 #define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
34 #define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
35 #define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
36 #define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
37 #define LMBC_DIF_DATA LM_ADD_PREFIX(lmbc_dif_data)
38 #define LMBC_DIF_FUNC LM_ADD_PREFIX(lmbc_dif_func)
39 #define LMBC_DIF_JACF LM_ADD_PREFIX(lmbc_dif_jacf)
40
41 #ifdef HAVE_LAPACK
42 #define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
43 #define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
44 #define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
45 #define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
46 #define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
47 #else
48 #define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
49 #endif /* HAVE_LAPACK */
50
51 /* find the median of 3 numbers */
52 #define __MEDIAN3(a, b, c) ( ((a) >= (b))?\
53 ( ((c) >= (a))? (a) : ( ((c) <= (b))? (b) : (c) ) ) : \
54 ( ((c) >= (b))? (b) : ( ((c) <= (a))? (a) : (c) ) ) )
55
56 #define _POW_ LM_CNST(2.1)
57
58 #define __LSITMAX 150 // max #iterations for line search
59
60 struct FUNC_STATE{
61 int n, *nfev;
62 LM_REAL *hx, *x;
63 void *adata;
64 };
65
66 static void
LNSRCH(int m,LM_REAL * x,LM_REAL f,LM_REAL * g,LM_REAL * p,LM_REAL alpha,LM_REAL * xpls,LM_REAL * ffpls,void (* func)(LM_REAL * p,LM_REAL * hx,int m,int n,void * adata),struct FUNC_STATE state,int * mxtake,int * iretcd,LM_REAL stepmx,LM_REAL steptl,LM_REAL * sx)67 LNSRCH(int m, LM_REAL *x, LM_REAL f, LM_REAL *g, LM_REAL *p, LM_REAL alpha, LM_REAL *xpls,
68 LM_REAL *ffpls, void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), struct FUNC_STATE state,
69 int *mxtake, int *iretcd, LM_REAL stepmx, LM_REAL steptl, LM_REAL *sx)
70 {
71 /* Find a next newton iterate by backtracking line search.
72 * Specifically, finds a \lambda such that for a fixed alpha<0.5 (usually 1e-4),
73 * f(x + \lambda*p) <= f(x) + alpha * \lambda * g^T*p
74 *
75 * Translated (with minor changes) from Schnabel, Koontz & Weiss uncmin.f, v1.3
76
77 * PARAMETERS :
78
79 * m --> dimension of problem (i.e. number of variables)
80 * x(m) --> old iterate: x[k-1]
81 * f --> function value at old iterate, f(x)
82 * g(m) --> gradient at old iterate, g(x), or approximate
83 * p(m) --> non-zero newton step
84 * alpha --> fixed constant < 0.5 for line search (see above)
85 * xpls(m) <-- new iterate x[k]
86 * ffpls <-- function value at new iterate, f(xpls)
87 * func --> name of subroutine to evaluate function
88 * state <--> information other than x and m that func requires.
89 * state is not modified in xlnsrch (but can be modified by func).
90 * iretcd <-- return code
91 * mxtake <-- boolean flag indicating step of maximum length used
92 * stepmx --> maximum allowable step size
93 * steptl --> relative step size at which successive iterates
94 * considered close enough to terminate algorithm
95 * sx(m) --> diagonal scaling matrix for x, can be NULL
96
97 * internal variables
98
99 * sln newton length
100 * rln relative length of newton step
101 */
102
103 register int i, j;
104 int firstback = 1;
105 LM_REAL disc;
106 LM_REAL a3, b;
107 LM_REAL t1, t2, t3, lambda, tlmbda, rmnlmb;
108 LM_REAL scl, rln, sln, slp;
109 LM_REAL tmp1, tmp2;
110 LM_REAL fpls, pfpls = 0., plmbda = 0.; /* -Wall */
111
112 f*=LM_CNST(0.5);
113 *mxtake = 0;
114 *iretcd = 2;
115 tmp1 = 0.;
116 if(!sx) /* no scaling */
117 for (i = 0; i < m; ++i)
118 tmp1 += p[i] * p[i];
119 else
120 for (i = 0; i < m; ++i)
121 tmp1 += sx[i] * sx[i] * p[i] * p[i];
122 sln = (LM_REAL)sqrt(tmp1);
123 if (sln > stepmx) {
124 /* newton step longer than maximum allowed */
125 scl = stepmx / sln;
126 for(i=0; i<m; ++i) /* p * scl */
127 p[i]*=scl;
128 sln = stepmx;
129 }
130 for(i=0, slp=0.; i<m; ++i) /* g^T * p */
131 slp+=g[i]*p[i];
132 rln = 0.;
133 if(!sx) /* no scaling */
134 for (i = 0; i < m; ++i) {
135 tmp1 = (FABS(x[i])>=LM_CNST(1.))? FABS(x[i]) : LM_CNST(1.);
136 tmp2 = FABS(p[i])/tmp1;
137 if(rln < tmp2) rln = tmp2;
138 }
139 else
140 for (i = 0; i < m; ++i) {
141 tmp1 = (FABS(x[i])>=LM_CNST(1.)/sx[i])? FABS(x[i]) : LM_CNST(1.)/sx[i];
142 tmp2 = FABS(p[i])/tmp1;
143 if(rln < tmp2) rln = tmp2;
144 }
145 rmnlmb = steptl / rln;
146 lambda = LM_CNST(1.0);
147
148 /* check if new iterate satisfactory. generate new lambda if necessary. */
149
150 for(j=__LSITMAX; j>=0; --j) {
151 for (i = 0; i < m; ++i)
152 xpls[i] = x[i] + lambda * p[i];
153
154 /* evaluate function at new point */
155 (*func)(xpls, state.hx, m, state.n, state.adata); ++(*(state.nfev));
156 /* ### state.hx=state.x-state.hx, tmp1=||state.hx|| */
157 #if 1
158 tmp1=LEVMAR_L2NRMXMY(state.hx, state.x, state.hx, state.n);
159 #else
160 for(i=0, tmp1=0.0; i<state.n; ++i){
161 state.hx[i]=tmp2=state.x[i]-state.hx[i];
162 tmp1+=tmp2*tmp2;
163 }
164 #endif
165 fpls=LM_CNST(0.5)*tmp1; *ffpls=tmp1;
166
167 if (fpls <= f + slp * alpha * lambda) { /* solution found */
168 *iretcd = 0;
169 if (lambda == LM_CNST(1.) && sln > stepmx * LM_CNST(.99)) *mxtake = 1;
170 return;
171 }
172
173 /* else : solution not (yet) found */
174
175 /* First find a point with a finite value */
176
177 if (lambda < rmnlmb) {
178 /* no satisfactory xpls found sufficiently distinct from x */
179
180 *iretcd = 1;
181 return;
182 }
183 else { /* calculate new lambda */
184
185 /* modifications to cover non-finite values */
186 if (!LM_FINITE(fpls)) {
187 lambda *= LM_CNST(0.1);
188 firstback = 1;
189 }
190 else {
191 if (firstback) { /* first backtrack: quadratic fit */
192 tlmbda = -lambda * slp / ((fpls - f - slp) * LM_CNST(2.));
193 firstback = 0;
194 }
195 else { /* all subsequent backtracks: cubic fit */
196 t1 = fpls - f - lambda * slp;
197 t2 = pfpls - f - plmbda * slp;
198 t3 = LM_CNST(1.) / (lambda - plmbda);
199 a3 = LM_CNST(3.) * t3 * (t1 / (lambda * lambda)
200 - t2 / (plmbda * plmbda));
201 b = t3 * (t2 * lambda / (plmbda * plmbda)
202 - t1 * plmbda / (lambda * lambda));
203 disc = b * b - a3 * slp;
204 if (disc > b * b)
205 /* only one positive critical point, must be minimum */
206 tlmbda = (-b + ((a3 < 0)? -(LM_REAL)sqrt(disc): (LM_REAL)sqrt(disc))) /a3;
207 else
208 /* both critical points positive, first is minimum */
209 tlmbda = (-b + ((a3 < 0)? (LM_REAL)sqrt(disc): -(LM_REAL)sqrt(disc))) /a3;
210
211 if (tlmbda > lambda * LM_CNST(.5))
212 tlmbda = lambda * LM_CNST(.5);
213 }
214 plmbda = lambda;
215 pfpls = fpls;
216 if (tlmbda < lambda * LM_CNST(.1))
217 lambda *= LM_CNST(.1);
218 else
219 lambda = tlmbda;
220 }
221 }
222 }
223 /* this point is reached when the iterations limit is exceeded */
224 *iretcd = 1; /* failed */
225 return;
226 } /* LNSRCH */
227
228 /* Projections to feasible set \Omega: P_{\Omega}(y) := arg min { ||x - y|| : x \in \Omega}, y \in R^m */
229
230 /* project vector p to a box shaped feasible set. p is a mx1 vector.
231 * Either lb, ub can be NULL. If not NULL, they are mx1 vectors
232 */
BOXPROJECT(LM_REAL * p,LM_REAL * lb,LM_REAL * ub,int m)233 static void BOXPROJECT(LM_REAL *p, LM_REAL *lb, LM_REAL *ub, int m)
234 {
235 register int i;
236
237 if(!lb){ /* no lower bounds */
238 if(!ub) /* no upper bounds */
239 return;
240 else{ /* upper bounds only */
241 for(i=0; i<m; ++i)
242 if(p[i]>ub[i]) p[i]=ub[i];
243 }
244 }
245 else
246 if(!ub){ /* lower bounds only */
247 for(i=0; i<m; ++i)
248 if(p[i]<lb[i]) p[i]=lb[i];
249 }
250 else /* box bounds */
251 for(i=0; i<m; ++i)
252 p[i]=__MEDIAN3(lb[i], p[i], ub[i]);
253 }
254
255 /*
256 * This function seeks the parameter vector p that best describes the measurements
257 * vector x under box constraints.
258 * More precisely, given a vector function func : R^m --> R^n with n>=m,
259 * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
260 * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i].
261 * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
262 * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
263 *
264 * This function requires an analytic Jacobian. In case the latter is unavailable,
265 * use LEVMAR_BC_DIF() bellow
266 *
267 * Returns the number of iterations (>=0) if successfull, LM_ERROR if failed
268 *
269 * For details, see C. Kanzow, N. Yamashita and M. Fukushima: "Levenberg-Marquardt
270 * methods for constrained nonlinear equations with strong local convergence properties",
271 * Journal of Computational and Applied Mathematics 172, 2004, pp. 375-397.
272 * Also, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on
273 * unconstrained Levenberg-Marquardt at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
274 */
275
LEVMAR_BC_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,int itmax,LM_REAL opts[4],LM_REAL info[LM_INFO_SZ],LM_REAL * work,LM_REAL * covar,void * adata)276 int LEVMAR_BC_DER(
277 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 */
278 void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */
279 LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
280 LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
281 int m, /* I: parameter vector dimension (i.e. #unknowns) */
282 int n, /* I: measurement vector dimension */
283 LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
284 LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
285 int itmax, /* I: maximum number of iterations */
286 LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
287 * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used.
288 * Note that ||J^T e||_inf is computed on free (not equal to lb[i] or ub[i]) variables only.
289 */
290 LM_REAL info[LM_INFO_SZ],
291 /* O: information regarding the minimization. Set to NULL if don't care
292 * info[0]= ||e||_2 at initial p.
293 * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
294 * info[5]= # iterations,
295 * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
296 * 2 - stopped by small Dp
297 * 3 - stopped by itmax
298 * 4 - singular matrix. Restart from current p with increased mu
299 * 5 - no further error reduction is possible. Restart with increased mu
300 * 6 - stopped by small ||e||_2
301 * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
302 * info[7]= # function evaluations
303 * info[8]= # Jacobian evaluations
304 */
305 LM_REAL *work, /* working memory at least LM_BC_DER_WORKSZ() reals large, allocated if NULL */
306 LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
307 void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.
308 * Set to NULL if not needed
309 */
310 {
311 register int i, j, k, l;
312 int worksz, freework=0, issolved;
313 /* temp work arrays */
314 LM_REAL *e, /* nx1 */
315 *hx, /* \hat{x}_i, nx1 */
316 *jacTe, /* J^T e_i mx1 */
317 *jac, /* nxm */
318 *jacTjac, /* mxm */
319 *Dp, /* mx1 */
320 *diag_jacTjac, /* diagonal of J^T J, mx1 */
321 *pDp; /* p + Dp, mx1 */
322
323 register LM_REAL mu, /* damping constant */
324 tmp; /* mainly used in matrix & vector multiplications */
325 LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
326 LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
327 LM_REAL tau, eps1, eps2, eps2_sq, eps3;
328 LM_REAL init_p_eL2;
329 int nu=2, nu2, stop=0, nfev, njev=0;
330 const int nm=n*m;
331
332 /* variables for constrained LM */
333 struct FUNC_STATE fstate;
334 LM_REAL alpha=LM_CNST(1e-4), beta=LM_CNST(0.9), gamma=LM_CNST(0.99995), gamma_sq=gamma*gamma, rho=LM_CNST(1e-8);
335 LM_REAL t, t0;
336 LM_REAL steptl=LM_CNST(1e3)*(LM_REAL)sqrt(LM_REAL_EPSILON), jacTeDp;
337 LM_REAL tmin=LM_CNST(1e-12), tming=LM_CNST(1e-18); /* minimum step length for LS and PG steps */
338 const LM_REAL tini=LM_CNST(1.0); /* initial step length for LS and PG steps */
339 int nLMsteps=0, nLSsteps=0, nPGsteps=0, gprevtaken=0;
340 int numactive;
341 int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
342
343 mu=jacTe_inf=t=0.0; tmin=tmin; /* -Wall */
344
345 if(n<m){
346 fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
347 return LM_ERROR;
348 }
349
350 if(!jacf){
351 fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BC_DER)
352 RCAT("().\nIf no such function is available, use ", LEVMAR_BC_DIF) RCAT("() rather than ", LEVMAR_BC_DER) "()\n");
353 return LM_ERROR;
354 }
355
356 if(!LEVMAR_BOX_CHECK(lb, ub, m)){
357 fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): at least one lower bound exceeds the upper one\n"));
358 return LM_ERROR;
359 }
360
361 if(opts){
362 tau=opts[0];
363 eps1=opts[1];
364 eps2=opts[2];
365 eps2_sq=opts[2]*opts[2];
366 eps3=opts[3];
367 }
368 else{ // use default values
369 tau=LM_CNST(LM_INIT_MU);
370 eps1=LM_CNST(LM_STOP_THRESH);
371 eps2=LM_CNST(LM_STOP_THRESH);
372 eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
373 eps3=LM_CNST(LM_STOP_THRESH);
374 }
375
376 if(!work){
377 worksz=LM_BC_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
378 work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
379 if(!work){
380 fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): memory allocation request failed\n"));
381 exit(1);
382 }
383 freework=1;
384 }
385
386 /* set up work arrays */
387 e=work;
388 hx=e + n;
389 jacTe=hx + n;
390 jac=jacTe + m;
391 jacTjac=jac + nm;
392 Dp=jacTjac + m*m;
393 diag_jacTjac=Dp + m;
394 pDp=diag_jacTjac + m;
395
396 fstate.n=n;
397 fstate.hx=hx;
398 fstate.x=x;
399 fstate.adata=adata;
400 fstate.nfev=&nfev;
401
402 /* see if starting point is within the feasile set */
403 for(i=0; i<m; ++i)
404 pDp[i]=p[i];
405 BOXPROJECT(p, lb, ub, m); /* project to feasible set */
406 for(i=0; i<m; ++i)
407 if(pDp[i]!=p[i])
408 fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_BC_DER) "()! [%g projected to %g]\n",
409 i, pDp[i], p[i]);
410
411 /* compute e=x - f(p) and its L2 norm */
412 (*func)(p, hx, m, n, adata); nfev=1;
413 /* ### e=x-hx, p_eL2=||e|| */
414 #if 1
415 p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
416 #else
417 for(i=0, p_eL2=0.0; i<n; ++i){
418 e[i]=tmp=x[i]-hx[i];
419 p_eL2+=tmp*tmp;
420 }
421 #endif
422 init_p_eL2=p_eL2;
423 if(!LM_FINITE(p_eL2)) stop=7;
424
425 for(k=0; k<itmax && !stop; ++k){
426 /* Note that p and e have been updated at a previous iteration */
427
428 if(p_eL2<=eps3){ /* error is small */
429 stop=6;
430 break;
431 }
432
433 /* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.
434 * Since J^T J is symmetric, its computation can be speeded up by computing
435 * only its upper triangular part and copying it to the lower part
436 */
437
438 (*jacf)(p, jac, m, n, adata); ++njev;
439
440 /* J^T J, J^T e */
441 if(nm<__BLOCKSZ__SQ){ // this is a small problem
442 /* This is the straightforward way to compute J^T J, J^T e. However, due to
443 * its noncontinuous memory access pattern, it incures many cache misses when
444 * applied to large minimization problems (i.e. problems involving a large
445 * number of free variables and measurements), in which J is too large to
446 * fit in the L1 cache. For such problems, a cache-efficient blocking scheme
447 * is preferable.
448 *
449 * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
450 * performance problem.
451 *
452 * On the other hand, the straightforward algorithm is faster on small
453 * problems since in this case it avoids the overheads of blocking.
454 */
455
456 for(i=0; i<m; ++i){
457 for(j=i; j<m; ++j){
458 int lm;
459
460 for(l=0, tmp=0.0; l<n; ++l){
461 lm=l*m;
462 tmp+=jac[lm+i]*jac[lm+j];
463 }
464
465 /* store tmp in the corresponding upper and lower part elements */
466 jacTjac[i*m+j]=jacTjac[j*m+i]=tmp;
467 }
468
469 /* J^T e */
470 for(l=0, tmp=0.0; l<n; ++l)
471 tmp+=jac[l*m+i]*e[l];
472 jacTe[i]=tmp;
473 }
474 }
475 else{ // this is a large problem
476 /* Cache efficient computation of J^T J based on blocking
477 */
478 LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
479
480 /* cache efficient computation of J^T e */
481 for(i=0; i<m; ++i)
482 jacTe[i]=0.0;
483
484 for(i=0; i<n; ++i){
485 register LM_REAL *jacrow;
486
487 for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
488 jacTe[l]+=jacrow[l]*tmp;
489 }
490 }
491
492 /* Compute ||J^T e||_inf and ||p||^2. Note that ||J^T e||_inf
493 * is computed for free (i.e. inactive) variables only.
494 * At a local minimum, if p[i]==ub[i] then g[i]>0;
495 * if p[i]==lb[i] g[i]<0; otherwise g[i]=0
496 */
497 for(i=j=numactive=0, p_L2=jacTe_inf=0.0; i<m; ++i){
498 if(ub && p[i]==ub[i]){ ++numactive; if(jacTe[i]>0.0) ++j; }
499 else if(lb && p[i]==lb[i]){ ++numactive; if(jacTe[i]<0.0) ++j; }
500 else if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
501
502 diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
503 p_L2+=p[i]*p[i];
504 }
505 //p_L2=sqrt(p_L2);
506
507 #if 0
508 if(!(k%100)){
509 printf("Current estimate: ");
510 for(i=0; i<m; ++i)
511 printf("%.9g ", p[i]);
512 printf("-- errors %.9g %0.9g, #active %d [%d]\n", jacTe_inf, p_eL2, numactive, j);
513 }
514 #endif
515
516 /* check for convergence */
517 if(j==numactive && (jacTe_inf <= eps1)){
518 Dp_L2=0.0; /* no increment for p in this case */
519 stop=1;
520 break;
521 }
522
523 /* compute initial damping factor */
524 if(k==0){
525 if(!lb && !ub){ /* no bounds */
526 for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
527 if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
528 mu=tau*tmp;
529 }
530 else
531 mu=LM_CNST(0.5)*tau*p_eL2; /* use Kanzow's starting mu */
532 }
533
534 /* determine increment using a combination of adaptive damping, line search and projected gradient search */
535 while(1){
536 /* augment normal equations */
537 for(i=0; i<m; ++i)
538 jacTjac[i*m+i]+=mu;
539
540 /* solve augmented equations */
541 #ifdef HAVE_LAPACK
542 /* 5 alternatives are available: LU, Cholesky, 2 variants of QR decomposition and SVD.
543 * Cholesky is the fastest but might be inaccurate; QR is slower but more accurate;
544 * SVD is the slowest but most accurate; LU offers a tradeoff between accuracy and speed
545 */
546
547 issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
548 //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_CHOL;
549 //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_QR;
550 //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); linsolver=AX_EQ_B_QRLS;
551 //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_SVD;
552
553 #else
554 /* use the LU included with levmar */
555 issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); linsolver=AX_EQ_B_LU;
556 #endif /* HAVE_LAPACK */
557
558 if(issolved){
559 for(i=0; i<m; ++i)
560 pDp[i]=p[i] + Dp[i];
561
562 /* compute p's new estimate and ||Dp||^2 */
563 BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
564 for(i=0, Dp_L2=0.0; i<m; ++i){
565 Dp[i]=tmp=pDp[i]-p[i];
566 Dp_L2+=tmp*tmp;
567 }
568 //Dp_L2=sqrt(Dp_L2);
569
570 if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
571 stop=2;
572 break;
573 }
574
575 if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
576 stop=4;
577 break;
578 }
579
580 (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
581 /* ### hx=x-hx, pDp_eL2=||hx|| */
582 #if 1
583 pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
584 #else
585 for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */
586 hx[i]=tmp=x[i]-hx[i];
587 pDp_eL2+=tmp*tmp;
588 }
589 #endif
590 if(!LM_FINITE(pDp_eL2)){
591 stop=7;
592 break;
593 }
594
595 if(pDp_eL2<=gamma_sq*p_eL2){
596 for(i=0, dL=0.0; i<m; ++i)
597 dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
598
599 #if 1
600 if(dL>0.0){
601 dF=p_eL2-pDp_eL2;
602 tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
603 tmp=LM_CNST(1.0)-tmp*tmp*tmp;
604 mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
605 }
606 else
607 mu=(mu>=pDp_eL2)? pDp_eL2 : mu; /* pDp_eL2 is the new pDp_eL2 */
608 #else
609
610 mu=(mu>=pDp_eL2)? pDp_eL2 : mu; /* pDp_eL2 is the new pDp_eL2 */
611 #endif
612
613 nu=2;
614
615 for(i=0 ; i<m; ++i) /* update p's estimate */
616 p[i]=pDp[i];
617
618 for(i=0; i<n; ++i) /* update e and ||e||_2 */
619 e[i]=hx[i];
620 p_eL2=pDp_eL2;
621 ++nLMsteps;
622 gprevtaken=0;
623 break;
624 }
625 }
626 else{
627
628 /* the augmented linear system could not be solved, increase mu */
629
630 mu*=nu;
631 nu2=nu<<1; // 2*nu;
632 if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
633 stop=5;
634 break;
635 }
636 nu=nu2;
637
638 for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
639 jacTjac[i*m+i]=diag_jacTjac[i];
640
641 continue; /* solve again with increased nu */
642 }
643
644 /* if this point is reached, the LM step did not reduce the error;
645 * see if it is a descent direction
646 */
647
648 /* negate jacTe (i.e. g) & compute g^T * Dp */
649 for(i=0, jacTeDp=0.0; i<m; ++i){
650 jacTe[i]=-jacTe[i];
651 jacTeDp+=jacTe[i]*Dp[i];
652 }
653
654 if(jacTeDp<=-rho*pow(Dp_L2, _POW_/LM_CNST(2.0))){
655 /* Dp is a descent direction; do a line search along it */
656 int mxtake, iretcd;
657 LM_REAL stepmx;
658
659 tmp=(LM_REAL)sqrt(p_L2); stepmx=LM_CNST(1e3)*( (tmp>=LM_CNST(1.0))? tmp : LM_CNST(1.0) );
660
661 #if 1
662 /* use Schnabel's backtracking line search; it requires fewer "func" evaluations */
663 LNSRCH(m, p, p_eL2, jacTe, Dp, alpha, pDp, &pDp_eL2, func, fstate,
664 &mxtake, &iretcd, stepmx, steptl, NULL); /* NOTE: LNSRCH() updates hx */
665 if(iretcd!=0) goto gradproj; /* rather inelegant but effective way to handle LNSRCH() failures... */
666 #else
667 /* use the simpler (but slower!) line search described by Kanzow et al */
668 for(t=tini; t>tmin; t*=beta){
669 for(i=0; i<m; ++i){
670 pDp[i]=p[i] + t*Dp[i];
671 //pDp[i]=__MEDIAN3(lb[i], pDp[i], ub[i]); /* project to feasible set */
672 }
673
674 (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + t*Dp */
675 for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */
676 hx[i]=tmp=x[i]-hx[i];
677 pDp_eL2+=tmp*tmp;
678 }
679 if(!LM_FINITE(pDp_eL2)) goto gradproj; /* treat as line search failure */
680
681 //if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + t*alpha*jacTeDp) break;
682 if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*t*alpha*jacTeDp) break;
683 }
684 #endif
685 ++nLSsteps;
686 gprevtaken=0;
687
688 /* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2.
689 * These values are used below to update their corresponding variables
690 */
691 }
692 else{
693 gradproj: /* Note that this point can also be reached via a goto when LNSRCH() fails */
694
695 /* jacTe is a descent direction; make a projected gradient step */
696
697 /* if the previous step was along the gradient descent, try to use the t employed in that step */
698 /* compute ||g|| */
699 for(i=0, tmp=0.0; i<m; ++i)
700 tmp+=jacTe[i]*jacTe[i];
701 tmp=(LM_REAL)sqrt(tmp);
702 tmp=LM_CNST(100.0)/(LM_CNST(1.0)+tmp);
703 t0=(tmp<=tini)? tmp : tini; /* guard against poor scaling & large steps; see (3.50) in C.T. Kelley's book */
704
705 for(t=(gprevtaken)? t : t0; t>tming; t*=beta){
706 for(i=0; i<m; ++i)
707 pDp[i]=p[i] - t*jacTe[i];
708 BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
709 for(i=0; i<m; ++i)
710 Dp[i]=pDp[i]-p[i];
711
712 (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p - t*g */
713 /* compute ||e(pDp)||_2 */
714 /* ### hx=x-hx, pDp_eL2=||hx|| */
715 #if 1
716 pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
717 #else
718 for(i=0, pDp_eL2=0.0; i<n; ++i){
719 hx[i]=tmp=x[i]-hx[i];
720 pDp_eL2+=tmp*tmp;
721 }
722 #endif
723 if(!LM_FINITE(pDp_eL2)){
724 stop=7;
725 goto breaknested;
726 }
727
728 for(i=0, tmp=0.0; i<m; ++i) /* compute ||g^T * Dp|| */
729 tmp+=jacTe[i]*Dp[i];
730
731 if(gprevtaken && pDp_eL2<=p_eL2 + LM_CNST(2.0)*LM_CNST(0.99999)*tmp){ /* starting t too small */
732 t=t0;
733 gprevtaken=0;
734 continue;
735 }
736 //if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + alpha*tmp) break;
737 if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*alpha*tmp) break;
738 }
739
740 ++nPGsteps;
741 gprevtaken=1;
742 /* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2 */
743 }
744
745 /* update using computed values */
746
747 for(i=0, Dp_L2=0.0; i<m; ++i){
748 tmp=pDp[i]-p[i];
749 Dp_L2+=tmp*tmp;
750 }
751 //Dp_L2=sqrt(Dp_L2);
752
753 if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
754 stop=2;
755 break;
756 }
757
758 for(i=0 ; i<m; ++i) /* update p's estimate */
759 p[i]=pDp[i];
760
761 for(i=0; i<n; ++i) /* update e and ||e||_2 */
762 e[i]=hx[i];
763 p_eL2=pDp_eL2;
764 break;
765 } /* inner loop */
766 }
767
768 breaknested: /* NOTE: this point is also reached via an explicit goto! */
769
770 if(k>=itmax) stop=3;
771
772 for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
773 jacTjac[i*m+i]=diag_jacTjac[i];
774
775 if(info){
776 info[0]=init_p_eL2;
777 info[1]=p_eL2;
778 info[2]=jacTe_inf;
779 info[3]=Dp_L2;
780 for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
781 if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
782 info[4]=mu/tmp;
783 info[5]=(LM_REAL)k;
784 info[6]=(LM_REAL)stop;
785 info[7]=(LM_REAL)nfev;
786 info[8]=(LM_REAL)njev;
787 }
788
789 /* covariance matrix */
790 if(covar){
791 LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
792 }
793
794 if(freework) free(work);
795
796 #ifdef LINSOLVERS_RETAIN_MEMORY
797 if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
798 #endif
799
800 #if 0
801 printf("%d LM steps, %d line search, %d projected gradient\n", nLMsteps, nLSsteps, nPGsteps);
802 #endif
803
804 return (stop!=4 && stop!=7)? k : LM_ERROR;
805 }
806
807 /* following struct & LMBC_DIF_XXX functions won't be necessary if a true secant
808 * version of LEVMAR_BC_DIF() is implemented...
809 */
810 struct LMBC_DIF_DATA{
811 void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
812 LM_REAL *hx, *hxx;
813 void *adata;
814 LM_REAL delta;
815 };
816
LMBC_DIF_FUNC(LM_REAL * p,LM_REAL * hx,int m,int n,void * data)817 void LMBC_DIF_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *data)
818 {
819 struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;
820
821 /* call user-supplied function passing it the user-supplied data */
822 (*(dta->func))(p, hx, m, n, dta->adata);
823 }
824
LMBC_DIF_JACF(LM_REAL * p,LM_REAL * jac,int m,int n,void * data)825 void LMBC_DIF_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *data)
826 {
827 struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;
828
829 /* evaluate user-supplied function at p */
830 (*(dta->func))(p, dta->hx, m, n, dta->adata);
831 LEVMAR_FDIF_FORW_JAC_APPROX(dta->func, p, dta->hx, dta->hxx, dta->delta, jac, m, n, dta->adata);
832 }
833
834
835 /* No Jacobian version of the LEVMAR_BC_DER() function above: the Jacobian is approximated with
836 * the aid of finite differences (forward or central, see the comment for the opts argument)
837 * Ideally, this function should be implemented with a secant approach. Currently, it just calls
838 * LEVMAR_BC_DER()
839 */
LEVMAR_BC_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,int itmax,LM_REAL opts[5],LM_REAL info[LM_INFO_SZ],LM_REAL * work,LM_REAL * covar,void * adata)840 int LEVMAR_BC_DIF(
841 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 */
842 LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
843 LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
844 int m, /* I: parameter vector dimension (i.e. #unknowns) */
845 int n, /* I: measurement vector dimension */
846 LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
847 LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
848 int itmax, /* I: maximum number of iterations */
849 LM_REAL opts[5], /* I: opts[0-4] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
850 * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
851 * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
852 * If \delta<0, the Jacobian is approximated with central differences which are more accurate
853 * (but slower!) compared to the forward differences employed by default.
854 */
855 LM_REAL info[LM_INFO_SZ],
856 /* O: information regarding the minimization. Set to NULL if don't care
857 * info[0]= ||e||_2 at initial p.
858 * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
859 * info[5]= # iterations,
860 * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
861 * 2 - stopped by small Dp
862 * 3 - stopped by itmax
863 * 4 - singular matrix. Restart from current p with increased mu
864 * 5 - no further error reduction is possible. Restart with increased mu
865 * 6 - stopped by small ||e||_2
866 * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
867 * info[7]= # function evaluations
868 * info[8]= # Jacobian evaluations
869 */
870 LM_REAL *work, /* working memory at least LM_BC_DIF_WORKSZ() reals large, allocated if NULL */
871 LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
872 void *adata) /* pointer to possibly additional data, passed uninterpreted to func.
873 * Set to NULL if not needed
874 */
875 {
876 struct LMBC_DIF_DATA data;
877 int ret;
878
879 //fprintf(stderr, RCAT("\nWarning: current implementation of ", LEVMAR_BC_DIF) "() does not use a secant approach!\n\n");
880
881 data.func=func;
882 data.hx=(LM_REAL *)malloc(2*n*sizeof(LM_REAL)); /* allocate a big chunk in one step */
883 if(!data.hx){
884 fprintf(stderr, LCAT(LEVMAR_BC_DIF, "(): memory allocation request failed\n"));
885 exit(1);
886 }
887 data.hxx=data.hx+n;
888 data.adata=adata;
889 data.delta=(opts)? FABS(opts[4]) : (LM_REAL)LM_DIFF_DELTA; // no central differences here...
890
891 ret=LEVMAR_BC_DER(LMBC_DIF_FUNC, LMBC_DIF_JACF, p, x, m, n, lb, ub, itmax, opts, info, work, covar, (void *)&data);
892
893 if(info) /* correct the number of function calls */
894 info[7]+=info[8]*(m+1); /* each Jacobian evaluation costs m+1 function calls */
895
896 free(data.hx);
897
898 return ret;
899 }
900 /* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
901 #undef FUNC_STATE
902 #undef LNSRCH
903 #undef BOXPROJECT
904 #undef LEVMAR_BOX_CHECK
905 #undef LEVMAR_BC_DER
906 #undef LMBC_DIF_DATA
907 #undef LMBC_DIF_FUNC
908 #undef LMBC_DIF_JACF
909 #undef LEVMAR_BC_DIF
910 #undef LEVMAR_FDIF_FORW_JAC_APPROX
911 #undef LEVMAR_FDIF_CENT_JAC_APPROX
912 #undef LEVMAR_COVAR
913 #undef LEVMAR_TRANS_MAT_MAT_MULT
914 #undef LEVMAR_L2NRMXMY
915 #undef AX_EQ_B_LU
916 #undef AX_EQ_B_CHOL
917 #undef AX_EQ_B_QR
918 #undef AX_EQ_B_QRLS
919 #undef AX_EQ_B_SVD
920