| [5443b1] | 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 | 
|---|