| 1 | /////////////////////////////////////////////////////////////////////////////////
|
|---|
| 2 | //
|
|---|
| 3 | // Levenberg - Marquardt non-linear minimization algorithm
|
|---|
| 4 | // Copyright (C) 2004-05 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 lmlec.c
|
|---|
| 21 | #error This file should not be compiled directly!
|
|---|
| 22 | #endif
|
|---|
| 23 |
|
|---|
| 24 |
|
|---|
| 25 | /* precision-specific definitions */
|
|---|
| 26 | #define LMLEC_DATA LM_ADD_PREFIX(lmlec_data)
|
|---|
| 27 | #define LMLEC_ELIM LM_ADD_PREFIX(lmlec_elim)
|
|---|
| 28 | #define LMLEC_FUNC LM_ADD_PREFIX(lmlec_func)
|
|---|
| 29 | #define LMLEC_JACF LM_ADD_PREFIX(lmlec_jacf)
|
|---|
| 30 | #define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
|
|---|
| 31 | #define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
|
|---|
| 32 | #define LEVMAR_DER LM_ADD_PREFIX(levmar_der)
|
|---|
| 33 | #define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)
|
|---|
| 34 | #define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
|
|---|
| 35 | #define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
|
|---|
| 36 | #define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
|
|---|
| 37 |
|
|---|
| 38 | #define GEQP3 LM_MK_LAPACK_NAME(geqp3)
|
|---|
| 39 | #define ORGQR LM_MK_LAPACK_NAME(orgqr)
|
|---|
| 40 | #define TRTRI LM_MK_LAPACK_NAME(trtri)
|
|---|
| 41 |
|
|---|
| 42 | struct LMLEC_DATA{
|
|---|
| 43 | LM_REAL *c, *Z, *p, *jac;
|
|---|
| 44 | int ncnstr;
|
|---|
| 45 | void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
|
|---|
| 46 | void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
|
|---|
| 47 | void *adata;
|
|---|
| 48 | };
|
|---|
| 49 |
|
|---|
| 50 | /* prototypes for LAPACK routines */
|
|---|
| 51 | #ifdef __cplusplus
|
|---|
| 52 | extern "C" {
|
|---|
| 53 | #endif
|
|---|
| 54 | extern int GEQP3(int *m, int *n, LM_REAL *a, int *lda, int *jpvt,
|
|---|
| 55 | LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
|
|---|
| 56 |
|
|---|
| 57 | extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau,
|
|---|
| 58 | LM_REAL *work, int *lwork, int *info);
|
|---|
| 59 |
|
|---|
| 60 | extern int TRTRI(char *uplo, char *diag, int *n, LM_REAL *a, int *lda, int *info);
|
|---|
| 61 | #ifdef __cplusplus
|
|---|
| 62 | }
|
|---|
| 63 | #endif
|
|---|
| 64 |
|
|---|
| 65 | /*
|
|---|
| 66 | * This function implements an elimination strategy for linearly constrained
|
|---|
| 67 | * optimization problems. The strategy relies on QR decomposition to transform
|
|---|
| 68 | * an optimization problem constrained by Ax=b to an equivalent, unconstrained
|
|---|
| 69 | * one. Also referred to as "null space" or "reduced Hessian" method.
|
|---|
| 70 | * See pp. 430-433 (chap. 15) of "Numerical Optimization" by Nocedal-Wright
|
|---|
| 71 | * for details.
|
|---|
| 72 | *
|
|---|
| 73 | * A is mxn with m<=n and rank(A)=m
|
|---|
| 74 | * Two matrices Y and Z of dimensions nxm and nx(n-m) are computed from A^T so that
|
|---|
| 75 | * their columns are orthonormal and every x can be written as x=Y*b + Z*x_z=
|
|---|
| 76 | * c + Z*x_z, where c=Y*b is a fixed vector of dimension n and x_z is an
|
|---|
| 77 | * arbitrary vector of dimension n-m. Then, the problem of minimizing f(x)
|
|---|
| 78 | * subject to Ax=b is equivalent to minimizing f(c + Z*x_z) with no constraints.
|
|---|
| 79 | * The computed Y and Z are such that any solution of Ax=b can be written as
|
|---|
| 80 | * x=Y*x_y + Z*x_z for some x_y, x_z. Furthermore, A*Y is nonsingular, A*Z=0
|
|---|
| 81 | * and Z spans the null space of A.
|
|---|
| 82 | *
|
|---|
| 83 | * The function accepts A, b and computes c, Y, Z. If b or c is NULL, c is not
|
|---|
| 84 | * computed. Also, Y can be NULL in which case it is not referenced.
|
|---|
| 85 | * The function returns LM_ERROR in case of error, A's computed rank if successful
|
|---|
| 86 | *
|
|---|
| 87 | */
|
|---|
| 88 | static int LMLEC_ELIM(LM_REAL *A, LM_REAL *b, LM_REAL *c, LM_REAL *Y, LM_REAL *Z, int m, int n)
|
|---|
| 89 | {
|
|---|
| 90 | static LM_REAL eps=LM_CNST(-1.0);
|
|---|
| 91 |
|
|---|
| 92 | LM_REAL *buf=NULL;
|
|---|
| 93 | LM_REAL *a, *tau, *work, *r, aux;
|
|---|
| 94 | register LM_REAL tmp;
|
|---|
| 95 | int a_sz, jpvt_sz, tau_sz, r_sz, Y_sz, worksz;
|
|---|
| 96 | int info, rank, *jpvt, tot_sz, mintmn, tm, tn;
|
|---|
| 97 | register int i, j, k;
|
|---|
| 98 |
|
|---|
| 99 | if(m>n){
|
|---|
| 100 | fprintf(stderr, RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!\n");
|
|---|
| 101 | return LM_ERROR;
|
|---|
| 102 | }
|
|---|
| 103 |
|
|---|
| 104 | tm=n; tn=m; // transpose dimensions
|
|---|
| 105 | mintmn=m;
|
|---|
| 106 |
|
|---|
| 107 | /* calculate required memory size */
|
|---|
| 108 | worksz=-1; // workspace query. Optimal work size is returned in aux
|
|---|
| 109 | //ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, NULL, (int *)&tm, NULL, (LM_REAL *)&aux, &worksz, &info);
|
|---|
| 110 | GEQP3((int *)&tm, (int *)&tn, NULL, (int *)&tm, NULL, NULL, (LM_REAL *)&aux, (int *)&worksz, &info);
|
|---|
| 111 | worksz=(int)aux;
|
|---|
| 112 | a_sz=tm*tm; // tm*tn is enough for xgeqp3()
|
|---|
| 113 | jpvt_sz=tn;
|
|---|
| 114 | tau_sz=mintmn;
|
|---|
| 115 | r_sz=mintmn*mintmn; // actually smaller if a is not of full row rank
|
|---|
| 116 | Y_sz=(Y)? 0 : tm*tn;
|
|---|
| 117 |
|
|---|
| 118 | tot_sz=(a_sz + tau_sz + r_sz + worksz + Y_sz)*sizeof(LM_REAL) + jpvt_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
|
|---|
| 119 | buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */
|
|---|
| 120 | if(!buf){
|
|---|
| 121 | fprintf(stderr, RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()\n");
|
|---|
| 122 | return LM_ERROR;
|
|---|
| 123 | }
|
|---|
| 124 |
|
|---|
| 125 | a=buf;
|
|---|
| 126 | tau=a+a_sz;
|
|---|
| 127 | r=tau+tau_sz;
|
|---|
| 128 | work=r+r_sz;
|
|---|
| 129 | if(!Y){
|
|---|
| 130 | Y=work+worksz;
|
|---|
| 131 | jpvt=(int *)(Y+Y_sz);
|
|---|
| 132 | }
|
|---|
| 133 | else
|
|---|
| 134 | jpvt=(int *)(work+worksz);
|
|---|
| 135 |
|
|---|
| 136 | /* copy input array so that LAPACK won't destroy it. Note that copying is
|
|---|
| 137 | * done in row-major order, which equals A^T in column-major
|
|---|
| 138 | */
|
|---|
| 139 | for(i=0; i<tm*tn; ++i)
|
|---|
| 140 | a[i]=A[i];
|
|---|
| 141 |
|
|---|
| 142 | /* clear jpvt */
|
|---|
| 143 | for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;
|
|---|
| 144 |
|
|---|
| 145 | /* rank revealing QR decomposition of A^T*/
|
|---|
| 146 | GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);
|
|---|
| 147 | //dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);
|
|---|
| 148 | /* error checking */
|
|---|
| 149 | if(info!=0){
|
|---|
| 150 | if(info<0){
|
|---|
| 151 | fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()\n", -info);
|
|---|
| 152 | }
|
|---|
| 153 | else if(info>0){
|
|---|
| 154 | fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()\n", info);
|
|---|
| 155 | }
|
|---|
| 156 | free(buf);
|
|---|
| 157 | return LM_ERROR;
|
|---|
| 158 | }
|
|---|
| 159 | /* the upper triangular part of a now contains the upper triangle of the unpermuted R */
|
|---|
| 160 |
|
|---|
| 161 | if(eps<0.0){
|
|---|
| 162 | LM_REAL aux;
|
|---|
| 163 |
|
|---|
| 164 | /* compute machine epsilon. DBL_EPSILON should do also */
|
|---|
| 165 | for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
|
|---|
| 166 | ;
|
|---|
| 167 | eps*=LM_CNST(2.0);
|
|---|
| 168 | }
|
|---|
| 169 |
|
|---|
| 170 | tmp=tm*LM_CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)
|
|---|
| 171 | tmp=(tmp>LM_CNST(1E-12))? tmp : LM_CNST(1E-12); // ensure that threshold is not too small
|
|---|
| 172 | /* compute A^T's numerical rank by counting the non-zeros in R's diagonal */
|
|---|
| 173 | for(i=rank=0; i<mintmn; ++i)
|
|---|
| 174 | if(a[i*(tm+1)]>tmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */
|
|---|
| 175 | else break; /* diagonal is arranged in absolute decreasing order */
|
|---|
| 176 |
|
|---|
| 177 | if(rank<tn){
|
|---|
| 178 | fprintf(stderr, RCAT("\nConstraints matrix in ", LMLEC_ELIM) "() is not of full row rank (i.e. %d < %d)!\n"
|
|---|
| 179 | "Make sure that you do not specify redundant or inconsistent constraints.\n\n", rank, tn);
|
|---|
| 180 | free(buf);
|
|---|
| 181 | return LM_ERROR;
|
|---|
| 182 | }
|
|---|
| 183 |
|
|---|
| 184 | /* compute the permuted inverse transpose of R */
|
|---|
| 185 | /* first, copy R from the upper triangular part of a to the lower part of r (thus transposing it). R is rank x rank */
|
|---|
| 186 | for(j=0; j<rank; ++j){
|
|---|
| 187 | for(i=0; i<=j; ++i)
|
|---|
| 188 | r[j+i*rank]=a[i+j*tm];
|
|---|
| 189 | for(i=j+1; i<rank; ++i)
|
|---|
| 190 | r[j+i*rank]=0.0; // upper part is zero
|
|---|
| 191 | }
|
|---|
| 192 | /* r now contains R^T */
|
|---|
| 193 |
|
|---|
| 194 | /* compute the inverse */
|
|---|
| 195 | TRTRI("L", "N", (int *)&rank, r, (int *)&rank, &info);
|
|---|
| 196 | /* error checking */
|
|---|
| 197 | if(info!=0){
|
|---|
| 198 | if(info<0){
|
|---|
| 199 | fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRI) " in ", LMLEC_ELIM) "()\n", -info);
|
|---|
| 200 | }
|
|---|
| 201 | else if(info>0){
|
|---|
| 202 | fprintf(stderr, RCAT(RCAT("A(%d, %d) is exactly zero for ", TRTRI) " (singular matrix) in ", LMLEC_ELIM) "()\n", info, info);
|
|---|
| 203 | }
|
|---|
| 204 | free(buf);
|
|---|
| 205 | return LM_ERROR;
|
|---|
| 206 | }
|
|---|
| 207 |
|
|---|
| 208 | /* finally, permute R^-T using Y as intermediate storage */
|
|---|
| 209 | for(j=0; j<rank; ++j)
|
|---|
| 210 | for(i=0, k=jpvt[j]-1; i<rank; ++i)
|
|---|
| 211 | Y[i+k*rank]=r[i+j*rank];
|
|---|
| 212 |
|
|---|
| 213 | for(i=0; i<rank*rank; ++i) // copy back to r
|
|---|
| 214 | r[i]=Y[i];
|
|---|
| 215 |
|
|---|
| 216 | /* resize a to be tm x tm, filling with zeroes */
|
|---|
| 217 | for(i=tm*tn; i<tm*tm; ++i)
|
|---|
| 218 | a[i]=0.0;
|
|---|
| 219 |
|
|---|
| 220 | /* compute Q in a as the product of elementary reflectors. Q is tm x tm */
|
|---|
| 221 | ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, a, (int *)&tm, tau, work, &worksz, &info);
|
|---|
| 222 | /* error checking */
|
|---|
| 223 | if(info!=0){
|
|---|
| 224 | if(info<0){
|
|---|
| 225 | fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", LMLEC_ELIM) "()\n", -info);
|
|---|
| 226 | }
|
|---|
| 227 | else if(info>0){
|
|---|
| 228 | fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", ORGQR) " in ", LMLEC_ELIM) "()\n", info);
|
|---|
| 229 | }
|
|---|
| 230 | free(buf);
|
|---|
| 231 | return LM_ERROR;
|
|---|
| 232 | }
|
|---|
| 233 |
|
|---|
| 234 | /* compute Y=Q_1*R^-T*P^T. Y is tm x rank */
|
|---|
| 235 | for(i=0; i<tm; ++i)
|
|---|
| 236 | for(j=0; j<rank; ++j){
|
|---|
| 237 | for(k=0, tmp=0.0; k<rank; ++k)
|
|---|
| 238 | tmp+=a[i+k*tm]*r[k+j*rank];
|
|---|
| 239 | Y[i*rank+j]=tmp;
|
|---|
| 240 | }
|
|---|
| 241 |
|
|---|
| 242 | if(b && c){
|
|---|
| 243 | /* compute c=Y*b */
|
|---|
| 244 | for(i=0; i<tm; ++i){
|
|---|
| 245 | for(j=0, tmp=0.0; j<rank; ++j)
|
|---|
| 246 | tmp+=Y[i*rank+j]*b[j];
|
|---|
| 247 |
|
|---|
| 248 | c[i]=tmp;
|
|---|
| 249 | }
|
|---|
| 250 | }
|
|---|
| 251 |
|
|---|
| 252 | /* copy Q_2 into Z. Z is tm x (tm-rank) */
|
|---|
| 253 | for(j=0; j<tm-rank; ++j)
|
|---|
| 254 | for(i=0, k=j+rank; i<tm; ++i)
|
|---|
| 255 | Z[i*(tm-rank)+j]=a[i+k*tm];
|
|---|
| 256 |
|
|---|
| 257 | free(buf);
|
|---|
| 258 |
|
|---|
| 259 | return rank;
|
|---|
| 260 | }
|
|---|
| 261 |
|
|---|
| 262 | /* constrained measurements: given pp, compute the measurements at c + Z*pp */
|
|---|
| 263 | static void LMLEC_FUNC(LM_REAL *pp, LM_REAL *hx, int mm, int n, void *adata)
|
|---|
| 264 | {
|
|---|
| 265 | struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
|
|---|
| 266 | int m;
|
|---|
| 267 | register int i, j;
|
|---|
| 268 | register LM_REAL sum;
|
|---|
| 269 | LM_REAL *c, *Z, *p, *Zimm;
|
|---|
| 270 |
|
|---|
| 271 | m=mm+data->ncnstr;
|
|---|
| 272 | c=data->c;
|
|---|
| 273 | Z=data->Z;
|
|---|
| 274 | p=data->p;
|
|---|
| 275 | /* p=c + Z*pp */
|
|---|
| 276 | for(i=0; i<m; ++i){
|
|---|
| 277 | Zimm=Z+i*mm;
|
|---|
| 278 | for(j=0, sum=c[i]; j<mm; ++j)
|
|---|
| 279 | sum+=Zimm[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
|
|---|
| 280 | p[i]=sum;
|
|---|
| 281 | }
|
|---|
| 282 |
|
|---|
| 283 | (*(data->func))(p, hx, m, n, data->adata);
|
|---|
| 284 | }
|
|---|
| 285 |
|
|---|
| 286 | /* constrained Jacobian: given pp, compute the Jacobian at c + Z*pp
|
|---|
| 287 | * Using the chain rule, the Jacobian with respect to pp equals the
|
|---|
| 288 | * product of the Jacobian with respect to p (at c + Z*pp) times Z
|
|---|
| 289 | */
|
|---|
| 290 | static void LMLEC_JACF(LM_REAL *pp, LM_REAL *jacjac, int mm, int n, void *adata)
|
|---|
| 291 | {
|
|---|
| 292 | struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
|
|---|
| 293 | int m;
|
|---|
| 294 | register int i, j, l;
|
|---|
| 295 | register LM_REAL sum, *aux1, *aux2;
|
|---|
| 296 | LM_REAL *c, *Z, *p, *jac;
|
|---|
| 297 |
|
|---|
| 298 | m=mm+data->ncnstr;
|
|---|
| 299 | c=data->c;
|
|---|
| 300 | Z=data->Z;
|
|---|
| 301 | p=data->p;
|
|---|
| 302 | jac=data->jac;
|
|---|
| 303 | /* p=c + Z*pp */
|
|---|
| 304 | for(i=0; i<m; ++i){
|
|---|
| 305 | aux1=Z+i*mm;
|
|---|
| 306 | for(j=0, sum=c[i]; j<mm; ++j)
|
|---|
| 307 | sum+=aux1[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
|
|---|
| 308 | p[i]=sum;
|
|---|
| 309 | }
|
|---|
| 310 |
|
|---|
| 311 | (*(data->jacf))(p, jac, m, n, data->adata);
|
|---|
| 312 |
|
|---|
| 313 | /* compute jac*Z in jacjac */
|
|---|
| 314 | if(n*m<=__BLOCKSZ__SQ){ // this is a small problem
|
|---|
| 315 | /* This is the straightforward way to compute jac*Z. However, due to
|
|---|
| 316 | * its noncontinuous memory access pattern, it incures many cache misses when
|
|---|
| 317 | * applied to large minimization problems (i.e. problems involving a large
|
|---|
| 318 | * number of free variables and measurements), in which jac is too large to
|
|---|
| 319 | * fit in the L1 cache. For such problems, a cache-efficient blocking scheme
|
|---|
| 320 | * is preferable. On the other hand, the straightforward algorithm is faster
|
|---|
| 321 | * on small problems since in this case it avoids the overheads of blocking.
|
|---|
| 322 | */
|
|---|
| 323 |
|
|---|
| 324 | for(i=0; i<n; ++i){
|
|---|
| 325 | aux1=jac+i*m;
|
|---|
| 326 | aux2=jacjac+i*mm;
|
|---|
| 327 | for(j=0; j<mm; ++j){
|
|---|
| 328 | for(l=0, sum=0.0; l<m; ++l)
|
|---|
| 329 | sum+=aux1[l]*Z[l*mm+j]; // sum+=jac[i*m+l]*Z[l*mm+j];
|
|---|
| 330 |
|
|---|
| 331 | aux2[j]=sum; // jacjac[i*mm+j]=sum;
|
|---|
| 332 | }
|
|---|
| 333 | }
|
|---|
| 334 | }
|
|---|
| 335 | else{ // this is a large problem
|
|---|
| 336 | /* Cache efficient computation of jac*Z based on blocking
|
|---|
| 337 | */
|
|---|
| 338 | #define __MIN__(x, y) (((x)<=(y))? (x) : (y))
|
|---|
| 339 | register int jj, ll;
|
|---|
| 340 |
|
|---|
| 341 | for(jj=0; jj<mm; jj+=__BLOCKSZ__){
|
|---|
| 342 | for(i=0; i<n; ++i){
|
|---|
| 343 | aux1=jacjac+i*mm;
|
|---|
| 344 | for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j)
|
|---|
| 345 | aux1[j]=0.0; //jacjac[i*mm+j]=0.0;
|
|---|
| 346 | }
|
|---|
| 347 |
|
|---|
| 348 | for(ll=0; ll<m; ll+=__BLOCKSZ__){
|
|---|
| 349 | for(i=0; i<n; ++i){
|
|---|
| 350 | aux1=jacjac+i*mm; aux2=jac+i*m;
|
|---|
| 351 | for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j){
|
|---|
| 352 | sum=0.0;
|
|---|
| 353 | for(l=ll; l<__MIN__(ll+__BLOCKSZ__, m); ++l)
|
|---|
| 354 | sum+=aux2[l]*Z[l*mm+j]; //jac[i*m+l]*Z[l*mm+j];
|
|---|
| 355 | aux1[j]+=sum; //jacjac[i*mm+j]+=sum;
|
|---|
| 356 | }
|
|---|
| 357 | }
|
|---|
| 358 | }
|
|---|
| 359 | }
|
|---|
| 360 | }
|
|---|
| 361 | }
|
|---|
| 362 | #undef __MIN__
|
|---|
| 363 |
|
|---|
| 364 |
|
|---|
| 365 | /*
|
|---|
| 366 | * This function is similar to LEVMAR_DER except that the minimization
|
|---|
| 367 | * is performed subject to the linear constraints A p=b, A is kxm, b kx1
|
|---|
| 368 | *
|
|---|
| 369 | * This function requires an analytic Jacobian. In case the latter is unavailable,
|
|---|
| 370 | * use LEVMAR_LEC_DIF() bellow
|
|---|
| 371 | *
|
|---|
| 372 | */
|
|---|
| 373 | int LEVMAR_LEC_DER(
|
|---|
| 374 | 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 */
|
|---|
| 375 | void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata), /* function to evaluate the Jacobian \part x / \part p */
|
|---|
| 376 | LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
|
|---|
| 377 | LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
|
|---|
| 378 | int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
|---|
| 379 | int n, /* I: measurement vector dimension */
|
|---|
| 380 | LM_REAL *A, /* I: constraints matrix, kxm */
|
|---|
| 381 | LM_REAL *b, /* I: right hand constraints vector, kx1 */
|
|---|
| 382 | int k, /* I: number of constraints (i.e. A's #rows) */
|
|---|
| 383 | int itmax, /* I: maximum number of iterations */
|
|---|
| 384 | LM_REAL opts[4], /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
|
|---|
| 385 | * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
|
|---|
| 386 | */
|
|---|
| 387 | LM_REAL info[LM_INFO_SZ],
|
|---|
| 388 | /* O: information regarding the minimization. Set to NULL if don't care
|
|---|
| 389 | * info[0]= ||e||_2 at initial p.
|
|---|
| 390 | * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
|
|---|
| 391 | * info[5]= # iterations,
|
|---|
| 392 | * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
|
|---|
| 393 | * 2 - stopped by small Dp
|
|---|
| 394 | * 3 - stopped by itmax
|
|---|
| 395 | * 4 - singular matrix. Restart from current p with increased mu
|
|---|
| 396 | * 5 - no further error reduction is possible. Restart with increased mu
|
|---|
| 397 | * 6 - stopped by small ||e||_2
|
|---|
| 398 | * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
|---|
| 399 | * info[7]= # function evaluations
|
|---|
| 400 | * info[8]= # Jacobian evaluations
|
|---|
| 401 | * info[9]= # linear systems solved, i.e. # attempts for reducing error
|
|---|
| 402 | */
|
|---|
| 403 | LM_REAL *work, /* working memory at least LM_LEC_DER_WORKSZ() reals large, allocated if NULL */
|
|---|
| 404 | LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
|
|---|
| 405 | void *adata) /* pointer to possibly additional data, passed uninterpreted to func & jacf.
|
|---|
| 406 | * Set to NULL if not needed
|
|---|
| 407 | */
|
|---|
| 408 | {
|
|---|
| 409 | struct LMLEC_DATA data;
|
|---|
| 410 | LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
|
|---|
| 411 | int mm, ret;
|
|---|
| 412 | register int i, j;
|
|---|
| 413 | register LM_REAL tmp;
|
|---|
| 414 | LM_REAL locinfo[LM_INFO_SZ];
|
|---|
| 415 |
|
|---|
| 416 | if(!jacf){
|
|---|
| 417 | fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_LEC_DER)
|
|---|
| 418 | RCAT("().\nIf no such function is available, use ", LEVMAR_LEC_DIF) RCAT("() rather than ", LEVMAR_LEC_DER) "()\n");
|
|---|
| 419 | return LM_ERROR;
|
|---|
| 420 | }
|
|---|
| 421 |
|
|---|
| 422 | mm=m-k;
|
|---|
| 423 |
|
|---|
| 424 | if(n<mm){
|
|---|
| 425 | fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
|
|---|
| 426 | return LM_ERROR;
|
|---|
| 427 | }
|
|---|
| 428 |
|
|---|
| 429 | ptr=(LM_REAL *)malloc((2*m + m*mm + n*m + mm)*sizeof(LM_REAL));
|
|---|
| 430 | if(!ptr){
|
|---|
| 431 | fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): memory allocation request failed\n"));
|
|---|
| 432 | return LM_ERROR;
|
|---|
| 433 | }
|
|---|
| 434 | data.p=p;
|
|---|
| 435 | p0=ptr;
|
|---|
| 436 | data.c=p0+m;
|
|---|
| 437 | data.Z=Z=data.c+m;
|
|---|
| 438 | data.jac=data.Z+m*mm;
|
|---|
| 439 | pp=data.jac+n*m;
|
|---|
| 440 | data.ncnstr=k;
|
|---|
| 441 | data.func=func;
|
|---|
| 442 | data.jacf=jacf;
|
|---|
| 443 | data.adata=adata;
|
|---|
| 444 |
|
|---|
| 445 | ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
|
|---|
| 446 | if(ret==LM_ERROR){
|
|---|
| 447 | free(ptr);
|
|---|
| 448 | return LM_ERROR;
|
|---|
| 449 | }
|
|---|
| 450 |
|
|---|
| 451 | /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
|
|---|
| 452 | * Due to orthogonality, Z^T Z = I and the last equation
|
|---|
| 453 | * becomes pp=Z^T*(p-c). Also, save the starting p in p0
|
|---|
| 454 | */
|
|---|
| 455 | for(i=0; i<m; ++i){
|
|---|
| 456 | p0[i]=p[i];
|
|---|
| 457 | p[i]-=data.c[i];
|
|---|
| 458 | }
|
|---|
| 459 |
|
|---|
| 460 | /* Z^T*(p-c) */
|
|---|
| 461 | for(i=0; i<mm; ++i){
|
|---|
| 462 | for(j=0, tmp=0.0; j<m; ++j)
|
|---|
| 463 | tmp+=Z[j*mm+i]*p[j];
|
|---|
| 464 | pp[i]=tmp;
|
|---|
| 465 | }
|
|---|
| 466 |
|
|---|
| 467 | /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
|
|---|
| 468 | for(i=0; i<m; ++i){
|
|---|
| 469 | Zimm=Z+i*mm;
|
|---|
| 470 | for(j=0, tmp=data.c[i]; j<mm; ++j)
|
|---|
| 471 | tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
|
|---|
| 472 | if(FABS(tmp-p0[i])>LM_CNST(1E-03))
|
|---|
| 473 | fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DER) "()! [%.10g reset to %.10g]\n",
|
|---|
| 474 | i, p0[i], tmp);
|
|---|
| 475 | }
|
|---|
| 476 |
|
|---|
| 477 | if(!info) info=locinfo; /* make sure that LEVMAR_DER() is called with non-null info */
|
|---|
| 478 | /* note that covariance computation is not requested from LEVMAR_DER() */
|
|---|
| 479 | ret=LEVMAR_DER(LMLEC_FUNC, LMLEC_JACF, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
|
|---|
| 480 |
|
|---|
| 481 | /* p=c + Z*pp */
|
|---|
| 482 | for(i=0; i<m; ++i){
|
|---|
| 483 | Zimm=Z+i*mm;
|
|---|
| 484 | for(j=0, tmp=data.c[i]; j<mm; ++j)
|
|---|
| 485 | tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
|
|---|
| 486 | p[i]=tmp;
|
|---|
| 487 | }
|
|---|
| 488 |
|
|---|
| 489 | /* compute the covariance from the Jacobian in data.jac */
|
|---|
| 490 | if(covar){
|
|---|
| 491 | LEVMAR_TRANS_MAT_MAT_MULT(data.jac, covar, n, m); /* covar = J^T J */
|
|---|
| 492 | LEVMAR_COVAR(covar, covar, info[1], m, n);
|
|---|
| 493 | }
|
|---|
| 494 |
|
|---|
| 495 | free(ptr);
|
|---|
| 496 |
|
|---|
| 497 | return ret;
|
|---|
| 498 | }
|
|---|
| 499 |
|
|---|
| 500 | /* Similar to the LEVMAR_LEC_DER() function above, except that the Jacobian is approximated
|
|---|
| 501 | * with the aid of finite differences (forward or central, see the comment for the opts argument)
|
|---|
| 502 | */
|
|---|
| 503 | int LEVMAR_LEC_DIF(
|
|---|
| 504 | 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 */
|
|---|
| 505 | LM_REAL *p, /* I/O: initial parameter estimates. On output has the estimated solution */
|
|---|
| 506 | LM_REAL *x, /* I: measurement vector. NULL implies a zero vector */
|
|---|
| 507 | int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
|---|
| 508 | int n, /* I: measurement vector dimension */
|
|---|
| 509 | LM_REAL *A, /* I: constraints matrix, kxm */
|
|---|
| 510 | LM_REAL *b, /* I: right hand constraints vector, kx1 */
|
|---|
| 511 | int k, /* I: number of constraints (i.e. A's #rows) */
|
|---|
| 512 | int itmax, /* I: maximum number of iterations */
|
|---|
| 513 | LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
|
|---|
| 514 | * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
|
|---|
| 515 | * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
|
|---|
| 516 | * If \delta<0, the Jacobian is approximated with central differences which are more accurate
|
|---|
| 517 | * (but slower!) compared to the forward differences employed by default.
|
|---|
| 518 | */
|
|---|
| 519 | LM_REAL info[LM_INFO_SZ],
|
|---|
| 520 | /* O: information regarding the minimization. Set to NULL if don't care
|
|---|
| 521 | * info[0]= ||e||_2 at initial p.
|
|---|
| 522 | * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
|
|---|
| 523 | * info[5]= # iterations,
|
|---|
| 524 | * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
|
|---|
| 525 | * 2 - stopped by small Dp
|
|---|
| 526 | * 3 - stopped by itmax
|
|---|
| 527 | * 4 - singular matrix. Restart from current p with increased mu
|
|---|
| 528 | * 5 - no further error reduction is possible. Restart with increased mu
|
|---|
| 529 | * 6 - stopped by small ||e||_2
|
|---|
| 530 | * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
|---|
| 531 | * info[7]= # function evaluations
|
|---|
| 532 | * info[8]= # Jacobian evaluations
|
|---|
| 533 | * info[9]= # linear systems solved, i.e. # attempts for reducing error
|
|---|
| 534 | */
|
|---|
| 535 | LM_REAL *work, /* working memory at least LM_LEC_DIF_WORKSZ() reals large, allocated if NULL */
|
|---|
| 536 | LM_REAL *covar, /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
|
|---|
| 537 | void *adata) /* pointer to possibly additional data, passed uninterpreted to func.
|
|---|
| 538 | * Set to NULL if not needed
|
|---|
| 539 | */
|
|---|
| 540 | {
|
|---|
| 541 | struct LMLEC_DATA data;
|
|---|
| 542 | LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
|
|---|
| 543 | int mm, ret;
|
|---|
| 544 | register int i, j;
|
|---|
| 545 | register LM_REAL tmp;
|
|---|
| 546 | LM_REAL locinfo[LM_INFO_SZ];
|
|---|
| 547 |
|
|---|
| 548 | mm=m-k;
|
|---|
| 549 |
|
|---|
| 550 | if(n<mm){
|
|---|
| 551 | fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
|
|---|
| 552 | return LM_ERROR;
|
|---|
| 553 | }
|
|---|
| 554 |
|
|---|
| 555 | ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));
|
|---|
| 556 | if(!ptr){
|
|---|
| 557 | fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
|
|---|
| 558 | return LM_ERROR;
|
|---|
| 559 | }
|
|---|
| 560 | data.p=p;
|
|---|
| 561 | p0=ptr;
|
|---|
| 562 | data.c=p0+m;
|
|---|
| 563 | data.Z=Z=data.c+m;
|
|---|
| 564 | data.jac=NULL;
|
|---|
| 565 | pp=data.Z+m*mm;
|
|---|
| 566 | data.ncnstr=k;
|
|---|
| 567 | data.func=func;
|
|---|
| 568 | data.jacf=NULL;
|
|---|
| 569 | data.adata=adata;
|
|---|
| 570 |
|
|---|
| 571 | ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
|
|---|
| 572 | if(ret==LM_ERROR){
|
|---|
| 573 | free(ptr);
|
|---|
| 574 | return LM_ERROR;
|
|---|
| 575 | }
|
|---|
| 576 |
|
|---|
| 577 | /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
|
|---|
| 578 | * Due to orthogonality, Z^T Z = I and the last equation
|
|---|
| 579 | * becomes pp=Z^T*(p-c). Also, save the starting p in p0
|
|---|
| 580 | */
|
|---|
| 581 | for(i=0; i<m; ++i){
|
|---|
| 582 | p0[i]=p[i];
|
|---|
| 583 | p[i]-=data.c[i];
|
|---|
| 584 | }
|
|---|
| 585 |
|
|---|
| 586 | /* Z^T*(p-c) */
|
|---|
| 587 | for(i=0; i<mm; ++i){
|
|---|
| 588 | for(j=0, tmp=0.0; j<m; ++j)
|
|---|
| 589 | tmp+=Z[j*mm+i]*p[j];
|
|---|
| 590 | pp[i]=tmp;
|
|---|
| 591 | }
|
|---|
| 592 |
|
|---|
| 593 | /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
|
|---|
| 594 | for(i=0; i<m; ++i){
|
|---|
| 595 | Zimm=Z+i*mm;
|
|---|
| 596 | for(j=0, tmp=data.c[i]; j<mm; ++j)
|
|---|
| 597 | tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
|
|---|
| 598 | if(FABS(tmp-p0[i])>LM_CNST(1E-03))
|
|---|
| 599 | fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]\n",
|
|---|
| 600 | i, p0[i], tmp);
|
|---|
| 601 | }
|
|---|
| 602 |
|
|---|
| 603 | if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */
|
|---|
| 604 | /* note that covariance computation is not requested from LEVMAR_DIF() */
|
|---|
| 605 | ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
|
|---|
| 606 |
|
|---|
| 607 | /* p=c + Z*pp */
|
|---|
| 608 | for(i=0; i<m; ++i){
|
|---|
| 609 | Zimm=Z+i*mm;
|
|---|
| 610 | for(j=0, tmp=data.c[i]; j<mm; ++j)
|
|---|
| 611 | tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
|
|---|
| 612 | p[i]=tmp;
|
|---|
| 613 | }
|
|---|
| 614 |
|
|---|
| 615 | /* compute the Jacobian with finite differences and use it to estimate the covariance */
|
|---|
| 616 | if(covar){
|
|---|
| 617 | LM_REAL *hx, *wrk, *jac;
|
|---|
| 618 |
|
|---|
| 619 | hx=(LM_REAL *)malloc((2*n+n*m)*sizeof(LM_REAL));
|
|---|
| 620 | if(!hx){
|
|---|
| 621 | fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
|
|---|
| 622 | free(ptr);
|
|---|
| 623 | return LM_ERROR;
|
|---|
| 624 | }
|
|---|
| 625 |
|
|---|
| 626 | wrk=hx+n;
|
|---|
| 627 | jac=wrk+n;
|
|---|
| 628 |
|
|---|
| 629 | (*func)(p, hx, m, n, adata); /* evaluate function at p */
|
|---|
| 630 | LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the Jacobian at p */
|
|---|
| 631 | LEVMAR_TRANS_MAT_MAT_MULT(jac, covar, n, m); /* covar = J^T J */
|
|---|
| 632 | LEVMAR_COVAR(covar, covar, info[1], m, n);
|
|---|
| 633 | free(hx);
|
|---|
| 634 | }
|
|---|
| 635 |
|
|---|
| 636 | free(ptr);
|
|---|
| 637 |
|
|---|
| 638 | return ret;
|
|---|
| 639 | }
|
|---|
| 640 |
|
|---|
| 641 | /* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
|
|---|
| 642 | #undef LMLEC_DATA
|
|---|
| 643 | #undef LMLEC_ELIM
|
|---|
| 644 | #undef LMLEC_FUNC
|
|---|
| 645 | #undef LMLEC_JACF
|
|---|
| 646 | #undef LEVMAR_FDIF_FORW_JAC_APPROX
|
|---|
| 647 | #undef LEVMAR_COVAR
|
|---|
| 648 | #undef LEVMAR_TRANS_MAT_MAT_MULT
|
|---|
| 649 | #undef LEVMAR_LEC_DER
|
|---|
| 650 | #undef LEVMAR_LEC_DIF
|
|---|
| 651 | #undef LEVMAR_DER
|
|---|
| 652 | #undef LEVMAR_DIF
|
|---|
| 653 |
|
|---|
| 654 | #undef GEQP3
|
|---|
| 655 | #undef ORGQR
|
|---|
| 656 | #undef TRTRI
|
|---|
