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