snaphu_cost.c

phase unwrapping algorithm for SAR interferometry

/*************************************************************************

  snaphu statistical cost model source file
  Written by Curtis W. Chen
  Copyright 2002 Board of Trustees, Leland Stanford Jr. University
  Please see the supporting documentation for terms of use.
  No warranty.

*************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <signal.h>
#include <limits.h>
#include <float.h>
#include <string.h>
#include <ctype.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <sys/time.h>
#include <sys/resource.h>

#include "snaphu.h"


/* function: BuildCostArrays()
 * ---------------------------
 * Builds cost arrays for arcs based on interferogram intensity 
 * and correlation, depending on options and passed parameters.  
 */
void BuildCostArrays(void ***costsptr, short ***mstcostsptr, 
		     float **mag, float **wrappedphase, 
		     float **unwrappedest, long linelen, long nlines, 
		     long nrow, long ncol, paramT *params, 
		     tileparamT *tileparams, infileT *infiles, 
		     outfileT *outfiles){
  
  long row, col, maxcol, tempcost;
  long poscost, negcost, costtypesize;
  float **pwr, **corr;
  short **weights, **rowweight, **colweight, **scalarcosts;
  void **costs, **rowcost, **colcost;
  void (*CalcStatCost)();

  /* read weights */
  weights=NULL;
  ReadWeightsFile(&weights,infiles->weightfile,linelen,nlines,tileparams);
  rowweight=weights;
  colweight=&weights[nrow-1];

  /* if we're only initializing and we don't want statistical weights */
  if(params->initonly && params->costmode==NOSTATCOSTS){
    *mstcostsptr=weights;
    return;
  }

  /* size of the data type for holding cost data depends on cost mode */
  if(params->costmode==TOPO){
    costtypesize=sizeof(costT);
  }else if(params->costmode==DEFO){
    costtypesize=sizeof(costT);
  }else if(params->costmode==SMOOTH){
    costtypesize=sizeof(smoothcostT);
  }

  /* build or read the statistical cost arrays unless we were told not to */
  if(strlen(infiles->costinfile)){
    fprintf(sp1,"Reading cost information from file %s\n",infiles->costinfile);
    costs=NULL;
    Read2DRowColFile((void ***)&costs,infiles->costinfile,
		     linelen,nlines,tileparams,costtypesize);
    (*costsptr)=costs;

  }else if(params->costmode!=NOSTATCOSTS){

    /* get intensity and correlation info */
    /* correlation generated from interferogram and amplitude if not given */
    GetIntensityAndCorrelation(mag,wrappedphase,&pwr,&corr,infiles,
			       linelen,nlines,nrow,ncol,outfiles,
			       params,tileparams);

    /* call specific functions for building cost array and */
    /* set global pointers to functions for calculating and evaluating costs */
    if(params->costmode==TOPO){
      fprintf(sp1,"Calculating topography-mode cost parameters\n");
      costs=BuildStatCostsTopo(wrappedphase,mag,unwrappedest,pwr,corr,
			       rowweight,colweight,nrow,ncol,tileparams,
			       outfiles,params);
    }else if(params->costmode==DEFO){
      fprintf(sp1,"Calculating deformation-mode cost parameters\n");
      costs=BuildStatCostsDefo(wrappedphase,mag,unwrappedest,corr,
			       rowweight,colweight,nrow,ncol,tileparams,
			       outfiles,params);
    }else if(params->costmode==SMOOTH){
      fprintf(sp1,"Calculating smooth-solution cost parameters\n");
      costs=BuildStatCostsSmooth(wrappedphase,mag,unwrappedest,corr,
				 rowweight,colweight,nrow,ncol,tileparams,
				 outfiles,params);
    }else{
      fprintf(sp0,"unrecognized cost mode\n");
      exit(ABNORMAL_EXIT);
    }
    (*costsptr)=costs;
    

  }/* end if(params->costmode!=NOSTATCOSTS) */
  
  /* set array subpointers and temporary cost-calculation function pointer */
  if(params->costmode==TOPO){
    rowcost=costs;
    colcost=(void **)&(((costT **)costs)[nrow-1]);
    CalcStatCost=CalcCostTopo;
  }else if(params->costmode==DEFO){
    rowcost=costs;
    colcost=(void **)&(((costT **)costs)[nrow-1]);
    CalcStatCost=CalcCostDefo;
  }else if(params->costmode==SMOOTH){
    rowcost=costs;
    colcost=(void **)&(((smoothcostT **)costs)[nrow-1]);
    CalcStatCost=CalcCostSmooth;
  }

  /* dump statistical cost arrays */
  if(strlen(infiles->costinfile) || params->costmode!=NOSTATCOSTS){
    if(strlen(outfiles->costoutfile)){
      Write2DRowColArray((void **)costs,outfiles->costoutfile,
			nrow,ncol,costtypesize);
    }else{
      if(strlen(outfiles->rowcostfile)){
	Write2DArray((void **)rowcost,outfiles->rowcostfile,
		     nrow-1,ncol,costtypesize);
      }
      if(strlen(outfiles->colcostfile)){
	Write2DArray((void **)colcost,outfiles->colcostfile,
		     nrow,ncol-1,costtypesize);
      }
    }
  }

  /* get memory for scalar costs if in Lp mode */
  if(params->p>=0){
    scalarcosts=(short **)Get2DRowColMem(nrow,ncol,
					 sizeof(short *),sizeof(short));
    (*costsptr)=(void **)scalarcosts;
  }

  /* now, set scalar costs for MST initialization or optimization if needed */
  if(params->costmode==NOSTATCOSTS){    

    /* if in no-statistical-costs mode, copy weights to scalarcosts array */
    if(!params->initonly){
      for(row=0;row<2*nrow-1;row++){
	if(row<nrow-1){
	  maxcol=ncol;
	}else{
	  maxcol=ncol-1;
	}
	for(col=0;col<maxcol;col++){
	  scalarcosts[row][col]=weights[row][col];
	}
      }
    }

    /* unless input is already unwrapped, use weights memory for mstcosts */
    if(!params->unwrapped){
      (*mstcostsptr)=weights;
    }else{
      Free2DArray((void **)weights,2*nrow-1);
      (*mstcostsptr)=NULL;
    }

  }else if(!params->unwrapped || params->p>=0){

    /* if we got here, we had statistical costs and we need scalar weights */
    /*   from them for MST initialization or for Lp optimization */
    for(row=0;row<2*nrow-1;row++){
      if(row<nrow-1){
	maxcol=ncol;
      }else{
	maxcol=ncol-1;
      }
      for(col=0;col<maxcol;col++){

	/* calculate incremental costs for flow=0, nflow=1 */
	CalcStatCost((void **)costs,0,row,col,1,nrow,params,
		     &poscost,&negcost);

	/* take smaller of positive and negative incremental cost */
	if(poscost<negcost){
	  tempcost=poscost;
	}else{
	  tempcost=negcost;
	}

	/* clip scalar cost so it is between 1 and params->maxcost */
	if(tempcost<params->maxcost){
	  if(tempcost>MINSCALARCOST){
	    weights[row][col]=tempcost;
	  }else{
	    weights[row][col]=MINSCALARCOST;
	  }
	}else{
	  weights[row][col]=params->maxcost;
	}
	if(params->p>=0){
	  scalarcosts[row][col]=weights[row][col];
	}
      }
    }

    /* set costs for corner arcs to prevent ambiguous flows */
    weights[nrow-1][0]=LARGESHORT;
    weights[nrow-1][ncol-2]=LARGESHORT;
    weights[2*nrow-2][0]=LARGESHORT;
    weights[2*nrow-2][ncol-2]=LARGESHORT;
    if(params->p>=0){
      scalarcosts[nrow-1][0]=LARGESHORT;
      scalarcosts[nrow-1][ncol-2]=LARGESHORT;
      scalarcosts[2*nrow-2][0]=LARGESHORT;
      scalarcosts[2*nrow-2][ncol-2]=LARGESHORT;
    }
    
    /* dump mst initialization costs */
    if(strlen(outfiles->mstrowcostfile)){
      Write2DArray((void **)rowweight,outfiles->mstrowcostfile,
		   nrow-1,ncol,sizeof(short));
    }
    if(strlen(outfiles->mstcolcostfile)){
      Write2DArray((void **)colweight,outfiles->mstcolcostfile,
		   nrow,ncol-1,sizeof(short)); 
    }
    if(strlen(outfiles->mstcostsfile)){
      Write2DRowColArray((void **)rowweight,outfiles->mstcostsfile,
			 nrow,ncol,sizeof(short));
    }

    /* unless input is unwrapped, calculate initialization max flow */
    if(params->initmaxflow==AUTOCALCSTATMAX && !params->unwrapped){
      CalcInitMaxFlow(params,(void **)costs,nrow,ncol);
    }

    /* free costs memory if in init-only or Lp mode */
    if(params->initonly || params->p>=0){
      Free2DArray((void **)costs,2*nrow-1);
    }

    /* use memory allocated for weights arrays for mstcosts if needed */
    if(!params->unwrapped){
      (*mstcostsptr)=weights;
    }else{
      Free2DArray((void **)weights,2*nrow-1);
    }
 
  }else{
    Free2DArray((void **)weights,2*nrow-1);
  }

}


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

  long row, col, iei, nrho, nominctablesize;
  long kperpdpsi, kpardpsi, sigsqshortmin;
  double a, re, dr, slantrange, nearrange, nominc0, dnominc;
  double nomincangle, nomincind, sinnomincangle, cosnomincangle, bperp;
  double baseline, baselineangle, lambda, lookangle;
  double dzlay, dzei, dzr0, dzrcrit, dzeimin, dphilaypeak, dzrhomax;
  double azdzfactor, dzeifactor, dzeiweight, dzlayfactor;
  double avgei, eicrit, layminei, laywidth, slope1, const1, slope2, const2;
  double rho, rho0, rhomin, drho, rhopow;
  double sigsqrho, sigsqrhoconst, sigsqei, sigsqlay;
  double glay, costscale, ambiguityheight, ztoshort, ztoshortsq;
  double nshortcycle, midrangeambight;
  float **ei, **dpsi, **avgdpsi, *dzrcrittable, **dzrhomaxtable;
  costT **costs, **rowcost, **colcost;
  signed char noshadow, nolayover;


  /* 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);
  rhomin=params->rhominfactor*rho0;
  rhopow=2*(params->cstd1)+(params->cstd2)*log(params->ncorrlooks)
    +(params->cstd3)*(params->ncorrlooks);
  sigsqshortmin=params->sigsqshortmin;
  kperpdpsi=params->kperpdpsi;
  kpardpsi=params->kpardpsi;
  dr=params->dr;
  nearrange=params->nearrange+dr*tileparams->firstcol;
  drho=params->drho;
  nrho=(long )floor((1-rhomin)/drho)+1;
  nshortcycle=params->nshortcycle;
  layminei=params->layminei;
  laywidth=params->laywidth;
  azdzfactor=params->azdzfactor;
  dzeifactor=params->dzeifactor;
  dzeiweight=params->dzeiweight;
  dzeimin=params->dzeimin;
  dzlayfactor=params->dzlayfactor;
  sigsqei=params->sigsqei;
  lambda=params->lambda;
  noshadow=!(params->shadow);
  a=params->orbitradius;
  re=params->earthradius;

  /* despeckle the interferogram intensity */
  fprintf(sp2,"Despeckling intensity image\n");
  ei=NULL;
  Despeckle(pwr,&ei,nrow,ncol);
  Free2DArray((void **)pwr,nrow);

  /* remove large-area average intensity */
  fprintf(sp2,"Normalizing intensity\n");
  RemoveMean(ei,nrow,ncol,params->krowei,params->kcolei);

  /* dump normalized, despeckled intensity */
  if(strlen(outfiles->eifile)){
    Write2DArray((void **)ei,outfiles->eifile,nrow,ncol,sizeof(float));
  }

  /* compute some midswath parameters */
  slantrange=nearrange+ncol/2*dr;
  sinnomincangle=sin(acos((a*a-slantrange*slantrange-re*re)
			  /(2*slantrange*re)));
  lookangle=asin(re/a*sinnomincangle);

  /* see if we were passed bperp rather than baseline and baselineangle */
  if(params->bperp){
    if(params->bperp>0){
      params->baselineangle=lookangle;
    }else{
      params->baselineangle=lookangle+PI;
    }
    params->baseline=fabs(params->bperp);
  }

  /* the baseline should be halved if we are in single antenna transmit mode */
  if(params->transmitmode==SINGLEANTTRANSMIT){
    params->baseline/=2.0;
  }
  baseline=params->baseline;
  baselineangle=params->baselineangle;

  /* build lookup table for dzrcrit vs incidence angle */
  dzrcrittable=BuildDZRCritLookupTable(&nominc0,&dnominc,&nominctablesize,
				       tileparams,params);

  /* build lookup table for dzrhomax vs incidence angle */
  dzrhomaxtable=BuildDZRhoMaxLookupTable(nominc0,dnominc,nominctablesize,
					 rhomin,drho,nrho,params);
  
  /* set cost autoscale factor based on midswath parameters */
  bperp=baseline*cos(lookangle-baselineangle);
  midrangeambight=fabs(lambda*slantrange*sinnomincangle/(2*bperp));
  costscale=params->costscale*fabs(params->costscaleambight/midrangeambight);
  glay=-costscale*log(params->layconst);

  /* 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) */
  /* loop over range */
  for(col=0;col<ncol-1;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;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=LARGESHORT/2;
	colcost[row][col].dzmax=LARGESHORT;
	colcost[row][col].sigsq=LARGESHORT;

      }else{

	/* topography-mode costs */

	/* calculate variance due to decorrelation */
	/* factor of 2 in sigsqrhoconst for pdf convolution */
	rho=corr[row][col];
	if(rho<rhomin){
	  rho=0;
	}
	sigsqrho=sigsqrhoconst*pow(1-rho,rhopow);

	/* calculate dz expected from EI if no layover */
	if(ei[row][col]>eicrit){
	  dzei=(slope2*ei[row][col]+const2)*dzeifactor;