mi_hpv_2d.cpp

一种新的方法

  T=2.0*(FP-2.0*(*FRET)+FPTT)*Sqr(FP-(*FRET)-DEL)-DEL*Sqr(FP-FPTT);
  if (T >= 0.0) goto e1;       //Other reason not to use new direction
  LINMIN(R, F, nX, nY,P,XIT,N,FRET);        //Move to the minimum of the new direction
  for (J=1; J<=N; J++)         //and save the new direction.
    XI[J][IBIG]=XIT[J];
  goto e1;
} //Powell()

void   LINMIN(unsigned char *R, unsigned char *F, long nX, long nY, double *P, double *XI, int N, double *FRET) {
/*---------------------------------------------------------
  Given an N dimensional point P and a N dimensional direc-
  tion XI, moves and resets P to where the function FUNC(P)
  takes on a minimum along the direction XI from P, and
  replaces XI by the actual vector displacement that P was
  moved. Also returns as FRET the value of FUNC at the
  returned location P. This is actually all accomplished by
  calling the routines MNBRAK and BRENT.
---------------------------------------------------------*/
  double AX,BX,FA,FB,FX,TOL,XMIN,XX;
  int J;
  TOL=1e-4;
  NCOM=N;
  for (J=1; J<=N; J++) {
    PCOM[J]=P[J];
    XICOM[J]=XI[J];
  }
  AX=0.0;
  XX=1.0;
  BX=2.0;
  MNBRAK(R,F,nX,nY,&AX,&XX,&BX,&FA,&FX,&FB);
  *FRET=BRENT(R,F,nX,nY,&AX,&XX,&BX,TOL,&XMIN);
  for (J=1; J<=N; J++) {
    XI[J]=XMIN*XI[J];
    P[J] += XI[J];
  }
}


double F1DIM(unsigned char *R, unsigned char *F, long nX, long nY, double X) {
  double XT[NMAX]; int J;
  for (J=1; J<=NCOM; J++)
    XT[J]=PCOM[J] + X*XICOM[J];
  return FUNC(R,F,nX,nY,XT);
}

void   MNBRAK(unsigned char *Ref, unsigned char *F, long nX, long nY, double *AX,double *BX,double *CX,
			  double *FA, double *FB, double *FC)  {
/*---------------------------------------------------------------------
 Given a Function F1DIM(X), and given distinct initial points AX and
 BX, this routine searches in the downhill direction (defined by the
 Function as evaluated at the initial points) and returns new points
 AX, BX, CX which bracket a minimum of the Function. Also returned
 are the Function values at the three points, FA, FB and FC.
---------------------------------------------------------------------*/
  //Label:  e1
  const double GOLD=1.618034, GLIMIT=100.0, TINY=1e-20;
/*The first parameter is the default ratio by which successive intervals
  are magnified; the second is the maximum magnification allowed for
  a parabolic-fit step. */

  double DUM,FU,Q,R,U,ULIM;
  *FA=F1DIM(Ref,F,nX,nY,*AX);
  *FB=F1DIM(Ref,F,nX,nY,*BX);
  if (*FB > *FA) {
    DUM=*AX;
    *AX=*BX;
    *BX=DUM;
    DUM=*FB;
    *FB=*FA;
    *FA=DUM;
  }
  *CX=*BX+GOLD*(*BX-*AX);
  *FC=F1DIM(Ref,F,nX,nY,*CX);
e1:if (*FB >= *FC) {
    R=(BX-AX)*(FB-FC);
    Q=(BX-CX)*(FB-FA);
    U=*BX-((*BX-*CX)*Q-(*BX-*AX)*R)/(2.0*Sign(MAX(fabs(Q-R),TINY),Q-R));
    ULIM=*BX+GLIMIT*(*CX-*BX);
    if ((*BX-U)*(U-*CX) > 0.0) {
      FU=F1DIM(Ref,F,nX,nY,U);
      if (FU < *FC) {
        *AX=*BX;
        *FA=*FB;
        *BX=U;
        *FB=FU;
        goto e1;
      }
      else if (FU > *FB) {
        *CX=U;
        *FC=FU;
        goto e1;
      }
      U=*CX+GOLD*(*CX-*BX);
      FU=F1DIM(Ref,F,nX,nY,U);
    }
    else if ((*CX-U)*(U-ULIM) > 0.0) {
      FU=F1DIM(Ref,F,nX,nY,U);
      if (FU < *FC) {
        *BX=*CX;
        *CX=U;
        U=*CX+GOLD*(*CX-*BX);
        *FB=*FC;
        *FC=FU;
        FU=F1DIM(Ref,F,nX,nY,U);
      }
    }
    else if ((U-ULIM)*(ULIM-*CX) >= 0.0) {
      U=ULIM;
      FU=F1DIM(Ref,F,nX,nY,U);
    }
    else {
      U=*CX+GOLD*(*CX-*BX);
      FU=F1DIM(Ref,F,nX,nY,U);
    }
    *AX=*BX;
    *BX=*CX;
    *CX=U;
    *FA=*FB;
    *FB=*FC;
    *FC=FU;
    goto e1;
  }  
}


