///////////////////////////////////////////////////////////////////////////////// // // Levenberg - Marquardt non-linear minimization algorithm // Copyright (C) 2009 Manolis Lourakis (lourakis at ics forth gr) // Institute of Computer Science, Foundation for Research & Technology - Hellas // Heraklion, Crete, Greece. // // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // ///////////////////////////////////////////////////////////////////////////////// #ifndef LM_REAL // not included by lmbleic.c #error This file should not be compiled directly! #endif /* precision-specific definitions */ #define LMBLEIC_DATA LM_ADD_PREFIX(lmbleic_data) #define LMBLEIC_ELIM LM_ADD_PREFIX(lmbleic_elim) #define LMBLEIC_FUNC LM_ADD_PREFIX(lmbleic_func) #define LMBLEIC_JACF LM_ADD_PREFIX(lmbleic_jacf) #define LEVMAR_BLEIC_DER LM_ADD_PREFIX(levmar_bleic_der) #define LEVMAR_BLEIC_DIF LM_ADD_PREFIX(levmar_bleic_dif) #define LEVMAR_BLIC_DER LM_ADD_PREFIX(levmar_blic_der) #define LEVMAR_BLIC_DIF LM_ADD_PREFIX(levmar_blic_dif) #define LEVMAR_LEIC_DER LM_ADD_PREFIX(levmar_leic_der) #define LEVMAR_LEIC_DIF LM_ADD_PREFIX(levmar_leic_dif) #define LEVMAR_LIC_DER LM_ADD_PREFIX(levmar_lic_der) #define LEVMAR_LIC_DIF LM_ADD_PREFIX(levmar_lic_dif) #define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der) #define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif) #define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult) #define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar) #define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx) struct LMBLEIC_DATA{ LM_REAL *jac; int nineqcnstr; // #inequality constraints void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata); void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata); void *adata; }; /* wrapper ensuring that the user-supplied function is called with the right number of variables (i.e. m) */ static void LMBLEIC_FUNC(LM_REAL *pext, LM_REAL *hx, int mm, int n, void *adata) { struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata; int m; m=mm-data->nineqcnstr; (*(data->func))(pext, hx, m, n, data->adata); } /* wrapper for computing the Jacobian at pext. The Jacobian is nxmm */ static void LMBLEIC_JACF(LM_REAL *pext, LM_REAL *jacext, int mm, int n, void *adata) { struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata; int m; register int i, j; LM_REAL *jac, *jacim, *jacextimm; m=mm-data->nineqcnstr; jac=data->jac; (*(data->jacf))(pext, jac, m, n, data->adata); for(i=0; i=d, C being k2xm, d k2x1. * * The inequalities are converted to equations by introducing surplus variables, * i.e. c^T*p >= d becomes c^T*p - y = d, with y>=0. To transform all inequalities * to equations, a total of k2 surplus variables are introduced; a problem with only * box and linear constraints results then and is solved with LEVMAR_BLEC_DER() * Note that opposite direction inequalities should be converted to the desired * direction by negating, i.e. c^T*p <= d becomes -c^T*p >= -d * * This function requires an analytic Jacobian. In case the latter is unavailable, * use LEVMAR_BLEIC_DIF() bellow * */ int LEVMAR_BLEIC_DER( 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 */ void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */ LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */ LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */ int m, /* I: parameter vector dimension (i.e. #unknowns) */ int n, /* I: measurement vector dimension */ LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */ LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */ LM_REAL *A, /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */ LM_REAL *b, /* I: right hand constraints vector, k1x1 */ int k1, /* I: number of constraints (i.e. A's #rows) */ LM_REAL *C, /* I: inequality constraints matrix, k2xm */ LM_REAL *d, /* I: right hand constraints vector, k2x1 */ int k2, /* I: number of inequality constraints (i.e. C's #rows) */ int itmax, /* I: maximum number of iterations */ LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu, * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used */ LM_REAL info[LM_INFO_SZ], /* O: information regarding the minimization. Set to NULL if don't care * info[0]= ||e||_2 at initial p. * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. * info[5]= # iterations, * info[6]=reason for terminating: 1 - stopped by small gradient J^T e * 2 - stopped by small Dp * 3 - stopped by itmax * 4 - singular matrix. Restart from current p with increased mu * 5 - no further error reduction is possible. Restart with increased mu * 6 - stopped by small ||e||_2 * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error * info[7]= # function evaluations * info[8]= # Jacobian evaluations * info[9]= # linear systems solved, i.e. # attempts for reducing error */ LM_REAL *work, /* working memory at least LM_BLEIC_DER_WORKSZ() reals large, allocated if NULL */ LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */ void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf. * Set to NULL if not needed */ { struct LMBLEIC_DATA data; LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables; pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm */ LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables int mm, ret, k12; register int i, j, ii; register LM_REAL tmp; LM_REAL locinfo[LM_INFO_SZ]; if(!jacf){ fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEIC_DER) RCAT("().\nIf no such function is available, use ", LEVMAR_BLEIC_DIF) RCAT("() rather than ", LEVMAR_BLEIC_DER) "()\n"); return LM_ERROR; } if(!C || !d){ fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DER, "(): missing inequality constraints, use "), LEVMAR_BLEC_DER) "() in this case!\n"); return LM_ERROR; } if(!A || !b) k1=0; // sanity check mm=m+k2; if(n=0 */ lbext[j]=0.0; ubext[j]=LM_REAL_MAX; } /* set the first m elements of pext equal to p */ for(i=0; i=0 */ lbext[j]=0.0; ubext[j]=LM_REAL_MAX; } /* set the first m elements of pext equal to p */ for(i=0; i