snaphu_cost.c

phase unwrapping algorithm for SAR interferometry

  nshortcycle=params->nshortcycle;
  idz1=labs(flow*nshortcycle+cost->offset);
  idz2pos=labs((flow+nflow)*nshortcycle+cost->offset);
  idz2neg=labs((flow-nflow)*nshortcycle+cost->offset);

  /* calculate cost1 */
  cost1=(idz1*idz1)/cost->sigsq;

  /* calculate positive cost increment */
  poscost=(idz2pos*idz2pos)/cost->sigsq-cost1;

  /* calculate negative cost increment */
  negcost=(idz2neg*idz2neg)/cost->sigsq-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: CalcCostL0()
 * ----------------------
 * Calculates the L0 arc distance given an array of short integer weights.
 */
void CalcCostL0(void **costs, long flow, long arcrow, long arccol, 
		long nflow, long nrow, paramT *params, 
		long *poscostptr, long *negcostptr){

  /* L0-norm */
  if(flow){
    if(flow+nflow){
      *poscostptr=0;
    }else{
      *poscostptr=-((short **)costs)[arcrow][arccol];
    }
    if(flow-nflow){
      *negcostptr=0;
    }else{
      *negcostptr=-((short **)costs)[arcrow][arccol];
    }
  }else{
    *poscostptr=((short **)costs)[arcrow][arccol];
    *negcostptr=((short **)costs)[arcrow][arccol];
  }
}


/* function: CalcCostL1()
 * ----------------------
 * Calculates the L1 arc distance given an array of short integer weights.
 */
void CalcCostL1(void **costs, long flow, long arcrow, long arccol, 
		long nflow, long nrow, paramT *params, 
		long *poscostptr, long *negcostptr){

  /* L1-norm */
  *poscostptr=((short **)costs)[arcrow][arccol]*(labs(flow+nflow)-labs(flow));
  *negcostptr=((short **)costs)[arcrow][arccol]*(labs(flow-nflow)-labs(flow));

}


/* function: CalcCostL2()
 * ----------------------
 * Calculates the L2 arc distance given an array of short integer weights.
 */
void CalcCostL2(void **costs, long flow, long arcrow, long arccol, 
		long nflow, long nrow, paramT *params, 
		long *poscostptr, long *negcostptr){

  long flow2, flowsq;

  /* L2-norm */
  flowsq=flow*flow;
  flow2=flow+nflow;
  *poscostptr=((short **)costs)[arcrow][arccol]*(flow2*flow2-flowsq);
  flow2=flow-nflow;
  *negcostptr=((short **)costs)[arcrow][arccol]*(flow2*flow2-flowsq);
}


/* function: CalcCostLP()
 * ----------------------
 * Calculates the Lp arc distance given an array of short integer weights.
 */
void CalcCostLP(void **costs, long flow, long arcrow, long arccol, 
		long nflow, long nrow, paramT *params, 
		long *poscostptr, long *negcostptr){

  long p;
  short flow2;

  /* Lp-norm */
  flow2=flow+nflow;
  p=params->p;
  *poscostptr=LRound(((short **)costs)[arcrow][arccol]*
		     (pow(labs(flow2),p)-pow(labs(flow),p)));
  flow2=flow-nflow;
  *negcostptr=LRound(((short **)costs)[arcrow][arccol]*
		     (pow(labs(flow2),p)-pow(labs(flow),p)));
}


/* function: CalcCostNonGrid()
 * ---------------------------
 * Calculates the arc cost given an array of long integer cost lookup tables.
 */
