snaphu_cost.c

来自「phase unwrapping algorithm for SAR inter」· C语言 代码 · 共 2,189 行 · 第 1/5 页

 * Numerically solve for the transition point of the linearized scattering 
 * model.
 */
double SolveDZRCrit(double sinnomincangle, double cosnomincangle, 
		    paramT *params, double threshold){

  double residual, thetai, kds, n, dr, dzr, dx;
  double costhetai, cos2thetai, step;
  double dzrcritfactor, diffuse, specular;
  long i;

  /* get parameters */
  kds=params->kds;
  n=params->specularexp;
  dr=params->dr;  
  dzrcritfactor=params->dzrcritfactor;

  /* solve for critical incidence angle */
  thetai=PI/4;
  step=PI/4-1e-6;
  i=0;
  while(TRUE){
    if((cos2thetai=cos(2*thetai))<0){
      cos2thetai=0;
    }
    diffuse=dzrcritfactor*kds*cos(thetai);
    specular=pow(cos2thetai,n);
    if(fabs(residual=diffuse-specular)<threshold*diffuse){
      break;
    }
    if(residual<0){
      thetai+=step;
    }else{
      thetai-=step;
    }
    step/=2.0;
    if(++i>MAXITERATION){
      fprintf(sp0,"Couldn't find critical incidence angle ");
      fprintf(sp0,"(check scattering parameters)\nAbort\n");
      exit(ABNORMAL_EXIT);
    }
  }

  /* solve for critical height change */
  costhetai=cos(thetai);
  dzr=params->initdzr;
  step=dzr+dr*cosnomincangle-1e-2;
  i=0;
  while(TRUE){
    dx=(dr+dzr*cosnomincangle)/sinnomincangle;
    if(fabs(residual=costhetai-(dzr*sinnomincangle+dx*cosnomincangle)
	    /sqrt(dzr*dzr+dx*dx))
       <threshold*costhetai){
      return(dzr);
    }
    if(residual<0){
      dzr-=step;
    }else{
      dzr+=step;
    }
    step/=2.0;
    if(++i>MAXITERATION){
      fprintf(sp0,"Couldn't find critical slope ");
      fprintf(sp0,"(check geometry parameters)\nAbort\n");
      exit(ABNORMAL_EXIT);
    }
  }
}


/* function: SolveEIModelParams()
 * ------------------------------
 * Calculates parameters for linearized model of EI vs. range slope
 * relationship.
 */
void SolveEIModelParams(double *slope1ptr, double *slope2ptr, 
			double *const1ptr, double *const2ptr, 
			double dzrcrit, double dzr0, double sinnomincangle, 
			double cosnomincangle, paramT *params){

  double slope1, slope2, const1, const2, sloperatio;
  double dzr3, ei3;
  
  /* set up */
  sloperatio=params->kds*params->sloperatiofactor;

  /* find normalized intensity at 15(dzrcrit-dzr0)+dzr0 */
  dzr3=15.0*(dzrcrit-dzr0)+dzr0;
  ei3=EIofDZR(dzr3,sinnomincangle,cosnomincangle,params)
    /EIofDZR(0,sinnomincangle,cosnomincangle,params);

  /* calculate parameters */
  const1=dzr0;
  slope2=(sloperatio*(dzrcrit-const1)-dzrcrit+dzr3)/ei3;
  slope1=slope2/sloperatio;
  const2=dzr3-slope2*ei3;

  /* set return values */
  *slope1ptr=slope1;
  *slope2ptr=slope2;
  *const1ptr=const1;
  *const2ptr=const2;

}


/* function: EIofDZR()
 * -------------------
 * Calculates expected value of intensity with arbitrary units for given
 * parameters.  Assumes azimuth slope is zero.
 */
double EIofDZR(double dzr, double sinnomincangle, double cosnomincangle,
	       paramT *params){

  double dr, da, dx, kds, n, dzr0, projarea;
  double costhetai, cos2thetai, sigma0;

  dr=params->dr;
  da=params->da;
  dx=dr/sinnomincangle+dzr*cosnomincangle/sinnomincangle;
  kds=params->kds;
  n=params->specularexp;
  dzr0=-dr*cosnomincangle;
  projarea=da*fabs((dzr-dzr0)/sinnomincangle);
  costhetai=projarea/sqrt(dzr*dzr*da*da + da*da*dx*dx);
  if(costhetai>SQRTHALF){
    cos2thetai=2*costhetai*costhetai-1;
    sigma0=kds*costhetai+pow(cos2thetai,n);
  }else{
    sigma0=kds*costhetai;
  }
  return(sigma0*projarea);

}


/* function: BuildDZRhoMaxLookupTable()
 * ------------------------------------
 * Builds a 2-D lookup table of dzrhomax values vs nominal incidence angle
 * (rad) and correlation.
 */
