示例#1
0
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;
		}
	}
}
示例#2
0
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);
}
示例#3
0
/*-------------------------------------------------------------------------------
 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;
	}
}