void CalcCostNonGrid(void **costs, long flow, long arcrow, long arccol, 
		     long nflow, long nrow, paramT *params, 
		     long *poscostptr, long *negcostptr){

  long xflow, flowmax, poscost, negcost, nflowsq, arroffset, sumsigsqinv;
  long abscost0;
  long *costarr;
  float c1;


  /* set up */
  flowmax=params->scndryarcflowmax;
  costarr=((long ***)costs)[arcrow][arccol];
  arroffset=costarr[0];
  sumsigsqinv=costarr[2*flowmax+1];

  /* return zero costs if this is a zero cost arc */
  if(sumsigsqinv==ZEROCOSTARC){
    *poscostptr=0;
    *negcostptr=0;
    return;
  }

  /* compute cost of current flow */
  xflow=flow+arroffset;
  if(xflow>flowmax){
    c1=costarr[flowmax]/(float )flowmax-sumsigsqinv*flowmax;
    abscost0=(sumsigsqinv*xflow+LRound(c1))*xflow;
  }else if(xflow<-flowmax){
    c1=costarr[2*flowmax]/(float )flowmax-sumsigsqinv*flowmax;
    abscost0=(sumsigsqinv*xflow+LRound(c1))*xflow;
  }else{
    if(xflow>0){
      abscost0=costarr[xflow];
    }else if(xflow<0){
      abscost0=costarr[flowmax-xflow];  
    }else{
      abscost0=0;
    }
  }

  /* compute costs of positive and negative flow increments */
  xflow=flow+arroffset+nflow;
  if(xflow>flowmax){
    c1=costarr[flowmax]/(float )flowmax-sumsigsqinv*flowmax;    
    poscost=((sumsigsqinv*xflow+LRound(c1))*xflow)-abscost0;
  }else if(xflow<-flowmax){
    c1=costarr[2*flowmax]/(float )flowmax-sumsigsqinv*flowmax;    
    poscost=((sumsigsqinv*xflow+LRound(c1))*xflow)-abscost0;
  }else{
    if(xflow>0){
      poscost=costarr[xflow]-abscost0;
    }else if(xflow<0){
      poscost=costarr[flowmax-xflow]-abscost0;
    }else{
      poscost=-abscost0;
    }
  }
  xflow=flow+arroffset-nflow;
  if(xflow>flowmax){
    c1=costarr[flowmax]/(float )flowmax-sumsigsqinv*flowmax;    
    negcost=((sumsigsqinv*xflow+LRound(c1))*xflow)-abscost0;
  }else if(xflow<-flowmax){
    c1=costarr[2*flowmax]/(float )flowmax-sumsigsqinv*flowmax;    
    negcost=((sumsigsqinv*xflow+LRound(c1))*xflow)-abscost0;
  }else{
    if(xflow>0){
      negcost=costarr[xflow]-abscost0;
    }else if(xflow<0){
      negcost=costarr[flowmax-xflow]-abscost0;
    }else{
      negcost=-abscost0;
    }
  }

  /* scale for this flow increment and set output values */
  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: EvalCostTopo()
 * ------------------------
 * Calculates topography arc cost given an array of cost data structures.
 */
long EvalCostTopo(void **costs, short **flows, long arcrow, long arccol,
		  long nrow, paramT *params){

  long idz1, cost1, dzmax;
  costT *cost;

  /* get arc info */
  cost=&((costT **)(costs))[arcrow][arccol];
  if(arcrow<nrow-1){

    /* row cost: dz symmetric with respect to origin */
    idz1=labs(flows[arcrow][arccol]*(params->nshortcycle)+cost->offset);
    dzmax=cost->dzmax;

  }else{

    /* column cost: non-symmetric dz */
    idz1=flows[arcrow][arccol]*(params->nshortcycle)+cost->offset;
    if((dzmax=cost->dzmax)<0){
      idz1*=-1;
      dzmax*=-1;
    }

  }

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


/* function: EvalCostDefo()
 * ------------------------
 * Calculates deformation arc cost given an array of cost data structures.
 */
long EvalCostDefo(void **costs, short **flows, long arcrow, long arccol,
		  long nrow, paramT *params){

  long idz1, cost1;
  costT *cost;

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

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


/* function: EvalCostSmooth()
 * --------------------------
 * Calculates smooth-solution arc cost given an array of 
 * smoothcost data structures.
 */
long EvalCostSmooth(void **costs, short **flows, long arcrow, long arccol,
		    long nrow, paramT *params){

  long idz1;
  smoothcostT *cost;

  /* get arc info */
  cost=&((smoothcostT **)(costs))[arcrow][arccol];
  idz1=labs(flows[arcrow][arccol]*(params->nshortcycle)+cost->offset);

  /* calculate and return cost */
  return((idz1*idz1)/cost->sigsq);

}


/* function: EvalCostL0()
 * ----------------------
 * Calculates the L0 arc cost given an array of cost data structures.
 */
long EvalCostL0(void **costs, short **flows, long arcrow, long arccol, 
		long nrow, paramT *params){

  /* L0-norm */
  if(flows[arcrow][arccol]){
    return((long)((short **)costs)[arcrow][arccol]);
  }else{
    return(0);
  }
}


/* function: EvalCostL1()
 * ----------------------
 * Calculates the L1 arc cost given an array of cost data structures.
 */
long EvalCostL1(void **costs, short **flows, long arcrow, long arccol, 
		long nrow, paramT *params){

  /* L1-norm */
  return( (((short **)costs)[arcrow][arccol]) * labs(flows[arcrow][arccol]) );
}


/* function: EvalCostL2()
 * ----------------------
 * Calculates the L2 arc cost given an array of cost data structures.
 */
long EvalCostL2(void **costs, short **flows, long arcrow, long arccol, 
		long nrow, paramT *params){

  /* L2-norm */
  return( (((short **)costs)[arcrow][arccol]) * 
	  (flows[arcrow][arccol]*flows[arcrow][arccol]) );
}


/* function: EvalCostLP()
 * ----------------------
 * Calculates the Lp arc cost given an array of cost data structures.
 */
long EvalCostLP(void **costs, short **flows, long arcrow, long arccol, 
		long nrow, paramT *params){

  /* Lp-norm */
  return( (((short **)costs)[arcrow][arccol]) * 
	  pow(labs(flows[arcrow][arccol]),params->p) );
}


/* function: EvalCostNonGrid()
 * ---------------------------
 * Calculates the arc cost given an array of long integer cost lookup tables.
 */
long EvalCostNonGrid(void **costs, short **flows, long arcrow, long arccol, 
		     long nrow, paramT *params){

  long flow, xflow, flowmax, arroffset, sumsigsqinv;
  long *costarr;
  float c1;

  /* set up */
  flow=flows[arcrow][arccol];
  flowmax=params->scndryarcflowmax;
  costarr=((long ***)costs)[arcrow][arccol];
  arroffset=costarr[0];
  sumsigsqinv=costarr[2*flowmax+1];

  /* return zero costs if this is a zero cost arc */
  if(sumsigsqinv==ZEROCOSTARC){
    return(0);
  }

  /* compute cost of current flow */
  xflow=flow+arroffset;
  if(xflow>flowmax){
    c1=costarr[flowmax]/(float )flowmax-sumsigsqinv*flowmax;
    return((sumsigsqinv*xflow+LRound(c1))*xflow);
  }else if(xflow<-flowmax){
    c1=costarr[2*flowmax]/(float )flowmax-sumsigsqinv*flowmax;
    return((sumsigsqinv*xflow+LRound(c1))*xflow);
  }else{
    if(xflow>0){
      return(costarr[xflow]);
    }else if(xflow<0){
      return(costarr[flowmax-xflow]);
    }else{
      return(0);
    }
  }
}


/* function: CalcInitMaxFlow()
 * ---------------------------
 * Calculates the maximum flow magnitude to allow in the initialization
 * by examining the dzmax members of arc statistical cost data structures.
 */
void CalcInitMaxFlow(paramT *params, void **costs, long nrow, long ncol){

  long row, col, maxcol, initmaxflow, arcmaxflow;

  if(params->initmaxflow<=0){
    if(params->costmode==NOSTATCOSTS){
      params->initmaxflow=NOSTATINITMAXFLOW;
    }else{
      if(params->costmode==TOPO || params->costmode==DEFO){
	initmaxflow=0;
	for(row=0;row<2*nrow-1;row++){
	  if(row<nrow-1){
	    maxcol=ncol;
	  }else{
	    maxcol=ncol-1;
	  }
	  for(col=0;col<maxcol;col++){
	    if(((costT **)costs)[row][col].dzmax!=LARGESHORT){
	      arcmaxflow=ceil(labs((long )((costT **)costs)[row][col].dzmax)/
			      (double )(params->nshortcycle)
			      +params->arcmaxflowconst);
	      if(arcmaxflow>initmaxflow){
		initmaxflow=arcmaxflow;
	      }
	    }
	  }
	}
	params->initmaxflow=initmaxflow;
      }else{
	params->initmaxflow=DEF_INITMAXFLOW;
      }
    }
  }
}