| 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 */
|
|---|
| 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 */
|
|---|
| 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 | */
|
|---|
| 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 | */
|
|---|
| 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
|
|---|
