snaphu_cost.c

phase unwrapping algorithm for SAR interferometry

	}else{
	  dzei=(slope1*ei[row][col]+const1)*dzeifactor;
	}
	if(noshadow && dzei<dzeimin){
	  dzei=dzeimin;
	}

	/* calculate dz expected from EI if layover exists */
	dzlay=0;
	if(ei[row][col]>layminei){
	  for(iei=0;iei<laywidth;iei++){
	    if(ei[row][col+iei]>eicrit){
	      dzlay+=slope2*ei[row][col+iei]+const2;
	    }else{
	      dzlay+=slope1*ei[row][col+iei]+const1;
	    }
	    if(col+iei>ncol-2){
	      break;
	    }
	  }
	}
	if(dzlay){
	  dzlay=(dzlay+iei*(-2.0*dzr0))*dzlayfactor;
	}
	  
	/* set maximum dz based on unbiased correlation and layover max */ 
	if(rho>0){
	  dzrhomax=LinInterp2D(dzrhomaxtable,nomincind,(rho-rhomin)/drho,
			       nominctablesize,nrho);
	  if(dzrhomax<dzlay){  
	    dzlay=dzrhomax;
	  }
	}

	/* set cost parameters in terms of flow, represented as shorts */
	nolayover=TRUE;
	if(dzlay){
	  if(rho>0){
	    colcost[row][col].offset=nshortcycle*
	      (dpsi[row][col]-0.5*(avgdpsi[row][col]+dphilaypeak));
	  }else{
	    colcost[row][col].offset=nshortcycle*
	      (dpsi[row][col]-0.25*avgdpsi[row][col]-0.75*dphilaypeak);
	  }
	  colcost[row][col].sigsq=(sigsqrho+sigsqei+sigsqlay)*ztoshortsq
	    /(costscale*colweight[row][col]);
	  if(colcost[row][col].sigsq<sigsqshortmin){
	    colcost[row][col].sigsq=sigsqshortmin;
	  }
	  colcost[row][col].dzmax=dzlay*ztoshort;
	  colcost[row][col].laycost=colweight[row][col]*glay;
	  if(labs(colcost[row][col].dzmax)
	     >floor(sqrt(colcost[row][col].laycost*colcost[row][col].sigsq))){
	    nolayover=FALSE;
	  }
	}
	if(nolayover){
	  colcost[row][col].sigsq=(sigsqrho+sigsqei)*ztoshortsq
	    /(costscale*colweight[row][col]);
	  if(colcost[row][col].sigsq<sigsqshortmin){
	    colcost[row][col].sigsq=sigsqshortmin;
	  }
	  if(rho>0){
	    colcost[row][col].offset=ztoshort*
	      (ambiguityheight*(dpsi[row][col]-0.5*avgdpsi[row][col])
	       -0.5*dzeiweight*dzei);
	  }else{
	    colcost[row][col].offset=ztoshort*
	      (ambiguityheight*(dpsi[row][col]-0.25*avgdpsi[row][col])
	       -0.75*dzeiweight*dzei);
	  }
	  colcost[row][col].laycost=NOCOSTSHELF;
	  colcost[row][col].dzmax=LARGESHORT;
	}

	/* shift PDF to account for flattening by coarse unwrapped estimate */
	if(unwrappedest!=NULL){
	  colcost[row][col].offset+=(nshortcycle/TWOPI*
				     (unwrappedest[row][col+1]
				      -unwrappedest[row][col]));
	}

      }
    }
  } /* end of range gradient cost calculation */

  /* reset layover constant for row (azimuth) costs */
  glay+=(-costscale*log(azdzfactor)); 

  /* build array of mean wrapped phase differences in azimuth */
  /* biased, but not much, so don't bother with complex averaging */
  fprintf(sp2,"Building azimuth cost arrays\n");
  CalcWrappedAzDiffs(dpsi,avgdpsi,wrappedphase,kperpdpsi,kpardpsi,
		     nrow,ncol);
  
  /* build rowcost array */
  /* for the rowcost array, there is symmetry between positive and */
  /*   negative flows, so we average ei[][] and corr[][] values in azimuth */
  /* loop over range */
  for(col=0;col<ncol;col++){

    /* compute range dependent parameters */
    slantrange=nearrange+col*dr;
    cosnomincangle=(a*a-slantrange*slantrange-re*re)/(2*slantrange*re);
    nomincangle=acos(cosnomincangle);
    sinnomincangle=sin(nomincangle);
    lookangle=asin(re/a*sinnomincangle);
    dzr0=-dr*cosnomincangle;
    bperp=baseline*cos(lookangle-baselineangle);
    ambiguityheight=-lambda*slantrange*sinnomincangle/(2*bperp);
    sigsqrhoconst=2.0*ambiguityheight*ambiguityheight/12.0;  
    ztoshort=nshortcycle/ambiguityheight;
    ztoshortsq=ztoshort*ztoshort;
    sigsqlay=ambiguityheight*ambiguityheight*params->sigsqlayfactor;

    /* interpolate scattering model parameters */
    nomincind=(nomincangle-nominc0)/dnominc;
    dzrcrit=LinInterp1D(dzrcrittable,nomincind,nominctablesize);
    SolveEIModelParams(&slope1,&slope2,&const1,&const2,dzrcrit,dzr0,
		       sinnomincangle,cosnomincangle,params);
    eicrit=(dzrcrit-const1)/slope1;
    dphilaypeak=params->dzlaypeak/ambiguityheight;

    /* loop over azimuth */
    for(row=0;row<nrow-1;row++){
	
      /* see if we have a masked pixel */
      if(mag[row][col]==0 || mag[row+1][col]==0){
	  
	/* masked pixel */
	rowcost[row][col].laycost=0;
	rowcost[row][col].offset=LARGESHORT/2;
	rowcost[row][col].dzmax=LARGESHORT;
	rowcost[row][col].sigsq=LARGESHORT;

      }else{

	/* topography-mode costs */

	/* variance due to decorrelation */
	/* get correlation and clip small values because of estimator bias */
	rho=(corr[row][col]+corr[row+1][col])/2.0;
	if(rho<rhomin){
	  rho=0;
	}
	sigsqrho=sigsqrhoconst*pow(1-rho,rhopow);

	/* if no layover, the expected dz for azimuth will always be 0 */
	dzei=0;
	
	/* calculate dz expected from EI if layover exists */
	dzlay=0;
	avgei=(ei[row][col]+ei[row+1][col])/2.0;
	if(avgei>layminei){
	  for(iei=0;iei<laywidth;iei++){
	    avgei=(ei[row][col+iei]+ei[row+1][col+iei])/2.0;
	    if(avgei>eicrit){
	      dzlay+=slope2*avgei+const2;
	    }else{
	      dzlay+=slope1*avgei+const1;
	    }
	    if(col+iei>ncol-2){
	      break;
	    }
	  }
	}
	if(dzlay){
	  dzlay=(dzlay+iei*(-2.0*dzr0))*dzlayfactor;
	}
	  
	/* set maximum dz based on correlation max and layover max */ 
	if(rho>0){
	  dzrhomax=LinInterp2D(dzrhomaxtable,nomincind,(rho-rhomin)/drho,
			       nominctablesize,nrho);
	  if(dzrhomax<dzlay){
	    dzlay=dzrhomax;
	  }
	}

	/* set cost parameters in terms of flow, represented as shorts */
	if(rho>0){
	  rowcost[row][col].offset=nshortcycle*
	    (dpsi[row][col]-avgdpsi[row][col]);
	}else{
	  rowcost[row][col].offset=nshortcycle*
	    (dpsi[row][col]-0.5*avgdpsi[row][col]);
	}
	nolayover=TRUE;
	if(dzlay){
	  rowcost[row][col].sigsq=(sigsqrho+sigsqei+sigsqlay)*ztoshortsq
	    /(costscale*rowweight[row][col]);
	  if(rowcost[row][col].sigsq<sigsqshortmin){
	    rowcost[row][col].sigsq=sigsqshortmin;
	  }
	  rowcost[row][col].dzmax=fabs(dzlay*ztoshort);
	  rowcost[row][col].laycost=rowweight[row][col]*glay;
	  if(labs(rowcost[row][col].dzmax)
	     >floor(sqrt(rowcost[row][col].laycost*rowcost[row][col].sigsq))){
	    nolayover=FALSE;
	  }
	}
	if(nolayover){
	  rowcost[row][col].sigsq=(sigsqrho+sigsqei)*ztoshortsq
	    /(costscale*rowweight[row][col]);
	  if(rowcost[row][col].sigsq<sigsqshortmin){
	    rowcost[row][col].sigsq=sigsqshortmin;
	  }
	  rowcost[row][col].laycost=NOCOSTSHELF;
	  rowcost[row][col].dzmax=LARGESHORT;
	}

	/* shift PDF to account for flattening by coarse unwrapped estimate */
	if(unwrappedest!=NULL){
	  rowcost[row][col].offset+=(nshortcycle/TWOPI*
				     (unwrappedest[row+1][col]
				      -unwrappedest[row][col]));
	}

      }
    }
  }  /* end of azimuth gradient cost calculation */

  /* free temporary arrays */
  Free2DArray((void **)corr,nrow);
  Free2DArray((void **)dpsi,nrow);
  Free2DArray((void **)avgdpsi,nrow);
  Free2DArray((void **)ei,nrow);
  free(dzrcrittable);
  Free2DArray((void **)dzrhomaxtable,nominctablesize);

  /* return pointer to costs arrays */
  return((void **)costs);

}


