void lsqdglinv(real_t **a, int m, real_t aid[], int ci[], real_t diag[])
{
real_t vecvec(int, int, int, real_t [], real_t []);
real_t tamvec(int, int, int, real_t **, real_t []);
int j,k,cik;
real_t w;
for (k=1; k<=m; k++) {
diag[k]=1.0/aid[k];
for (j=k+1; j<=m; j++) diag[j] = -tamvec(k,j-1,j,a,diag)/aid[j];
diag[k]=vecvec(k,m,0,diag,diag);
}
for (k=m; k>=1; k--) {
cik=ci[k];
if (cik != k) {
w=diag[k];
diag[k]=diag[cik];
diag[cik]=w;
}
}
}
void peide(int n, int m, int nobs, int *nbp, real_t par[],
real_t res[], int bp[], real_t **jtjinv,
real_t in[], real_t out[],
int (*deriv)(int,int,real_t [],real_t [],real_t,real_t []),
int (*jacdfdy)(int,int,real_t [],real_t [],real_t,real_t **),
int (*jacdfdp)(int,int,real_t [],real_t [],real_t,real_t **),
void (*callystart)(int,int,real_t [],real_t [],real_t[]),
void (*data)(int,real_t [],real_t [],int[]),
void (*monitor)(int,int,int,real_t [],real_t [],int,int))
{
int i,j,weight,ncol,nrow,away,max,nfe,nis,*cobs,
first,sec,clean,nbpold,maxfe,fe,it,err,emergency;
real_t eps1,res1,in3,in4,fac3,fac4,aux[4],*obs,*save,*tobs,
**yp,*ymax,*y,**fy,**fp,w,**aid,temp,
vv,ww,w2,mu,res2,fpar,fparpres,lambda,lambdamin,p,pw,
reltolres,abstolres,em[8],*val,*b,*bb,*parpres,**jaco;
static real_t save1[35]={1.0, 1.0, 9.0, 4.0, 0.0, 2.0/3.0, 1.0,
1.0/3.0, 36.0, 20.25, 1.0, 6.0/11.0, 1.0, 6.0/11.0,
1.0/11.0, 84.028, 53.778, 0.25, 0.48, 1.0, 0.7, 0.2,
0.02, 156.25, 108.51, 0.027778, 120.0/274.0, 1.0,
225.0/274.0, 85.0/274.0, 15.0/274.0, 1.0/274.0, 0.0,
187.69, 0.0047361};
nbpold=(*nbp);
cobs=allocate_integer_vector(1,nobs);
obs=allocate_real_vector(1,nobs);
save=allocate_real_vector(-38,6*n);
tobs=allocate_real_vector(0,nobs);
ymax=allocate_real_vector(1,n);
y=allocate_real_vector(1,6*n*(nbpold+m+1));
yp=allocate_real_matrix(1,nbpold+nobs,1,nbpold+m);
fy=allocate_real_matrix(1,n,1,n);
fp=allocate_real_matrix(1,n,1,m+nbpold);
aid=allocate_real_matrix(1,m+nbpold,1,m+nbpold);
for (i=0; i<=34; i++) save[-38+i]=save1[i];
(*data)(nobs,tobs,obs,cobs);
weight=1;
first=sec=0;
clean=(*nbp > 0);
aux[2]=FLT_EPSILON;
eps1=1.0e10;
out[1]=0.0;
bp[0]=max=0;
/* smooth integration without break-points */
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor)) goto Escape;
res1=sqrt(vecvec(1,nobs,0,res,res));
nfe=1;
if (in[5] == 1.0) {
out[1]=1.0;
goto Escape;
}
if (clean) {
first=1;
clean=0;
fac3=sqrt(sqrt(in[3]/res1));
fac4=sqrt(sqrt(in[4]/res1));
eps1=res1*fac4;
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor)) goto Escape;
first=0;
} else
nfe=0;
ncol=m+(*nbp);
nrow=nobs+(*nbp);
sec=1;
in3=in[3];
in4=in[4];
in[3]=res1;
weight=away=0;
out[4]=out[5]=w=0.0;
temp=sqrt(weight)+1.0;
weight=temp*temp;
while (weight != 16 && *nbp > 0) {
if (away == 0 && w != 0.0) {
/* if no break-points were omitted then one function
function evaluation is saved */
w=weight/w;
for (i=nobs+1; i<=nrow; i++) {
for (j=1; j<=ncol; j++) yp[i][j] *= w;
res[i] *= w;
}
sec=1;
nfe--;
}
in[3] *= fac3*weight;
in[4]=eps1;
(*monitor)(2,ncol,nrow,par,res,weight,nis);
/* marquardt's method */
val=allocate_real_vector(1,ncol);
b=allocate_real_vector(1,ncol);
bb=allocate_real_vector(1,ncol);
parpres=allocate_real_vector(1,ncol);
jaco=allocate_real_matrix(1,nrow,1,ncol);
vv=10.0;
w2=0.5;
mu=0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0]=em[2]=em[6]=in[0];
em[4]=10*ncol;
reltolres=in[3];
abstolres=in[4]*in[4];
maxfe=in[5];
err=0;
fe=it=1;
p=fpar=res2=0.0;
pw = -log(ww*in[0])/2.30;
if (!peidefunct(nrow,ncol,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,
weight,bp,save,ymax,y,yp,fy,fp,cobs,tobs,obs,
in,aux,clean,deriv,jacdfdy,jacdfdp,
callystart,monitor))
err=3;
else {
fpar=vecvec(1,nrow,0,res,res);
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
dupmat(1,nrow,1,ncol,jaco,yp);
i=qrisngvaldec(jaco,nrow,ncol,val,aid,em);
if (it == 1)
lambda=in[6]*vecvec(1,ncol,0,val,val);
else
if (p == 0.0) lambda *= w2;
for (i=1; i<=ncol; i++)
b[i]=val[i]*tamvec(1,nrow,i,jaco,res);
while (1) {
for (i=1; i<=ncol; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=ncol; i++)
parpres[i]=par[i]-matvec(1,ncol,i,aid,bb);
fe++;
if (fe >= maxfe)
err=1;
else
if (!peidefunct(nrow,ncol,parpres,res,
n,m,nobs,nbp,first,&sec,&max,&nis,
eps1,weight,bp,save,ymax,y,yp,fy,fp,
cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor))
err=2;
if (err != 0) {
emergency=1;
break;
}
fparpres=vecvec(1,nrow,0,res,res);
res2=fpar-fparpres;
if (res2 < mu*vecvec(1,ncol,0,b,bb)) {
p += 1.0;
lambda *= vv;
if (p == 1.0) {
lambdamin=ww*vecvec(1,ncol,0,val,val);
if (lambda < lambdamin) lambda=lambdamin;
}
if (p >= pw) {
err=4;
emergency=1;
break;
}
} else {
dupvec(1,ncol,0,par,parpres);
fpar=fparpres;
break;
}
}
if (emergency) break;
it++;
} while (fpar>abstolres &&
res2>reltolres*fpar+abstolres);
for (i=1; i<=ncol; i++)
mulcol(1,ncol,i,i,jaco,aid,1.0/(val[i]+in[0]));
for (i=1; i<=ncol; i++)
for (j=1; j<=i; j++)
aid[i][j]=aid[j][i]=mattam(1,ncol,i,j,jaco,jaco);
lambda=lambdamin=val[1];
for (i=2; i<=ncol; i++)
if (val[i] > lambda)
lambda=val[i];
else
if (val[i] < lambdamin) lambdamin=val[i];
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res2+fpar)-out[2];
}
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jaco,1,nrow,1);
if (out[1] > 0.0) goto Escape;
/* the relative starting value of lambda is adjusted
to the last value of lambda used */
away=out[4]-out[5]-1.0;
in[6] *= pow(5.0,away)*pow(2.0,away-out[5]);
nfe += out[4];
w=weight;
temp=sqrt(weight)+1.0;
eps1=temp*temp*in[4]*fac4;
away=0;
/* omit useless break-points */
for (j=1; j<=(*nbp); j++)
if (fabs(obs[bp[j]]+res[bp[j]]-par[j+m]) < eps1) {
(*nbp)--;
for (i=j; i<=(*nbp); i++) bp[i]=bp[i+1];
dupvec(j+m,(*nbp)+m,1,par,par);
j--;
away++;
bp[*nbp+1]=0;
}
ncol -= away;
nrow -= away;
temp=sqrt(weight)+1.0;
weight=temp*temp;
}
in[3]=in3;
in[4]=in4;
*nbp=0;
weight=1;
(*monitor)(2,m,nobs,par,res,weight,nis);
/* marquardt's method */
val=allocate_real_vector(1,m);
b=allocate_real_vector(1,m);
bb=allocate_real_vector(1,m);
parpres=allocate_real_vector(1,m);
jaco=allocate_real_matrix(1,nobs,1,m);
vv=10.0;
w2=0.5;
mu=0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0]=em[2]=em[6]=in[0];
em[4]=10*m;
reltolres=in[3];
abstolres=in[4]*in[4];
maxfe=in[5];
err=0;
fe=it=1;
p=fpar=res2=0.0;
pw = -log(ww*in[0])/2.30;
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,
deriv,jacdfdy,jacdfdp,callystart,monitor))
err=3;
else {
fpar=vecvec(1,nobs,0,res,res);
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
dupmat(1,nobs,1,m,jaco,yp);
i=qrisngvaldec(jaco,nobs,m,val,jtjinv,em);
if (it == 1)
lambda=in[6]*vecvec(1,m,0,val,val);
else
if (p == 0.0) lambda *= w2;
for (i=1; i<=m; i++)
b[i]=val[i]*tamvec(1,nobs,i,jaco,res);
while (1) {
for (i=1; i<=m; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=m; i++)
parpres[i]=par[i]-matvec(1,m,i,jtjinv,bb);
fe++;
if (fe >= maxfe)
err=1;
else
if (!peidefunct(nobs,m,parpres,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,
weight,bp,save,ymax,y,yp,fy,fp,cobs,tobs,
obs,in,aux,clean,deriv,jacdfdy,jacdfdp,
callystart,monitor))
err=2;
if (err != 0) {
emergency=1;
break;
}
fparpres=vecvec(1,nobs,0,res,res);
res2=fpar-fparpres;
if (res2 < mu*vecvec(1,m,0,b,bb)) {
p += 1.0;
lambda *= vv;
if (p == 1.0) {
lambdamin=ww*vecvec(1,m,0,val,val);
if (lambda < lambdamin) lambda=lambdamin;
}
if (p >= pw) {
err=4;
emergency=1;
break;
}
} else {
dupvec(1,m,0,par,parpres);
fpar=fparpres;
break;
}
}
if (emergency) break;
it++;
} while (fpar>abstolres && res2>reltolres*fpar+abstolres);
for (i=1; i<=m; i++)
mulcol(1,m,i,i,jaco,jtjinv,1.0/(val[i]+in[0]));
for (i=1; i<=m; i++)
for (j=1; j<=i; j++)
jtjinv[i][j]=jtjinv[j][i]=mattam(1,m,i,j,jaco,jaco);
lambda=lambdamin=val[1];
for (i=2; i<=m; i++)
if (val[i] > lambda)
lambda=val[i];
else
if (val[i] < lambdamin) lambdamin=val[i];
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res2+fpar)-out[2];
}
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jaco,1,nobs,1);
nfe += out[4];
Escape:
if (out[1] == 3.0)
out[1]=2.0;
else
if (out[1] == 4.0) out[1]=6.0;
if (save[-3] != 0.0) out[1]=save[-3];
out[3]=res1;
out[4]=nfe;
out[5]=max;
free_integer_vector(cobs,1);
free_real_vector(obs,1);
free_real_vector(save,-38);
free_real_vector(tobs,0);
free_real_vector(ymax,1);
free_real_vector(y,1);
free_real_matrix(yp,1,nbpold+nobs,1);
free_real_matrix(fy,1,n,1);
free_real_matrix(fp,1,n,1);
free_real_matrix(aid,1,m+nbpold,1);
}
/*-------------------------------------------------------------------------------
calculate the least squares solution of an overdetermined system of nonlinear equations
with Marquardt's method
-------------------------------------------------------------------------------*/
void Ti_Optimization::MarquardtforCylinderFitting(
int m,
int n,
double**g_pnt,
double* const par,
double*& g,
double**v,
int (*funct)(int m, int n, double* const par, double* g,double**g_pnt),
void (*jacobian)(int m, int n, double* const par, double*& g, double **jac,double**g_pnt),
double in[],
double out[]
)
{
int maxfe,fe,it,i,j,err,emergency;
double vv,ww,w,mu,res,fpar,fparpres,lambda,lambdamin,p,pw,reltolres,
abstolres,em[8],*val,*b,*bb,*parpres,**jac,temp;
val = allocate_real_vector(1,n);
b = allocate_real_vector(1,n);
bb = allocate_real_vector(1,n);
parpres = allocate_real_vector(1,n);
jac = allocate_real_matrix(1,m,1,n);
assert( (val != NULL) &&
(b != NULL) &&
(bb != NULL) &&
(parpres!= NULL)&&
(jac != NULL)
);
vv = 10.0;
w = 0.5;
mu = 0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0] = em[2] = em[6] = in[0];
em[4] = 10*n;
reltolres =in[3];
abstolres=in[4]*in[4];
maxfe=(int)in[5];
err=0;
fe=it=1;
p=fpar=res=0.0;
pw = -log(ww*in[0])/2.30;
if (!(*funct)(m,n,par,g,g_pnt))
{
err=3;
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jac,1,m,1);
return;
}
fpar=vecvec(1,m,0,g,g);// norm of residual vector
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
(*jacobian)(m,n,par,g,jac,g_pnt);
i = qrisngvaldec(jac,m,n,val,v,em);
if (it == 1)
lambda = in[6]*vecvec(1,n,0,val,val);
else
if (p == 0.0)
lambda *= w;
for (i=1; i<=n; i++)
b[i] = val[i]*tamvec(1,m,i,jac,g);
while (1)
{
for (i=1; i<=n; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=n; i++)
parpres[i]=par[i]-matvec(1,n,i,v,bb);
//normalization ,this section only used for cylinder fitting,
//when it is used in other situations,it should be removed
temp = sqrt(parpres[4]*parpres[4]+parpres[5]*parpres[5]+parpres[6]*parpres[6]);
parpres[4] /= temp;
parpres[5] /= temp;
parpres[6] /= temp;
//end normalization
fe++;
if (fe >= maxfe)
err=1;
else
if (!(*funct)(m,n,parpres,g,g_pnt))
err=2;
if (err != 0)
{
emergency = 1;
break;
}
fparpres=vecvec(1,m,0,g,g);
res=fpar-fparpres;
if (res < mu*vecvec(1,n,0,b,bb))
{
p += 1.0;
lambda *= vv;
if (p == 1.0)
{
lambdamin=ww*vecvec(1,n,0,val,val);
if (lambda < lambdamin)
lambda=lambdamin;
}
if (p >= pw)
{
err=4;
emergency=1;
break;
}
} // end if
else
{
dupvec(1,n,0,par,parpres);
fpar=fparpres;
break;
} // end else
} // end while
if (emergency)
break;
it++;
}
while (
(fpar > abstolres) &&
(res > reltolres*fpar+abstolres)
);
for (i=1; i<=n; i++)
mulcol(1,n,i,i,jac,v,1.0/(val[i]+in[0]));
for (i=1; i<=n; i++)
{
for (j=1; j<=i; j++)
v[i][j]=v[j][i]=mattam(1,n,i,j,jac,jac);
lambda=lambdamin=val[1];
}
for (i=2; i<=n; i++)
{
if (val[i] > lambda)
lambda=val[i];
else
{
if (val[i] < lambdamin)
lambdamin=val[i];
}
}
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res+fpar)-out[2];
out[4]=fe;
out[5]=it-1;
out[1]=err;
if(val != NULL)
{
free_real_vector(val,1);
val = NULL;
}
if (b != NULL)
{
free_real_vector(b,1);
b = NULL;
}
if(bb!=NULL)
{
free_real_vector(bb,1);
bb = NULL;
}
if(parpres != NULL)
{
free_real_vector(parpres,1);
parpres = NULL;
}
if (jac != NULL)
{
free_real_matrix(jac,1,m,1);
jac = NULL;
}
}