double BRENT(unsigned char *Ref, unsigned char *F, long nX, long nY, double *AX,double *BX,double *CX, double TOL, double *XMIN) {
/*------------------------------------------------------------------
 Given a function F1DIM, and a bracketing triplet of abscissas AX,
 BX,CX (such that BX is between AX and CX, and F(BX) is less than
 both F(AX) and F(CX)), this routine isolates the minimum to a
 fractional precision of about TOL using Brent's method. The abscissa
 of the minimum is returned in XMIN, and the minimum function value 
 is returned as BRENT, the returned function value.
-------------------------------------------------------------------*/
//Labels: e1,e2,e3
  const double CGOLD=0.3819660, ZEPS=1e-10;
/*Maximum allowed number of iterations; golden ratio; and a small
  number which protects against trying to achieve fractional accuracy
  for a minimum that happens to be exactly zero */
  double A,B,D,E,ETEMP,FX,FU,FV,FW,P,Q,R,TOL1,TOL2,U,V,W,X,XM;
  int ITER;
  A=MIN(*AX,*CX);
  B=MAX(*AX,*CX);
  V=*BX;
  W=V;
  X=V;
  E=0.0;
  FX=F1DIM(Ref,F,nX,nY,X);
  FV=FX;
  FW=FX;
  for (ITER=1; ITER<=ITMAX; ITER++) {                    //main loop
    XM=0.5*(A+B);
    TOL1=TOL*fabs(X)+ZEPS;
    TOL2=2.0*TOL1;
    if (fabs(X-XM) <= (TOL2-0.5*(B-A))) goto e3;     //Test for done here
    if (fabs(E) > TOL1) {               //Construct a trial parabolic fit
      R=(X-W)*(FX-FV);
      Q=(X-V)*(FX-FW);
      P=(X-V)*Q-(X-W)*R;
      Q=0.2*(Q-R);
      if (Q > 0.0) P=-P;
      Q=fabs(Q);
      ETEMP=E;
      E=D;
      if (fabs(P) >= fabs(0.5*Q*ETEMP) || P <= Q*(A-X) || P >= Q*(B-X)) goto e1;
//  The above conditions determine the acceptability of the
//  parabolic fit. Here it is o.k.
      D=P/Q;
      U=X+D;
      if (U-A < TOL2 || B-U < TOL2)  D=Sign(TOL1,XM-X);
      goto e2;
    }
e1: if (X >= XM)
      E=A-X;
    else
      E=B-X;
    D=CGOLD*E;
e2: if (fabs(D) >= TOL1)
      U=X+D;
    else
      U=X+Sign(TOL1,D);
    FU=F1DIM(Ref,F,nX,nY,U);   //This the one function evaluation per iteration
    if (FU <= FX) {
      if (U >= X)
        A=X;
      else
        B=X;
      V=W;
      FV=FW;
      W=X;
      FW=FX;
      X=U;
      FX=FU;
    }
    else {
      if (U < X)
        A=U;
      else
        B=U;
      if (FU <= FW || W == X) {
        V=W;
        FV=FW;
        W=U;
        FW=FU;
      }
      else if (FU <= FV || V == X || V == W) {
        V=U;
        FV=FU;
      }
    }
  }
  printf("\n Brent exceed maximum iterations.\n\n");
e3:*XMIN=X;   //exit section
  return FX;
}








