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