nmelone/GoldenCheetah
1
0
Fork 0
mirror of https://github.com/GoldenCheetah/GoldenCheetah.git synced 2026-02-13 08:08:42 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
e1ac00b8604e40b70875cdae91a4c10fe53bd528
GoldenCheetah/contrib/levmar/lmlec_core.c
Mark Liversedge e1ac00b860 Move contributed sources to contrib directory 
.. Makes it easier to identify code that has been snaffled in from
   other repositories and check licensing

.. The httpserver is now no longer optional, since it is delivered
   as contributed source.
2021-05-16 10:33:09 +01:00

657 lines
24 KiB
C
Raw Blame History

/////////////////////////////////////////////////////////////////////////////////
//
// Levenberg - Marquardt non-linear minimization algorithm
// Copyright (C) 2004-05 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 lmlec.c
#error This file should not be compiled directly!
#endif
/* precision-specific definitions */
#define LMLEC_DATA LM_ADD_PREFIX(lmlec_data)
#define LMLEC_ELIM LM_ADD_PREFIX(lmlec_elim)
#define LMLEC_FUNC LM_ADD_PREFIX(lmlec_func)
#define LMLEC_JACF LM_ADD_PREFIX(lmlec_jacf)
#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)
#define LEVMAR_DIF LM_ADD_PREFIX(levmar_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)
#define GEQP3 LM_MK_LAPACK_NAME(geqp3)
#define ORGQR LM_MK_LAPACK_NAME(orgqr)
#define TRTRI LM_MK_LAPACK_NAME(trtri)
struct LMLEC_DATA{
LM_REAL *c, *Z, *p, *jac;
int ncnstr;
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;
};
/* prototypes for LAPACK routines */
#ifdef __cplusplus
extern "C" {
#endif
extern int GEQP3(int *m, int *n, LM_REAL *a, int *lda, int *jpvt,
LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau,
LM_REAL *work, int *lwork, int *info);
extern int TRTRI(char *uplo, char *diag, int *n, LM_REAL *a, int *lda, int *info);
#ifdef __cplusplus
}
#endif
/*
* This function implements an elimination strategy for linearly constrained
* optimization problems. The strategy relies on QR decomposition to transform
* an optimization problem constrained by Ax=b to an equivalent, unconstrained
* one. Also referred to as "null space" or "reduced Hessian" method.
* See pp. 430-433 (chap. 15) of "Numerical Optimization" by Nocedal-Wright
* for details.
*
* A is mxn with m<=n and rank(A)=m
* Two matrices Y and Z of dimensions nxm and nx(n-m) are computed from A^T so that
* their columns are orthonormal and every x can be written as x=Y*b + Z*x_z=
* c + Z*x_z, where c=Y*b is a fixed vector of dimension n and x_z is an
* arbitrary vector of dimension n-m. Then, the problem of minimizing f(x)
* subject to Ax=b is equivalent to minimizing f(c + Z*x_z) with no constraints.
* The computed Y and Z are such that any solution of Ax=b can be written as
* x=Y*x_y + Z*x_z for some x_y, x_z. Furthermore, A*Y is nonsingular, A*Z=0
* and Z spans the null space of A.
*
* The function accepts A, b and computes c, Y, Z. If b or c is NULL, c is not
* computed. Also, Y can be NULL in which case it is not referenced.
* The function returns LM_ERROR in case of error, A's computed rank if successful
*
*/
static int LMLEC_ELIM(LM_REAL *A, LM_REAL *b, LM_REAL *c, LM_REAL *Y, LM_REAL *Z, int m, int n)
{
static LM_REAL eps=LM_CNST(-1.0);
LM_REAL *buf=NULL;
LM_REAL *a, *tau, *work, *r, aux;
register LM_REAL tmp;
int a_sz, jpvt_sz, tau_sz, r_sz, Y_sz, worksz;
int info, rank, *jpvt, tot_sz, mintmn, tm, tn;
register int i, j, k;
if(m>n){
fprintf(stderr, RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!\n");
return LM_ERROR;
}
tm=n; tn=m; // transpose dimensions
mintmn=m;
/* calculate required memory size */
worksz=-1; // workspace query. Optimal work size is returned in aux
//ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, NULL, (int *)&tm, NULL, (LM_REAL *)&aux, &worksz, &info);
GEQP3((int *)&tm, (int *)&tn, NULL, (int *)&tm, NULL, NULL, (LM_REAL *)&aux, (int *)&worksz, &info);
worksz=(int)aux;
a_sz=tm*tm; // tm*tn is enough for xgeqp3()
jpvt_sz=tn;
tau_sz=mintmn;
r_sz=mintmn*mintmn; // actually smaller if a is not of full row rank
Y_sz=(Y)? 0 : tm*tn;
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 */
buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */
if(!buf){
fprintf(stderr, RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()\n");
return LM_ERROR;
}
a=buf;
tau=a+a_sz;
r=tau+tau_sz;
work=r+r_sz;
if(!Y){
Y=work+worksz;
jpvt=(int *)(Y+Y_sz);
}
else
jpvt=(int *)(work+worksz);
/* copy input array so that LAPACK won't destroy it. Note that copying is
* done in row-major order, which equals A^T in column-major
*/
for(i=0; i<tm*tn; ++i)
a[i]=A[i];
/* clear jpvt */
for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;
/* rank revealing QR decomposition of A^T*/
GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);
//dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);
/* error checking */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()\n", -info);
}
else if(info>0){
fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()\n", info);
}
free(buf);
return LM_ERROR;
}
/* the upper triangular part of a now contains the upper triangle of the unpermuted R */
if(eps<0.0){
LM_REAL aux;
/* compute machine epsilon. DBL_EPSILON should do also */
for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
;
eps*=LM_CNST(2.0);
}
tmp=tm*LM_CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)
tmp=(tmp>LM_CNST(1E-12))? tmp : LM_CNST(1E-12); // ensure that threshold is not too small
/* compute A^T's numerical rank by counting the non-zeros in R's diagonal */
for(i=rank=0; i<mintmn; ++i)
if(a[i*(tm+1)]>tmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */
else break; /* diagonal is arranged in absolute decreasing order */
if(rank<tn){
fprintf(stderr, RCAT("\nConstraints matrix in ", LMLEC_ELIM) "() is not of full row rank (i.e. %d < %d)!\n"
"Make sure that you do not specify redundant or inconsistent constraints.\n\n", rank, tn);
free(buf);
return LM_ERROR;
}
/* compute the permuted inverse transpose of R */
/* first, copy R from the upper triangular part of a to the lower part of r (thus transposing it). R is rank x rank */
for(j=0; j<rank; ++j){
for(i=0; i<=j; ++i)
r[j+i*rank]=a[i+j*tm];
for(i=j+1; i<rank; ++i)
r[j+i*rank]=0.0; // upper part is zero
}
/* r now contains R^T */
/* compute the inverse */
TRTRI("L", "N", (int *)&rank, r, (int *)&rank, &info);
/* error checking */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRI) " in ", LMLEC_ELIM) "()\n", -info);
}
else if(info>0){
fprintf(stderr, RCAT(RCAT("A(%d, %d) is exactly zero for ", TRTRI) " (singular matrix) in ", LMLEC_ELIM) "()\n", info, info);
}
free(buf);
return LM_ERROR;
}
/* finally, permute R^-T using Y as intermediate storage */
for(j=0; j<rank; ++j)
for(i=0, k=jpvt[j]-1; i<rank; ++i)
Y[i+k*rank]=r[i+j*rank];
for(i=0; i<rank*rank; ++i) // copy back to r
r[i]=Y[i];
/* resize a to be tm x tm, filling with zeroes */
for(i=tm*tn; i<tm*tm; ++i)
a[i]=0.0;
/* compute Q in a as the product of elementary reflectors. Q is tm x tm */
ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, a, (int *)&tm, tau, work, &worksz, &info);
/* error checking */
if(info!=0){
if(info<0){
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", LMLEC_ELIM) "()\n", -info);
}
else if(info>0){
fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", ORGQR) " in ", LMLEC_ELIM) "()\n", info);
}
free(buf);
return LM_ERROR;
}
/* compute Y=Q_1*R^-T*P^T. Y is tm x rank */
for(i=0; i<tm; ++i)
for(j=0; j<rank; ++j){
for(k=0, tmp=0.0; k<rank; ++k)
tmp+=a[i+k*tm]*r[k+j*rank];
Y[i*rank+j]=tmp;
}
if(b && c){
/* compute c=Y*b */
for(i=0; i<tm; ++i){
for(j=0, tmp=0.0; j<rank; ++j)
tmp+=Y[i*rank+j]*b[j];
c[i]=tmp;
}
}
/* copy Q_2 into Z. Z is tm x (tm-rank) */
for(j=0; j<tm-rank; ++j)
for(i=0, k=j+rank; i<tm; ++i)
Z[i*(tm-rank)+j]=a[i+k*tm];
free(buf);
return rank;
}
/* constrained measurements: given pp, compute the measurements at c + Z*pp */
static void LMLEC_FUNC(LM_REAL *pp, LM_REAL *hx, int mm, int n, void *adata)
{
struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
int m;
register int i, j;
register LM_REAL sum;
LM_REAL *c, *Z, *p, *Zimm;
m=mm+data->ncnstr;
c=data->c;
Z=data->Z;
p=data->p;
/* p=c + Z*pp */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, sum=c[i]; j<mm; ++j)
sum+=Zimm[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
p[i]=sum;
}
(*(data->func))(p, hx, m, n, data->adata);
}
/* constrained Jacobian: given pp, compute the Jacobian at c + Z*pp
* Using the chain rule, the Jacobian with respect to pp equals the
* product of the Jacobian with respect to p (at c + Z*pp) times Z
*/
static void LMLEC_JACF(LM_REAL *pp, LM_REAL *jacjac, int mm, int n, void *adata)
{
struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
int m;
register int i, j, l;
register LM_REAL sum, *aux1, *aux2;
LM_REAL *c, *Z, *p, *jac;
m=mm+data->ncnstr;
c=data->c;
Z=data->Z;
p=data->p;
jac=data->jac;
/* p=c + Z*pp */
for(i=0; i<m; ++i){
aux1=Z+i*mm;
for(j=0, sum=c[i]; j<mm; ++j)
sum+=aux1[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
p[i]=sum;
}
(*(data->jacf))(p, jac, m, n, data->adata);
/* compute jac*Z in jacjac */
if(n*m<=__BLOCKSZ__SQ){ // this is a small problem
/* This is the straightforward way to compute jac*Z. However, due to
* its noncontinuous memory access pattern, it incures many cache misses when
* applied to large minimization problems (i.e. problems involving a large
* number of free variables and measurements), in which jac is too large to
* fit in the L1 cache. For such problems, a cache-efficient blocking scheme
* is preferable. On the other hand, the straightforward algorithm is faster
* on small problems since in this case it avoids the overheads of blocking.
*/
for(i=0; i<n; ++i){
aux1=jac+i*m;
aux2=jacjac+i*mm;
for(j=0; j<mm; ++j){
for(l=0, sum=0.0; l<m; ++l)
sum+=aux1[l]*Z[l*mm+j]; // sum+=jac[i*m+l]*Z[l*mm+j];
aux2[j]=sum; // jacjac[i*mm+j]=sum;
}
}
}
else{ // this is a large problem
/* Cache efficient computation of jac*Z based on blocking
*/
#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
register int jj, ll;
for(jj=0; jj<mm; jj+=__BLOCKSZ__){
for(i=0; i<n; ++i){
aux1=jacjac+i*mm;
for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j)
aux1[j]=0.0; //jacjac[i*mm+j]=0.0;
}
for(ll=0; ll<m; ll+=__BLOCKSZ__){
for(i=0; i<n; ++i){
aux1=jacjac+i*mm; aux2=jac+i*m;
for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j){
sum=0.0;
for(l=ll; l<__MIN__(ll+__BLOCKSZ__, m); ++l)
sum+=aux2[l]*Z[l*mm+j]; //jac[i*m+l]*Z[l*mm+j];
aux1[j]+=sum; //jacjac[i*mm+j]+=sum;
}
}
}
}
}
}
#undef __MIN__
/*
* This function is similar to LEVMAR_DER except that the minimization
* is performed subject to the linear constraints A p=b, A is kxm, b kx1
*
* This function requires an analytic Jacobian. In case the latter is unavailable,
* use LEVMAR_LEC_DIF() bellow
*
*/
int LEVMAR_LEC_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 *A, /* I: constraints matrix, kxm */
LM_REAL *b, /* I: right hand constraints vector, kx1 */
int k, /* I: number of constraints (i.e. A'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_LEC_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 LMLEC_DATA data;
LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
int mm, ret;
register int i, j;
register LM_REAL tmp;
LM_REAL locinfo[LM_INFO_SZ];
if(!jacf){
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_LEC_DER)
RCAT("().\nIf no such function is available, use ", LEVMAR_LEC_DIF) RCAT("() rather than ", LEVMAR_LEC_DER) "()\n");
return LM_ERROR;
}
mm=m-k;
if(n<mm){
fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
return LM_ERROR;
}
ptr=(LM_REAL *)malloc((2*m + m*mm + n*m + mm)*sizeof(LM_REAL));
if(!ptr){
fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): memory allocation request failed\n"));
return LM_ERROR;
}
data.p=p;
p0=ptr;
data.c=p0+m;
data.Z=Z=data.c+m;
data.jac=data.Z+m*mm;
pp=data.jac+n*m;
data.ncnstr=k;
data.func=func;
data.jacf=jacf;
data.adata=adata;
ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
if(ret==LM_ERROR){
free(ptr);
return LM_ERROR;
}
/* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
* Due to orthogonality, Z^T Z = I and the last equation
* becomes pp=Z^T*(p-c). Also, save the starting p in p0
*/
for(i=0; i<m; ++i){
p0[i]=p[i];
p[i]-=data.c[i];
}
/* Z^T*(p-c) */
for(i=0; i<mm; ++i){
for(j=0, tmp=0.0; j<m; ++j)
tmp+=Z[j*mm+i]*p[j];
pp[i]=tmp;
}
/* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
if(FABS(tmp-p0[i])>LM_CNST(1E-03))
fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DER) "()! [%.10g reset to %.10g]\n",
i, p0[i], tmp);
}
if(!info) info=locinfo; /* make sure that LEVMAR_DER() is called with non-null info */
/* note that covariance computation is not requested from LEVMAR_DER() */
ret=LEVMAR_DER(LMLEC_FUNC, LMLEC_JACF, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
/* p=c + Z*pp */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
p[i]=tmp;
}
/* compute the covariance from the Jacobian in data.jac */
if(covar){
LEVMAR_TRANS_MAT_MAT_MULT(data.jac, covar, n, m); /* covar = J^T J */
LEVMAR_COVAR(covar, covar, info[1], m, n);
}
free(ptr);
return ret;
}
/* Similar to the LEVMAR_LEC_DER() function above, except that the Jacobian is approximated
* with the aid of finite differences (forward or central, see the comment for the opts argument)
*/
int LEVMAR_LEC_DIF(
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 */
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 *A, /* I: constraints matrix, kxm */
LM_REAL *b, /* I: right hand constraints vector, kx1 */
int k, /* I: number of constraints (i.e. A's #rows) */
int itmax, /* I: maximum number of iterations */
LM_REAL opts[5], /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
* scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
* the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
* If \delta<0, the Jacobian is approximated with central differences which are more accurate
* (but slower!) compared to the forward differences employed by default.
*/
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_LEC_DIF_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.
* Set to NULL if not needed
*/
{
struct LMLEC_DATA data;
LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
int mm, ret;
register int i, j;
register LM_REAL tmp;
LM_REAL locinfo[LM_INFO_SZ];
mm=m-k;
if(n<mm){
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
return LM_ERROR;
}
ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));
if(!ptr){
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
return LM_ERROR;
}
data.p=p;
p0=ptr;
data.c=p0+m;
data.Z=Z=data.c+m;
data.jac=NULL;
pp=data.Z+m*mm;
data.ncnstr=k;
data.func=func;
data.jacf=NULL;
data.adata=adata;
ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
if(ret==LM_ERROR){
free(ptr);
return LM_ERROR;
}
/* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
* Due to orthogonality, Z^T Z = I and the last equation
* becomes pp=Z^T*(p-c). Also, save the starting p in p0
*/
for(i=0; i<m; ++i){
p0[i]=p[i];
p[i]-=data.c[i];
}
/* Z^T*(p-c) */
for(i=0; i<mm; ++i){
for(j=0, tmp=0.0; j<m; ++j)
tmp+=Z[j*mm+i]*p[j];
pp[i]=tmp;
}
/* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
if(FABS(tmp-p0[i])>LM_CNST(1E-03))
fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]\n",
i, p0[i], tmp);
}
if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */
/* note that covariance computation is not requested from LEVMAR_DIF() */
ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
/* p=c + Z*pp */
for(i=0; i<m; ++i){
Zimm=Z+i*mm;
for(j=0, tmp=data.c[i]; j<mm; ++j)
tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
p[i]=tmp;
}
/* compute the Jacobian with finite differences and use it to estimate the covariance */
if(covar){
LM_REAL *hx, *wrk, *jac;
hx=(LM_REAL *)malloc((2*n+n*m)*sizeof(LM_REAL));
if(!hx){
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
free(ptr);
return LM_ERROR;
}
wrk=hx+n;
jac=wrk+n;
(*func)(p, hx, m, n, adata); /* evaluate function at p */
LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the Jacobian at p */
LEVMAR_TRANS_MAT_MAT_MULT(jac, covar, n, m); /* covar = J^T J */
LEVMAR_COVAR(covar, covar, info[1], m, n);
free(hx);
}
free(ptr);
return ret;
}
/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LMLEC_DATA
#undef LMLEC_ELIM
#undef LMLEC_FUNC
#undef LMLEC_JACF
#undef LEVMAR_FDIF_FORW_JAC_APPROX
#undef LEVMAR_COVAR
#undef LEVMAR_TRANS_MAT_MAT_MULT
#undef LEVMAR_LEC_DER
#undef LEVMAR_LEC_DIF
#undef LEVMAR_DER
#undef LEVMAR_DIF
#undef GEQP3
#undef ORGQR
#undef TRTRI
Reference in New Issue View Git Blame Copy Permalink