/* function: BuildStatCostsDefo()
 * ------------------------------
 * Builds statistical cost arrays for deformation mode.
 */
void **BuildStatCostsDefo(float **wrappedphase, float **mag, 
			  float **unwrappedest, float **corr, 
			  short **rowweight, short **colweight,
			  long nrow, long ncol, tileparamT *tileparams, 
			  outfileT *outfiles, paramT *params){

  long row, col;
  long kperpdpsi, kpardpsi, sigsqshortmin, defomax;
  double rho, rho0, rhopow;
  double defocorrthresh, sigsqcorr, sigsqrho, sigsqrhoconst;
  double glay, costscale;
  double nshortcycle, nshortcyclesq;
  float **dpsi, **avgdpsi;
  costT **costs, **rowcost, **colcost;

  /* get memory and set cost array pointers */
  costs=(costT **)Get2DRowColMem(nrow,ncol,sizeof(costT *),sizeof(costT));
  rowcost=(costT **)costs;
  colcost=(costT **)&costs[nrow-1];

  /* set up */
  rho0=(params->rhosconst1)/(params->ncorrlooks)+(params->rhosconst2);
  defocorrthresh=params->defothreshfactor*rho0;
  rhopow=2*(params->cstd1)+(params->cstd2)*log(params->ncorrlooks)
    +(params->cstd3)*(params->ncorrlooks);
  sigsqrhoconst=2.0/12.0;
  sigsqcorr=params->sigsqcorr;
  sigsqshortmin=params->sigsqshortmin;
  kperpdpsi=params->kperpdpsi;
  kpardpsi=params->kpardpsi;
  costscale=params->costscale; 
  nshortcycle=params->nshortcycle;
  nshortcyclesq=nshortcycle*nshortcycle;
  glay=-costscale*log(params->defolayconst);
  defomax=(long )ceil(params->defomax*nshortcycle);

  /* get memory for wrapped difference arrays */
  dpsi=(float **)Get2DMem(nrow,ncol,sizeof(float *),sizeof(float));
  avgdpsi=(float **)Get2DMem(nrow,ncol,sizeof(float *),sizeof(float));

  /* build array of mean wrapped phase differences in range */
  /* simple average of phase differences is biased, but mean phase */
  /*   differences usually near zero, so don't bother with complex average */
  fprintf(sp2,"Building range cost arrays\n");
  CalcWrappedRangeDiffs(dpsi,avgdpsi,wrappedphase,kperpdpsi,kpardpsi,
			nrow,ncol);

  /* build colcost array (range slopes) */
  for(col=0;col<ncol-1;col++){
    for(row=0;row<nrow;row++){

      /* see if we have a masked pixel */
      if(mag[row][col]==0 || mag[row][col+1]==0){
	  
	/* masked pixel */
	colcost[row][col].laycost=0;
	colcost[row][col].offset=0;
	colcost[row][col].dzmax=LARGESHORT;
	colcost[row][col].sigsq=LARGESHORT;

      }else{

	/* deformation-mode costs */
	
	/* calculate variance due to decorrelation */
	/* need symmetry for range if deformation */
	rho=(corr[row][col]+corr[row][col+1])/2.0;
	if(rho<defocorrthresh){
	  rho=0;
	}
	sigsqrho=(sigsqrhoconst*pow(1-rho,rhopow)+sigsqcorr)*nshortcyclesq;

	/* set cost paramaters in terms of flow, represented as shorts */
	if(rho>0){
	  colcost[row][col].offset=nshortcycle*
	    (dpsi[row][col]-avgdpsi[row][col]);
	}else{
	  colcost[row][col].offset=nshortcycle*
	    (dpsi[row][col]-0.5*avgdpsi[row][col]);       
	}
	colcost[row][col].sigsq=sigsqrho/(costscale*colweight[row][col]);
	if(colcost[row][col].sigsq<sigsqshortmin){
	  colcost[row][col].sigsq=sigsqshortmin;
	}
	if(rho<defocorrthresh){
	  colcost[row][col].dzmax=defomax;
	  colcost[row][col].laycost=colweight[row][col]*glay;
	  if(colcost[row][col].dzmax<floor(sqrt(colcost[row][col].laycost
						*colcost[row][col].sigsq))){
	    colcost[row][col].laycost=NOCOSTSHELF;
	    colcost[row][col].dzmax=LARGESHORT;
	  }
	}else{
	  colcost[row][col].laycost=NOCOSTSHELF;
	  colcost[row][col].dzmax=LARGESHORT;
	}
      }

      /* shift PDF to account for flattening by coarse unwrapped estimate */
      if(unwrappedest!=NULL){
	colcost[row][col].offset+=(nshortcycle/TWOPI*
				   (unwrappedest[row][col+1]
				    -unwrappedest[row][col]));
      }
    }
  }  /* end of range gradient cost calculation */

  /* build array of mean wrapped phase differences in azimuth */
  /* biased, but not much, so don't bother with complex averaging */
  fprintf(sp2,"Building azimuth cost arrays\n");
  CalcWrappedAzDiffs(dpsi,avgdpsi,wrappedphase,kperpdpsi,kpardpsi,
		     nrow,ncol);

  /* build rowcost array */
  for(col=0;col<ncol;col++){
    for(row=0;row<nrow-1;row++){

      /* see if we have a masked pixel */
      if(mag[row][col]==0 || mag[row+1][col]==0){
	  
	/* masked pixel */
	rowcost[row][col].laycost=0;
	rowcost[row][col].offset=0;
	rowcost[row][col].dzmax=LARGESHORT;
	rowcost[row][col].sigsq=LARGESHORT;

      }else{

	/* deformation-mode costs */

	/* variance due to decorrelation */
	/* get correlation and clip small values because of estimator bias */
	rho=(corr[row][col]+corr[row+1][col])/2.0;
	if(rho<defocorrthresh){
	  rho=0;
	}
        sigsqrho=(sigsqrhoconst*pow(1-rho,rhopow)+sigsqcorr)*nshortcyclesq;

	/* set cost paramaters in terms of flow, represented as shorts */
	if(rho>0){
	  rowcost[row][col].offset=nshortcycle*
	    (dpsi[row][col]-avgdpsi[row][col]);
	}else{
	  rowcost[row][col].offset=nshortcycle*
	    (dpsi[row][col]-0.5*avgdpsi[row][col]);
	}
	rowcost[row][col].sigsq=sigsqrho/(costscale*rowweight[row][col]);
	if(rowcost[row][col].sigsq<sigsqshortmin){
	  rowcost[row][col].sigsq=sigsqshortmin;
	}
	if(rho<defocorrthresh){
	  rowcost[row][col].dzmax=defomax;
	  rowcost[row][col].laycost=rowweight[row][col]*glay;
	  if(rowcost[row][col].dzmax<floor(sqrt(rowcost[row][col].laycost
						*rowcost[row][col].sigsq))){
	    rowcost[row][col].laycost=NOCOSTSHELF;
	    rowcost[row][col].dzmax=LARGESHORT;
	  }
	}else{
	  rowcost[row][col].laycost=NOCOSTSHELF;
	  rowcost[row][col].dzmax=LARGESHORT;
	}
      }

      /* shift PDF to account for flattening by coarse unwrapped estimate */
      if(unwrappedest!=NULL){
	rowcost[row][col].offset+=(nshortcycle/TWOPI*
				   (unwrappedest[row+1][col]
				    -unwrappedest[row][col]));
      }
    }
  } /* end of azimuth cost calculation */

  /* free temporary arrays */
  Free2DArray((void **)corr,nrow);
  Free2DArray((void **)dpsi,nrow);
  Free2DArray((void **)avgdpsi,nrow);

  /* return pointer to costs arrays */
  return((void **)costs);

}


/* function: BuildStatCostsSmooth()
 * --------------------------------
 * Builds statistical cost arrays for smooth-solution mode.
 */
void **BuildStatCostsSmooth(float **wrappedphase, float **mag, 
			    float **unwrappedest, float **corr, 
			    short **rowweight, short **colweight,
			    long nrow, long ncol, tileparamT *tileparams, 
			    outfileT *outfiles, paramT *params){

  long row, col;
  long kperpdpsi, kpardpsi, sigsqshortmin;
  double rho, rho0, rhopow;
  double defocorrthresh, sigsqcorr, sigsqrho, sigsqrhoconst;