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