void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs != 2) {
		mexErrMsgTxt("Two input arguments required.");
	} else if (nlhs > 3) {
		mexErrMsgTxt("Only three output arguments allowed.");
	}
	if (mxIsUint8(prhs[0])==0){
		mexErrMsgTxt("First input image must be uint8.");
	}
	if (mxIsUint8(prhs[1])==0){
		mexErrMsgTxt("Second input image must be uint8.");
	}
	mexPrintf("Co-registering images, please wait...\n");
	long numX,numY;
	unsigned char *pRef, *pFloat;
	double *pTranslate, *pTranslateOut, *pq, *pq_0;

	N=3;
	P[1]=0.0; P[2]=0.0; P[3]=0.0;
	XI[1][1]=1.0; XI[1][2]=0.0; XI[1][3]=0.0;
	XI[2][1]=0.0; XI[2][2]=1.0; XI[2][3]=0.0;
	XI[3][1]=0.0; XI[3][2]=0.0; XI[3][3]=1.0;

	FTOL=1e-8;

	numX = mxGetN(prhs[0]);
	numY = mxGetM(prhs[0]);
	if ((numY != mxGetM(prhs[1])) || (numX != mxGetN(prhs[1]))){
		mexErrMsgTxt("Input images must be the same size.");
	}
//	mexPrintf("y=%d  x=%d\n",numY,numX);
	pRef = (unsigned char *)mxGetPr(prhs[0]);
	pFloat = (unsigned char *)mxGetPr(prhs[1]);
	pTranslate = (double *)mxGetPr(prhs[2]);
	plhs[0]=mxCreateDoubleMatrix(3,1,mxREAL);
	pTranslateOut = (double *)mxGetPr(plhs[0]);
	POWELL(pRef,pFloat,numX,numY,P,XI,N,FTOL,&ITER,&FRET);
	pTranslateOut[0]=P[1];
	pTranslateOut[1]=P[2];
	pTranslateOut[2]=P[3];
//	P[1] = 5.13;
//	P[2] = -4.3;
//	P[3] = M_PI_2/3.0;
	mexPrintf("Co-registration complete: dX=%3.1lf  dY=%3.1lf  dTheta=%1.5lf\n",P[1],P[2],P[3]);
	if (nlhs == 2){
		plhs[1]=mxCreateDoubleMatrix(8,1,mxREAL);
		pq = (double *)mxGetPr(plhs[1]);
		corners(P[1],P[2],P[3], numX, numY, pq);
	}
	if (nlhs == 3){
		plhs[1]=mxCreateDoubleMatrix(8,1,mxREAL);
		pq = (double *)mxGetPr(plhs[1]);
		corners(P[1],P[2],P[3], numX, numY, pq);
		plhs[2]=mxCreateDoubleMatrix(8,1,mxREAL);
		pq_0 = (double *)mxGetPr(plhs[2]);
		pq_0[0] = 0.0;
		pq_0[1] = 0.0;
		pq_0[2] = (double)numY-1.0;
		pq_0[3] = 0.0;
		pq_0[4] = (double)numY-1.0;
		pq_0[5] = (double)numX-1.0;
		pq_0[6] = 0.0;
		pq_0[7] = (double)numX-1.0;

	}
}