float **BuildDZRhoMaxLookupTable(double nominc0, double dnominc, 
				 long nominctablesize, double rhomin, 
				 double drho, long nrho, paramT *params){

  long krho, knominc;
  double nominc, rho;
  float **dzrhomaxtable;

  dzrhomaxtable=(float **)Get2DMem(nominctablesize,nrho,
				   sizeof(float *),sizeof(float));
  nominc=nominc0;
  for(knominc=0;knominc<nominctablesize;knominc++){
    rho=rhomin;
    for(krho=0;krho<nrho;krho++){
      dzrhomaxtable[knominc][krho]=(float )CalcDZRhoMax(rho,nominc,params,
							params->threshold);
      rho+=drho;
    }
    nominc+=dnominc;
  }
  return(dzrhomaxtable);

}


/* function: CalcDZRhoMax()
 * ------------------------
 * Calculates the maximum slope (in range) for the given unbiased correlation
 * using spatial decorrelation as an upper limit (Zebker & Villasenor,
 * 1992).
 */
double CalcDZRhoMax(double rho, double nominc, paramT *params, 
		    double threshold){

  long i;
  double dx, dr, dz, dzstep, rhos, sintheta, costheta, numerator;
  double a, re, bperp, slantrange, lookangle;
  double costhetairsq, rhosfactor, residual;


  /* set up */
  i=0;
  dr=params->dr;
  costheta=cos(nominc);
  sintheta=sin(nominc);
  dzstep=params->initdzstep;
  a=params->orbitradius;
  re=params->earthradius;
  lookangle=asin(re/a*sintheta);
  bperp=params->baseline*cos(lookangle-params->baselineangle);
  slantrange=sqrt(a*a+re*re-2*a*re*cos(nominc-lookangle));
  rhosfactor=2.0*fabs(bperp)*(params->rangeres)/((params->lambda)*slantrange);

  /* take care of the extremes */
  if(rho>=1.0){
    return(-dr*costheta);
  }else if(rho<=0){
    return(LARGEFLOAT);
  }

  /* start with slope for unity correlation, step slope upwards */
  dz=-dr*costheta;
  rhos=1.0;
  while(rhos>rho){
    dz+=dzstep;
    dx=(dr+dz*costheta)/sintheta;
    numerator=dz*sintheta+dx*costheta;
    costhetairsq=numerator*numerator/(dz*dz+dx*dx);
    rhos=1-rhosfactor*sqrt(costhetairsq/(1-costhetairsq));
    if(rhos<0){
      rhos=0;
    }
    if(dz>BIGGESTDZRHOMAX){
      return(BIGGESTDZRHOMAX);
    }
  }

  /* now iteratively decrease step size and narrow in on correct slope */
  while(fabs(residual=rhos-rho)>threshold*rho){
    dzstep/=2.0;
    if(residual<0){
      dz-=dzstep;
    }else{
      dz+=dzstep;
    }
    dx=(dr+dz*costheta)/sintheta;
    numerator=dz*sintheta+dx*costheta;
    costhetairsq=numerator*numerator/(dz*dz+dx*dx);
    rhos=1-rhosfactor*sqrt(costhetairsq/(1-costhetairsq));
    if(rhos<0){
      rhos=0;
    }
    if(++i>MAXITERATION){
      fprintf(sp0,"Couldn't find slope for correlation of %f\n",rho);
      fprintf(sp0,"(check geometry and spatial decorrelation parameters)\n");
      fprintf(sp0,"Abort\n");
      exit(ABNORMAL_EXIT);
    }
  }

  return(dz);
}


/* function: CalcCostTopo()
 * ------------------------
 * Calculates topography arc distance given an array of cost data structures.
 */
