| 1 | ///////////////////////////////////////////////////////////////////////////////// | 
|---|
| 2 | // | 
|---|
| 3 | //  Levenberg - Marquardt non-linear minimization algorithm | 
|---|
| 4 | //  Copyright (C) 2009  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 lmbleic.c | 
|---|
| 21 | #error This file should not be compiled directly! | 
|---|
| 22 | #endif | 
|---|
| 23 |  | 
|---|
| 24 |  | 
|---|
| 25 | /* precision-specific definitions */ | 
|---|
| 26 | #define LMBLEIC_DATA LM_ADD_PREFIX(lmbleic_data) | 
|---|
| 27 | #define LMBLEIC_ELIM LM_ADD_PREFIX(lmbleic_elim) | 
|---|
| 28 | #define LMBLEIC_FUNC LM_ADD_PREFIX(lmbleic_func) | 
|---|
| 29 | #define LMBLEIC_JACF LM_ADD_PREFIX(lmbleic_jacf) | 
|---|
| 30 | #define LEVMAR_BLEIC_DER LM_ADD_PREFIX(levmar_bleic_der) | 
|---|
| 31 | #define LEVMAR_BLEIC_DIF LM_ADD_PREFIX(levmar_bleic_dif) | 
|---|
| 32 | #define LEVMAR_BLIC_DER LM_ADD_PREFIX(levmar_blic_der) | 
|---|
| 33 | #define LEVMAR_BLIC_DIF LM_ADD_PREFIX(levmar_blic_dif) | 
|---|
| 34 | #define LEVMAR_LEIC_DER LM_ADD_PREFIX(levmar_leic_der) | 
|---|
| 35 | #define LEVMAR_LEIC_DIF LM_ADD_PREFIX(levmar_leic_dif) | 
|---|
| 36 | #define LEVMAR_LIC_DER LM_ADD_PREFIX(levmar_lic_der) | 
|---|
| 37 | #define LEVMAR_LIC_DIF LM_ADD_PREFIX(levmar_lic_dif) | 
|---|
| 38 | #define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der) | 
|---|
| 39 | #define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif) | 
|---|
| 40 | #define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult) | 
|---|
| 41 | #define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar) | 
|---|
| 42 | #define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx) | 
|---|
| 43 |  | 
|---|
| 44 | struct LMBLEIC_DATA{ | 
|---|
| 45 | LM_REAL *jac; | 
|---|
| 46 | int nineqcnstr; // #inequality constraints | 
|---|
| 47 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata); | 
|---|
| 48 | void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata); | 
|---|
| 49 | void *adata; | 
|---|
| 50 | }; | 
|---|
| 51 |  | 
|---|
| 52 |  | 
|---|
| 53 | /* wrapper ensuring that the user-supplied function is called with the right number of variables (i.e. m) */ | 
|---|
| 54 | static void LMBLEIC_FUNC(LM_REAL *pext, LM_REAL *hx, int mm, int n, void *adata) | 
|---|
| 55 | { | 
|---|
| 56 | struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata; | 
|---|
| 57 | int m; | 
|---|
| 58 |  | 
|---|
| 59 | m=mm-data->nineqcnstr; | 
|---|
| 60 | (*(data->func))(pext, hx, m, n, data->adata); | 
|---|
| 61 | } | 
|---|
| 62 |  | 
|---|
| 63 |  | 
|---|
| 64 | /* wrapper for computing the Jacobian at pext. The Jacobian is nxmm */ | 
|---|
| 65 | static void LMBLEIC_JACF(LM_REAL *pext, LM_REAL *jacext, int mm, int n, void *adata) | 
|---|
| 66 | { | 
|---|
| 67 | struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata; | 
|---|
| 68 | int m; | 
|---|
| 69 | register int i, j; | 
|---|
| 70 | LM_REAL *jac, *jacim, *jacextimm; | 
|---|
| 71 |  | 
|---|
| 72 | m=mm-data->nineqcnstr; | 
|---|
| 73 | jac=data->jac; | 
|---|
| 74 |  | 
|---|
| 75 | (*(data->jacf))(pext, jac, m, n, data->adata); | 
|---|
| 76 |  | 
|---|
| 77 | for(i=0; i<n; ++i){ | 
|---|
| 78 | jacextimm=jacext+i*mm; | 
|---|
| 79 | jacim=jac+i*m; | 
|---|
| 80 | for(j=0; j<m; ++j) | 
|---|
| 81 | jacextimm[j]=jacim[j]; //jacext[i*mm+j]=jac[i*m+j]; | 
|---|
| 82 |  | 
|---|
| 83 | for(j=m; j<mm; ++j) | 
|---|
| 84 | jacextimm[j]=0.0; //jacext[i*mm+j]=0.0; | 
|---|
| 85 | } | 
|---|
| 86 | } | 
|---|
| 87 |  | 
|---|
| 88 |  | 
|---|
| 89 | /* | 
|---|
| 90 | * This function is similar to LEVMAR_DER except that the minimization is | 
|---|
| 91 | * performed subject to the box constraints lb[i]<=p[i]<=ub[i], the linear | 
|---|
| 92 | * equation constraints A*p=b, A being k1xm, b k1x1, and the linear inequality | 
|---|
| 93 | * constraints C*p>=d, C being k2xm, d k2x1. | 
|---|
| 94 | * | 
|---|
| 95 | * The inequalities are converted to equations by introducing surplus variables, | 
|---|
| 96 | * i.e. c^T*p >= d becomes c^T*p - y = d, with y>=0. To transform all inequalities | 
|---|
| 97 | * to equations, a total of k2 surplus variables are introduced; a problem with only | 
|---|
| 98 | * box and linear constraints results then and is solved with LEVMAR_BLEC_DER() | 
|---|
| 99 | * Note that opposite direction inequalities should be converted to the desired | 
|---|
| 100 | * direction by negating, i.e. c^T*p <= d becomes -c^T*p >= -d | 
|---|
| 101 | * | 
|---|
| 102 | * This function requires an analytic Jacobian. In case the latter is unavailable, | 
|---|
| 103 | * use LEVMAR_BLEIC_DIF() bellow | 
|---|
| 104 | * | 
|---|
| 105 | */ | 
|---|
| 106 | int LEVMAR_BLEIC_DER( | 
|---|
| 107 | 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 */ | 
|---|
| 108 | void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ | 
|---|
| 109 | LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */ | 
|---|
| 110 | LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */ | 
|---|
| 111 | int m,              /* I: parameter vector dimension (i.e. #unknowns) */ | 
|---|
| 112 | int n,              /* I: measurement vector dimension */ | 
|---|
| 113 | LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */ | 
|---|
| 114 | LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */ | 
|---|
| 115 | LM_REAL *A,         /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */ | 
|---|
| 116 | LM_REAL *b,         /* I: right hand constraints vector, k1x1 */ | 
|---|
| 117 | int k1,             /* I: number of constraints (i.e. A's #rows) */ | 
|---|
| 118 | LM_REAL *C,         /* I: inequality constraints matrix, k2xm */ | 
|---|
| 119 | LM_REAL *d,         /* I: right hand constraints vector, k2x1 */ | 
|---|
| 120 | int k2,             /* I: number of inequality constraints (i.e. C's #rows) */ | 
|---|
| 121 | int itmax,          /* I: maximum number of iterations */ | 
|---|
| 122 | LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu, | 
|---|
| 123 | * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used | 
|---|
| 124 | */ | 
|---|
| 125 | LM_REAL info[LM_INFO_SZ], | 
|---|
| 126 | /* O: information regarding the minimization. Set to NULL if don't care | 
|---|
| 127 | * info[0]= ||e||_2 at initial p. | 
|---|
| 128 | * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. | 
|---|
| 129 | * info[5]= # iterations, | 
|---|
| 130 | * info[6]=reason for terminating: 1 - stopped by small gradient J^T e | 
|---|
| 131 | *                                 2 - stopped by small Dp | 
|---|
| 132 | *                                 3 - stopped by itmax | 
|---|
| 133 | *                                 4 - singular matrix. Restart from current p with increased mu | 
|---|
| 134 | *                                 5 - no further error reduction is possible. Restart with increased mu | 
|---|
| 135 | *                                 6 - stopped by small ||e||_2 | 
|---|
| 136 | *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error | 
|---|
| 137 | * info[7]= # function evaluations | 
|---|
| 138 | * info[8]= # Jacobian evaluations | 
|---|
| 139 | * info[9]= # linear systems solved, i.e. # attempts for reducing error | 
|---|
| 140 | */ | 
|---|
| 141 | LM_REAL *work,     /* working memory at least LM_BLEIC_DER_WORKSZ() reals large, allocated if NULL */ | 
|---|
| 142 | LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */ | 
|---|
| 143 | void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf. | 
|---|
| 144 | * Set to NULL if not needed | 
|---|
| 145 | */ | 
|---|
| 146 | { | 
|---|
| 147 | struct LMBLEIC_DATA data; | 
|---|
| 148 | LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables; | 
|---|
| 149 | pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm | 
|---|
| 150 | */ | 
|---|
| 151 | LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables | 
|---|
| 152 | int mm, ret, k12; | 
|---|
| 153 | register int i, j, ii; | 
|---|
| 154 | register LM_REAL tmp; | 
|---|
| 155 | LM_REAL locinfo[LM_INFO_SZ]; | 
|---|
| 156 |  | 
|---|
| 157 | if(!jacf){ | 
|---|
| 158 | fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEIC_DER) | 
|---|
| 159 | RCAT("().\nIf no such function is available, use ", LEVMAR_BLEIC_DIF) RCAT("() rather than ", LEVMAR_BLEIC_DER) "()\n"); | 
|---|
| 160 | return LM_ERROR; | 
|---|
| 161 | } | 
|---|
| 162 |  | 
|---|
| 163 | if(!C || !d){ | 
|---|
| 164 | fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DER, "(): missing inequality constraints, use "), LEVMAR_BLEC_DER) "() in this case!\n"); | 
|---|
| 165 | return LM_ERROR; | 
|---|
| 166 | } | 
|---|
| 167 |  | 
|---|
| 168 | if(!A || !b) k1=0; // sanity check | 
|---|
| 169 |  | 
|---|
| 170 | mm=m+k2; | 
|---|
| 171 |  | 
|---|
| 172 | if(n<m-k1){ | 
|---|
| 173 | fprintf(stderr, LCAT(LEVMAR_BLEIC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k1, m); | 
|---|
| 174 | return LM_ERROR; | 
|---|
| 175 | } | 
|---|
| 176 |  | 
|---|
| 177 | k12=k1+k2; | 
|---|
| 178 | ptr=(LM_REAL *)malloc((3*mm + k12*mm + k12 + n*m + (covar? mm*mm : 0))*sizeof(LM_REAL)); | 
|---|
| 179 | if(!ptr){ | 
|---|
| 180 | fprintf(stderr, LCAT(LEVMAR_BLEIC_DER, "(): memory allocation request failed\n")); | 
|---|
| 181 | return LM_ERROR; | 
|---|
| 182 | } | 
|---|
| 183 | pext=ptr; | 
|---|
| 184 | lbext=pext+mm; | 
|---|
| 185 | ubext=lbext+mm; | 
|---|
| 186 | Aext=ubext+mm; | 
|---|
| 187 | bext=Aext+k12*mm; | 
|---|
| 188 | data.jac=bext+k12; | 
|---|
| 189 | covext=covar? data.jac+n*m : NULL; | 
|---|
| 190 | data.nineqcnstr=k2; | 
|---|
| 191 | data.func=func; | 
|---|
| 192 | data.jacf=jacf; | 
|---|
| 193 | data.adata=adata; | 
|---|
| 194 |  | 
|---|
| 195 | /* compute y s.t. C*p - y=d, i.e. y=C*p-d. | 
|---|
| 196 | * y is stored in the last k2 elements of pext | 
|---|
| 197 | */ | 
|---|
| 198 | for(i=0; i<k2; ++i){ | 
|---|
| 199 | for(j=0, tmp=0.0; j<m; ++j) | 
|---|
| 200 | tmp+=C[i*m+j]*p[j]; | 
|---|
| 201 | pext[j=i+m]=tmp-d[i]; | 
|---|
| 202 |  | 
|---|
| 203 | /* surplus variables must be >=0 */ | 
|---|
| 204 | lbext[j]=0.0; | 
|---|
| 205 | ubext[j]=LM_REAL_MAX; | 
|---|
| 206 | } | 
|---|
| 207 | /* set the first m elements of pext equal to p */ | 
|---|
| 208 | for(i=0; i<m; ++i){ | 
|---|
| 209 | pext[i]=p[i]; | 
|---|
| 210 | lbext[i]=lb? lb[i] : LM_REAL_MIN; | 
|---|
| 211 | ubext[i]=ub? ub[i] : LM_REAL_MAX; | 
|---|
| 212 | } | 
|---|
| 213 |  | 
|---|
| 214 | /* setup the constraints matrix */ | 
|---|
| 215 | /* original linear equation constraints */ | 
|---|
| 216 | for(i=0; i<k1; ++i){ | 
|---|
| 217 | for(j=0; j<m; ++j) | 
|---|
| 218 | Aext[i*mm+j]=A[i*m+j]; | 
|---|
| 219 |  | 
|---|
| 220 | for(j=m; j<mm; ++j) | 
|---|
| 221 | Aext[i*mm+j]=0.0; | 
|---|
| 222 |  | 
|---|
| 223 | bext[i]=b[i]; | 
|---|
| 224 | } | 
|---|
| 225 | /* linear equation constraints resulting from surplus variables */ | 
|---|
| 226 | for(i=0, ii=k1; i<k2; ++i, ++ii){ | 
|---|
| 227 | for(j=0; j<m; ++j) | 
|---|
| 228 | Aext[ii*mm+j]=C[i*m+j]; | 
|---|
| 229 |  | 
|---|
| 230 | for(j=m; j<mm; ++j) | 
|---|
| 231 | Aext[ii*mm+j]=0.0; | 
|---|
| 232 |  | 
|---|
| 233 | Aext[ii*mm+m+i]=-1.0; | 
|---|
| 234 |  | 
|---|
| 235 | bext[ii]=d[i]; | 
|---|
| 236 | } | 
|---|
| 237 |  | 
|---|
| 238 | if(!info) info=locinfo; /* make sure that LEVMAR_BLEC_DER() is called with non-null info */ | 
|---|
| 239 | /* note that the default weights for the penalty terms are being used below */ | 
|---|
| 240 | ret=LEVMAR_BLEC_DER(LMBLEIC_FUNC, LMBLEIC_JACF, pext, x, mm, n, lbext, ubext, Aext, bext, k12, NULL, itmax, opts, info, work, covext, (void *)&data); | 
|---|
| 241 |  | 
|---|
| 242 | /* copy back the minimizer */ | 
|---|
| 243 | for(i=0; i<m; ++i) | 
|---|
| 244 | p[i]=pext[i]; | 
|---|
| 245 |  | 
|---|
| 246 | #if 0 | 
|---|
| 247 | printf("Surplus variables for the minimizer:\n"); | 
|---|
| 248 | for(i=m; i<mm; ++i) | 
|---|
| 249 | printf("%g ", pext[i]); | 
|---|
| 250 | printf("\n\n"); | 
|---|
| 251 | #endif | 
|---|
| 252 |  | 
|---|
| 253 | if(covar){ | 
|---|
| 254 | for(i=0; i<m; ++i){ | 
|---|
| 255 | for(j=0; j<m; ++j) | 
|---|
| 256 | covar[i*m+j]=covext[i*mm+j]; | 
|---|
| 257 | } | 
|---|
| 258 | } | 
|---|
| 259 |  | 
|---|
| 260 | free(ptr); | 
|---|
| 261 |  | 
|---|
| 262 | return ret; | 
|---|
| 263 | } | 
|---|
| 264 |  | 
|---|
| 265 | /* Similar to the LEVMAR_BLEIC_DER() function above, except that the Jacobian is approximated | 
|---|
| 266 | * with the aid of finite differences (forward or central, see the comment for the opts argument) | 
|---|
| 267 | */ | 
|---|
| 268 | int LEVMAR_BLEIC_DIF( | 
|---|
| 269 | 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 */ | 
|---|
| 270 | LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */ | 
|---|
| 271 | LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */ | 
|---|
| 272 | int m,              /* I: parameter vector dimension (i.e. #unknowns) */ | 
|---|
| 273 | int n,              /* I: measurement vector dimension */ | 
|---|
| 274 | LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */ | 
|---|
| 275 | LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */ | 
|---|
| 276 | LM_REAL *A,         /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */ | 
|---|
| 277 | LM_REAL *b,         /* I: right hand constraints vector, k1x1 */ | 
|---|
| 278 | int k1,             /* I: number of constraints (i.e. A's #rows) */ | 
|---|
| 279 | LM_REAL *C,         /* I: inequality constraints matrix, k2xm */ | 
|---|
| 280 | LM_REAL *d,         /* I: right hand constraints vector, k2x1 */ | 
|---|
| 281 | int k2,             /* I: number of inequality constraints (i.e. C's #rows) */ | 
|---|
| 282 | int itmax,          /* I: maximum number of iterations */ | 
|---|
| 283 | LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the | 
|---|
| 284 | * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and | 
|---|
| 285 | * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used. | 
|---|
| 286 | * If \delta<0, the Jacobian is approximated with central differences which are more accurate | 
|---|
| 287 | * (but slower!) compared to the forward differences employed by default. | 
|---|
| 288 | */ | 
|---|
| 289 | LM_REAL info[LM_INFO_SZ], | 
|---|
| 290 | /* O: information regarding the minimization. Set to NULL if don't care | 
|---|
| 291 | * info[0]= ||e||_2 at initial p. | 
|---|
| 292 | * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. | 
|---|
| 293 | * info[5]= # iterations, | 
|---|
| 294 | * info[6]=reason for terminating: 1 - stopped by small gradient J^T e | 
|---|
| 295 | *                                 2 - stopped by small Dp | 
|---|
| 296 | *                                 3 - stopped by itmax | 
|---|
| 297 | *                                 4 - singular matrix. Restart from current p with increased mu | 
|---|
| 298 | *                                 5 - no further error reduction is possible. Restart with increased mu | 
|---|
| 299 | *                                 6 - stopped by small ||e||_2 | 
|---|
| 300 | *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error | 
|---|
| 301 | * info[7]= # function evaluations | 
|---|
| 302 | * info[8]= # Jacobian evaluations | 
|---|
| 303 | * info[9]= # linear systems solved, i.e. # attempts for reducing error | 
|---|
| 304 | */ | 
|---|
| 305 | LM_REAL *work,     /* working memory at least LM_BLEIC_DIF_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. | 
|---|
| 308 | * Set to NULL if not needed | 
|---|
| 309 | */ | 
|---|
| 310 | { | 
|---|
| 311 | struct LMBLEIC_DATA data; | 
|---|
| 312 | LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables; | 
|---|
| 313 | pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm | 
|---|
| 314 | */ | 
|---|
| 315 | LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables | 
|---|
| 316 | int mm, ret, k12; | 
|---|
| 317 | register int i, j, ii; | 
|---|
| 318 | register LM_REAL tmp; | 
|---|
| 319 | LM_REAL locinfo[LM_INFO_SZ]; | 
|---|
| 320 |  | 
|---|
| 321 | if(!C || !d){ | 
|---|
| 322 | fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DIF, "(): missing inequality constraints, use "), LEVMAR_BLEC_DIF) "() in this case!\n"); | 
|---|
| 323 | return LM_ERROR; | 
|---|
| 324 | } | 
|---|
| 325 | if(!A || !b) k1=0; // sanity check | 
|---|
| 326 |  | 
|---|
| 327 | mm=m+k2; | 
|---|
| 328 |  | 
|---|
| 329 | if(n<m-k1){ | 
|---|
| 330 | fprintf(stderr, LCAT(LEVMAR_BLEIC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k1, m); | 
|---|
| 331 | return LM_ERROR; | 
|---|
| 332 | } | 
|---|
| 333 |  | 
|---|
| 334 | k12=k1+k2; | 
|---|
| 335 | ptr=(LM_REAL *)malloc((3*mm + k12*mm + k12 + (covar? mm*mm : 0))*sizeof(LM_REAL)); | 
|---|
| 336 | if(!ptr){ | 
|---|
| 337 | fprintf(stderr, LCAT(LEVMAR_BLEIC_DIF, "(): memory allocation request failed\n")); | 
|---|
| 338 | return LM_ERROR; | 
|---|
| 339 | } | 
|---|
| 340 | pext=ptr; | 
|---|
| 341 | lbext=pext+mm; | 
|---|
| 342 | ubext=lbext+mm; | 
|---|
| 343 | Aext=ubext+mm; | 
|---|
| 344 | bext=Aext+k12*mm; | 
|---|
| 345 | data.jac=NULL; | 
|---|
| 346 | covext=covar? bext+k12 : NULL; | 
|---|
| 347 | data.nineqcnstr=k2; | 
|---|
| 348 | data.func=func; | 
|---|
| 349 | data.jacf=NULL; | 
|---|
| 350 | data.adata=adata; | 
|---|
| 351 |  | 
|---|
| 352 | /* compute y s.t. C*p - y=d, i.e. y=C*p-d. | 
|---|
| 353 | * y is stored in the last k2 elements of pext | 
|---|
| 354 | */ | 
|---|
| 355 | for(i=0; i<k2; ++i){ | 
|---|
| 356 | for(j=0, tmp=0.0; j<m; ++j) | 
|---|
| 357 | tmp+=C[i*m+j]*p[j]; | 
|---|
| 358 | pext[j=i+m]=tmp-d[i]; | 
|---|
| 359 |  | 
|---|
| 360 | /* surplus variables must be >=0 */ | 
|---|
| 361 | lbext[j]=0.0; | 
|---|
| 362 | ubext[j]=LM_REAL_MAX; | 
|---|
| 363 | } | 
|---|
| 364 | /* set the first m elements of pext equal to p */ | 
|---|
| 365 | for(i=0; i<m; ++i){ | 
|---|
| 366 | pext[i]=p[i]; | 
|---|
| 367 | lbext[i]=lb? lb[i] : LM_REAL_MIN; | 
|---|
| 368 | ubext[i]=ub? ub[i] : LM_REAL_MAX; | 
|---|
| 369 | } | 
|---|
| 370 |  | 
|---|
| 371 | /* setup the constraints matrix */ | 
|---|
| 372 | /* original linear equation constraints */ | 
|---|
| 373 | for(i=0; i<k1; ++i){ | 
|---|
| 374 | for(j=0; j<m; ++j) | 
|---|
| 375 | Aext[i*mm+j]=A[i*m+j]; | 
|---|
| 376 |  | 
|---|
| 377 | for(j=m; j<mm; ++j) | 
|---|
| 378 | Aext[i*mm+j]=0.0; | 
|---|
| 379 |  | 
|---|
| 380 | bext[i]=b[i]; | 
|---|
| 381 | } | 
|---|
| 382 | /* linear equation constraints resulting from surplus variables */ | 
|---|
| 383 | for(i=0, ii=k1; i<k2; ++i, ++ii){ | 
|---|
| 384 | for(j=0; j<m; ++j) | 
|---|
| 385 | Aext[ii*mm+j]=C[i*m+j]; | 
|---|
| 386 |  | 
|---|
| 387 | for(j=m; j<mm; ++j) | 
|---|
| 388 | Aext[ii*mm+j]=0.0; | 
|---|
| 389 |  | 
|---|
| 390 | Aext[ii*mm+m+i]=-1.0; | 
|---|
| 391 |  | 
|---|
| 392 | bext[ii]=d[i]; | 
|---|
| 393 | } | 
|---|
| 394 |  | 
|---|
| 395 | if(!info) info=locinfo; /* make sure that LEVMAR_BLEC_DIF() is called with non-null info */ | 
|---|
| 396 | /* note that the default weights for the penalty terms are being used below */ | 
|---|
| 397 | ret=LEVMAR_BLEC_DIF(LMBLEIC_FUNC, pext, x, mm, n, lbext, ubext, Aext, bext, k12, NULL, itmax, opts, info, work, covext, (void *)&data); | 
|---|
| 398 |  | 
|---|
| 399 | /* copy back the minimizer */ | 
|---|
| 400 | for(i=0; i<m; ++i) | 
|---|
| 401 | p[i]=pext[i]; | 
|---|
| 402 |  | 
|---|
| 403 | #if 0 | 
|---|
| 404 | printf("Surplus variables for the minimizer:\n"); | 
|---|
| 405 | for(i=m; i<mm; ++i) | 
|---|
| 406 | printf("%g ", pext[i]); | 
|---|
| 407 | printf("\n\n"); | 
|---|
| 408 | #endif | 
|---|
| 409 |  | 
|---|
| 410 | if(covar){ | 
|---|
| 411 | for(i=0; i<m; ++i){ | 
|---|
| 412 | for(j=0; j<m; ++j) | 
|---|
| 413 | covar[i*m+j]=covext[i*mm+j]; | 
|---|
| 414 | } | 
|---|
| 415 | } | 
|---|
| 416 |  | 
|---|
| 417 | free(ptr); | 
|---|
| 418 |  | 
|---|
| 419 | return ret; | 
|---|
| 420 | } | 
|---|
| 421 |  | 
|---|
| 422 |  | 
|---|
| 423 | /* convenience wrappers to LEVMAR_BLEIC_DER/LEVMAR_BLEIC_DIF */ | 
|---|
| 424 |  | 
|---|
| 425 | /* box & linear inequality constraints */ | 
|---|
| 426 | int LEVMAR_BLIC_DER( | 
|---|
| 427 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), | 
|---|
| 428 | void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), | 
|---|
| 429 | LM_REAL *p, LM_REAL *x, int m, int n, | 
|---|
| 430 | LM_REAL *lb, LM_REAL *ub, | 
|---|
| 431 | LM_REAL *C, LM_REAL *d, int k2, | 
|---|
| 432 | int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata) | 
|---|
| 433 | { | 
|---|
| 434 | return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata); | 
|---|
| 435 | } | 
|---|
| 436 |  | 
|---|
| 437 | int LEVMAR_BLIC_DIF( | 
|---|
| 438 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), | 
|---|
| 439 | LM_REAL *p, LM_REAL *x, int m, int n, | 
|---|
| 440 | LM_REAL *lb, LM_REAL *ub, | 
|---|
| 441 | LM_REAL *C, LM_REAL *d, int k2, | 
|---|
| 442 | int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata) | 
|---|
| 443 | { | 
|---|
| 444 | return LEVMAR_BLEIC_DIF(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata); | 
|---|
| 445 | } | 
|---|
| 446 |  | 
|---|
| 447 | /* linear equation & inequality constraints */ | 
|---|
| 448 | int LEVMAR_LEIC_DER( | 
|---|
| 449 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), | 
|---|
| 450 | void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), | 
|---|
| 451 | LM_REAL *p, LM_REAL *x, int m, int n, | 
|---|
| 452 | LM_REAL *A, LM_REAL *b, int k1, | 
|---|
| 453 | LM_REAL *C, LM_REAL *d, int k2, | 
|---|
| 454 | int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata) | 
|---|
| 455 | { | 
|---|
| 456 | return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, NULL, NULL, A, b, k1, C, d, k2, itmax, opts, info, work, covar, adata); | 
|---|
| 457 | } | 
|---|
| 458 |  | 
|---|
| 459 | int LEVMAR_LEIC_DIF( | 
|---|
| 460 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), | 
|---|
| 461 | LM_REAL *p, LM_REAL *x, int m, int n, | 
|---|
| 462 | LM_REAL *A, LM_REAL *b, int k1, | 
|---|
| 463 | LM_REAL *C, LM_REAL *d, int k2, | 
|---|
| 464 | int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata) | 
|---|
| 465 | { | 
|---|
| 466 | return LEVMAR_BLEIC_DIF(func, p, x, m, n, NULL, NULL, A, b, k1, C, d, k2, itmax, opts, info, work, covar, adata); | 
|---|
| 467 | } | 
|---|
| 468 |  | 
|---|
| 469 | /* linear inequality constraints */ | 
|---|
| 470 | int LEVMAR_LIC_DER( | 
|---|
| 471 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), | 
|---|
| 472 | void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), | 
|---|
| 473 | LM_REAL *p, LM_REAL *x, int m, int n, | 
|---|
| 474 | LM_REAL *C, LM_REAL *d, int k2, | 
|---|
| 475 | int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata) | 
|---|
| 476 | { | 
|---|
| 477 | return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata); | 
|---|
| 478 | } | 
|---|
| 479 |  | 
|---|
| 480 | int LEVMAR_LIC_DIF( | 
|---|
| 481 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), | 
|---|
| 482 | LM_REAL *p, LM_REAL *x, int m, int n, | 
|---|
| 483 | LM_REAL *C, LM_REAL *d, int k2, | 
|---|
| 484 | int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata) | 
|---|
| 485 | { | 
|---|
| 486 | return LEVMAR_BLEIC_DIF(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata); | 
|---|
| 487 | } | 
|---|
| 488 |  | 
|---|
| 489 | /* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */ | 
|---|
| 490 | #undef LMBLEIC_DATA | 
|---|
| 491 | #undef LMBLEIC_ELIM | 
|---|
| 492 | #undef LMBLEIC_FUNC | 
|---|
| 493 | #undef LMBLEIC_JACF | 
|---|
| 494 | #undef LEVMAR_FDIF_FORW_JAC_APPROX | 
|---|
| 495 | #undef LEVMAR_COVAR | 
|---|
| 496 | #undef LEVMAR_TRANS_MAT_MAT_MULT | 
|---|
| 497 | #undef LEVMAR_BLEIC_DER | 
|---|
| 498 | #undef LEVMAR_BLEIC_DIF | 
|---|
| 499 | #undef LEVMAR_BLIC_DER | 
|---|
| 500 | #undef LEVMAR_BLIC_DIF | 
|---|
| 501 | #undef LEVMAR_LEIC_DER | 
|---|
| 502 | #undef LEVMAR_LEIC_DIF | 
|---|
| 503 | #undef LEVMAR_LIC_DER | 
|---|
| 504 | #undef LEVMAR_LIC_DIF | 
|---|
| 505 | #undef LEVMAR_BLEC_DER | 
|---|
| 506 | #undef LEVMAR_BLEC_DIF | 
|---|