void CalcCostTopo(void **costs, long flow, long arcrow, long arccol, 
		  long nflow, long nrow, paramT *params, 
		  long *poscostptr, long *negcostptr){

  long idz1, idz2pos, idz2neg, cost1, nflowsq, poscost, negcost;
  long nshortcycle, layfalloffconst;
  long offset, sigsq, laycost, dzmax;
  costT *cost;


  /* get arc info */
  cost=&((costT **)(costs))[arcrow][arccol];
  dzmax=cost->dzmax;
  offset=cost->offset;
  sigsq=cost->sigsq;
  laycost=cost->laycost;
  nshortcycle=params->nshortcycle;
  layfalloffconst=params->layfalloffconst;
  if(arcrow<nrow-1){

    /* row cost: dz symmetric with respect to origin */
    idz1=labs(flow*nshortcycle+offset);
    idz2pos=labs((flow+nflow)*nshortcycle+offset);
    idz2neg=labs((flow-nflow)*nshortcycle+offset);

  }else{

    /* column cost: non-symmetric dz */
    /* dzmax will only be < 0 if we have a column arc */
    if(dzmax<0){
      dzmax*=-1;
      idz1=-(flow*nshortcycle+offset);
      idz2pos=-((flow+nflow)*nshortcycle+offset);
      idz2neg=-((flow-nflow)*nshortcycle+offset);
    }else{
      idz1=flow*nshortcycle+offset;
      idz2pos=(flow+nflow)*nshortcycle+offset;
      idz2neg=(flow-nflow)*nshortcycle+offset;
    }

  }

  /* calculate cost1 */
  if(idz1>dzmax){
    idz1-=dzmax;
    cost1=(idz1*idz1)/(layfalloffconst*sigsq)+laycost; 
  }else{
    cost1=(idz1*idz1)/sigsq;
    if(laycost!=NOCOSTSHELF && idz1>0 && cost1>laycost){
      cost1=laycost;
    }
  }

  /* calculate positive cost increment */
  if(idz2pos>dzmax){
    idz2pos-=dzmax;
    poscost=(idz2pos*idz2pos)/(layfalloffconst*sigsq)
      +laycost-cost1;
  }else{
    poscost=(idz2pos*idz2pos)/sigsq;
    if(laycost!=NOCOSTSHELF && idz2pos>0 && poscost>laycost){
      poscost=laycost-cost1;
    }else{
      poscost-=cost1;
    }
  }

  /* calculate negative cost increment */
  if(idz2neg>dzmax){
    idz2neg-=dzmax;
    negcost=(idz2neg*idz2neg)/(layfalloffconst*sigsq)
      +laycost-cost1;
  }else{
    negcost=(idz2neg*idz2neg)/sigsq;
    if(laycost!=NOCOSTSHELF && idz2neg>0 && negcost>laycost){
      negcost=laycost-cost1;
    }else{
      negcost-=cost1;
    }
  }

  /* scale costs for this nflow */
  nflowsq=nflow*nflow;
  if(poscost>0){
    *poscostptr=(long )ceil((float )poscost/nflowsq);
  }else{
    *poscostptr=(long )floor((float )poscost/nflowsq);
  }
  if(negcost>0){
    *negcostptr=(long )ceil((float )negcost/nflowsq);
  }else{
    *negcostptr=(long )floor((float )negcost/nflowsq);
  }

}


/* function: CalcCostDefo()
 * ------------------------
 * Calculates deformation arc distance given an array of cost data structures.
 */
void CalcCostDefo(void **costs, long flow, long arcrow, long arccol, 
		  long nflow, long nrow, paramT *params, 
		  long *poscostptr, long *negcostptr){

  long idz1, idz2pos, idz2neg, cost1, nflowsq, poscost, negcost;
  long nshortcycle, layfalloffconst;
  costT *cost;


  /* get arc info */
  cost=&((costT **)(costs))[arcrow][arccol];
  nshortcycle=params->nshortcycle;
  layfalloffconst=params->layfalloffconst;
  idz1=labs(flow*nshortcycle+cost->offset);
  idz2pos=labs((flow+nflow)*nshortcycle+cost->offset);
  idz2neg=labs((flow-nflow)*nshortcycle+cost->offset);

  /* calculate cost1 */
  if(idz1>cost->dzmax){
    idz1-=cost->dzmax;
    cost1=(idz1*idz1)/(layfalloffconst*(cost->sigsq))+cost->laycost; 
  }else{
    cost1=(idz1*idz1)/cost->sigsq;
    if(cost->laycost!=NOCOSTSHELF && cost1>cost->laycost){
      cost1=cost->laycost;
    }
  }

  /* calculate positive cost increment */
  if(idz2pos>cost->dzmax){
    idz2pos-=cost->dzmax;
    poscost=(idz2pos*idz2pos)/(layfalloffconst*(cost->sigsq))
      +cost->laycost-cost1;
  }else{
    poscost=(idz2pos*idz2pos)/cost->sigsq;
    if(cost->laycost!=NOCOSTSHELF && poscost>cost->laycost){
      poscost=cost->laycost-cost1;
    }else{
      poscost-=cost1;
    }
  }

  /* calculate negative cost increment */
  if(idz2neg>cost->dzmax){
    idz2neg-=cost->dzmax;
    negcost=(idz2neg*idz2neg)/(layfalloffconst*(cost->sigsq))
      +cost->laycost-cost1;
  }else{
    negcost=(idz2neg*idz2neg)/cost->sigsq;
    if(cost->laycost!=NOCOSTSHELF && negcost>cost->laycost){
      negcost=cost->laycost-cost1;
    }else{
      negcost-=cost1;
    }
  }

  /* scale costs for this nflow */
  nflowsq=nflow*nflow;
  if(poscost>0){
    *poscostptr=(long )ceil((float )poscost/nflowsq);
  }else{
    *poscostptr=(long )floor((float )poscost/nflowsq);
  }
  if(negcost>0){
    *negcostptr=(long )ceil((float )negcost/nflowsq);
  }else{
    *negcostptr=(long )floor((float )negcost/nflowsq);
  }

}


/* function: CalcCostSmooth()
 * --------------------------
 * Calculates smooth-solution arc distance given an array of smoothcost
 *  data structures.
 */
void CalcCostSmooth(void **costs, long flow, long arcrow, long arccol, 
		    long nflow, long nrow, paramT *params, 
		    long *poscostptr, long *negcostptr){

  long idz1, idz2pos, idz2neg, cost1, nflowsq, poscost, negcost;
  long nshortcycle;
  smoothcostT *cost;


  /* get arc info */
  cost=&((smoothcostT **)(costs))[arcrow][arccol];