snaphu_solver.c

phase unwrapping algorithm for SAR interferometry

    if(iclipped>1){
      strcpy(pl,"s");
    }else{
      strcpy(pl,"");
    }
    fprintf(sp0,"%ld incremental cost%s clipped to avoid overflow (%.3f%%)\n",
	    iclipped,pl,((double )iclipped)/(2*narcs));
  }
}


/* function: EvaluateTotalCost()
 * -----------------------------
 * Computes the total cost of the flow array and prints it out.  Pass nrow
 * and ncol if in grid mode (primary network), or pass nrow=ntiles and 
 * ncol=0 for nongrid mode (secondary network).
 */
totalcostT EvaluateTotalCost(void **costs, short **flows, long nrow, long ncol,
			     short *narcsperrow,paramT *params){

  totalcostT rowcost, totalcost;
  long row, col, maxrow, maxcol;

  /* sum cost for each row and column arc */
  totalcost=0;
  if(ncol){
    maxrow=2*nrow-1;
  }else{
    maxrow=nrow;
  }
  for(row=0;row<maxrow;row++){
    rowcost=0;
    if(ncol){
      if(row<nrow-1){
	maxcol=ncol;
      }else{
	maxcol=ncol-1;
      }
    }else{
      maxcol=narcsperrow[row];
    }
    for(col=0;col<maxcol;col++){
      rowcost+=EvalCost(costs,flows,row,col,nrow,params);
    }
    totalcost+=rowcost;
  }

  return(totalcost);
}


/* function: MSTInitFlows()
 * ------------------------
 * Initializes the flow on a the network using minimum spanning tree
 * algorithm.  
 */
void MSTInitFlows(float **wrappedphase, short ***flowsptr, 
		  short **mstcosts, long nrow, long ncol, 
		  nodeT ***nodesptr, nodeT *ground, long maxflow){

  long row, col, i, maxcost;
  signed char **residue, **arcstatus;
  short **flows;
  nodeT *source;
  bucketT bkts[1];

  /* get and initialize memory for ground, nodes, buckets, and child array */
  *nodesptr=(nodeT **)Get2DMem(nrow-1,ncol-1,sizeof(nodeT *),sizeof(nodeT));
  InitNodeNums(nrow-1,ncol-1,*nodesptr,ground);

  /* find maximum cost */
  maxcost=0;
  for(row=0;row<2*nrow-1;row++){
    if(row<nrow-1){
      i=ncol;
    }else{
      i=ncol-1;
    }
    for(col=0;col<i;col++){
      if(mstcosts[row][col]>maxcost 
	 && !((row==nrow-1 || 2*nrow-2) && (col==0 || col==ncol-2))){
	maxcost=mstcosts[row][col];
      }
    }
  }

  /* get memory for buckets and arc status */
  bkts->size=LRound((maxcost+1)*(nrow+ncol+1));
  bkts->bucketbase=(nodeT **)MAlloc(bkts->size*sizeof(nodeT *));
  bkts->minind=0;
  bkts->maxind=bkts->size-1;
  bkts->bucket=bkts->bucketbase;
  arcstatus=(signed char **)Get2DRowColMem(nrow,ncol,sizeof(signed char *),
					   sizeof(signed char));

  /* calculate phase residues (integer numbers of cycles) */
  fprintf(sp1,"Initializing flows with MST algorithm\n");
  residue=(signed char **)Get2DMem(nrow-1,ncol-1,sizeof(signed char *),
				   sizeof(signed char));
  CycleResidue(wrappedphase,residue,nrow,ncol);

  /* get memory for flow arrays */
  (*flowsptr)=(short **)Get2DRowColZeroMem(nrow,ncol,
					   sizeof(short *),sizeof(short));
  flows=*flowsptr;

  /* loop until no flows exceed the maximum flow */
  fprintf(sp2,"Running approximate minimum spanning tree solver\n");
  while(TRUE){

    /* set up the source to be the first non-zero residue that we find */
    source=NULL;
    for(row=0;row<nrow-1 && source==NULL;row++){
      for(col=0;col<ncol-1 && source==NULL;col++){
	if(residue[row][col]){
	  source=&(*nodesptr)[row][col];
	}
      }
    }
    if(source==NULL){
      fprintf(sp1,"No residues found\n");
      break;
    }

    /* initialize data structures */
    InitNodes(nrow-1,ncol-1,*nodesptr,ground);
    InitBuckets(bkts,source,bkts->size);
    
    /* solve the mst problem */
    SolveMST(*nodesptr,source,ground,bkts,mstcosts,residue,arcstatus,
	     nrow,ncol);
    
    /* find flows on minimum tree (only one feasible flow exists) */
    DischargeTree(source,mstcosts,flows,residue,arcstatus,
		  *nodesptr,ground,nrow,ncol);
    
    /* do pushes to clip the flows and make saturated arcs ineligible */
    /* break out of loop if there is no flow greater than the limit */
    if(ClipFlow(residue,flows,mstcosts,nrow,ncol,maxflow)){
      break;
    }
  }
   
  /* free memory and return */
  Free2DArray((void **)residue,nrow-1);
  Free2DArray((void **)arcstatus,2*nrow-1);
  Free2DArray((void **)mstcosts,2*nrow-1);
  free(bkts->bucketbase);
  return;
  
}


/* function: SolveMST()
 * --------------------
 * Finds tree which spans all residue nodes of approximately minimal length.
 * Note that this function may produce a Steiner tree (tree may split at 
 * non-residue node), though finding the exactly minimum Steiner tree is 
 * NP-hard.  This function uses Prim's algorithm, nesting Dijkstra's 
 * shortest path algorithm in each iteration to find next closest residue 
 * node to tree.  See Ahuja, Orlin, and Magnanti 1993 for details.  
 *
 * Dijkstra implementation and some associated functions adapted from SPLIB 
 * shortest path codes written by Cherkassky, Goldberg, and Radzik.
 */
void SolveMST(nodeT **nodes, nodeT *source, nodeT *ground, 
	      bucketT *bkts, short **mstcosts, signed char **residue, 
	      signed char **arcstatus, long nrow, long ncol){

  nodeT *from, *to, *pathfrom, *pathto;
  nodesuppT **nodesupp;
  long fromdist, newdist, arcdist, ngroundarcs, groundcharge;
  long fromrow, fromcol, row, col, arcnum, upperarcnum, maxcol;
  long pathfromrow, pathfromcol;
  long arcrow, arccol, arcdir;

  /* initialize some variables */
  nodesupp=NULL;

  /* calculate the number of ground arcs */
  ngroundarcs=2*(nrow+ncol-2)-4;

  /* calculate charge on ground */
  groundcharge=0;
  for(row=0;row<nrow-1;row++){
    for(col=0;col<ncol-1;col++){
      groundcharge-=residue[row][col];
    }
  }

  /* initialize arc status array */
  for(arcrow=0;arcrow<2*nrow-1;arcrow++){
    if(arcrow<nrow-1){
      maxcol=ncol;
    }else{
      maxcol=ncol-1;
    }
    for(arccol=0;arccol<maxcol;arccol++){
      arcstatus[arcrow][arccol]=0;
    }
  }

  /* loop until there are no more nodes in any bucket */
  while((from=ClosestNode(bkts))!=NULL){

    /* info for current node */
    fromrow=from->row;
    fromcol=from->col;
    
    /* if we found a residue */
    if(((fromrow!=GROUNDROW && residue[fromrow][fromcol]) || 
       (fromrow==GROUNDROW && groundcharge)) && from!=source){
      
      /* set node and its predecessor */
      pathto=from;
      pathfrom=from->pred;

      /* go back and make arcstatus -1 along path */
      while(TRUE){

	/* give to node zero distance label */
	pathto->outcost=0;

	/* get arc indices for arc between pathfrom and pathto */
	GetArc(pathfrom,pathto,&arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
	
	/* set arc status to -1 to mark arc on tree */
	arcstatus[arcrow][arccol]=-1;
	
	/* stop when we get to a residue */
	pathfromrow=pathfrom->row;
	pathfromcol=pathfrom->col;
	if((pathfromrow!=GROUNDROW && residue[pathfromrow][pathfromcol])
	   || (pathfromrow==GROUNDROW && groundcharge)){
	  break;
	}
	
	/* move up to previous node pair in path */
	pathto=pathfrom;
	pathfrom=pathfrom->pred;

      } /* end while loop marking costs on path */
      
    } /* end if we found a residue */

    /* set a variable for from node's distance */
    fromdist=from->outcost;

    /* scan from's neighbors */
    if(fromrow!=GROUNDROW){
      arcnum=-5;
      upperarcnum=-1;
    }else{
      arcnum=-1;
      upperarcnum=ngroundarcs-1;
    }
    while(arcnum<upperarcnum){

      /* get row, col indices and distance of next node */
      to=NeighborNode(from,++arcnum,&upperarcnum,nodes,ground,
                      &arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
      row=to->row;
      col=to->col;

      /* get cost of arc to new node (if arc on tree, cost is 0) */
      if(arcstatus[arcrow][arccol]<0){
	arcdist=0;
      }else if((arcdist=mstcosts[arcrow][arccol])==LARGESHORT){
	arcdist=VERYFAR;
      }

      /* compare distance of new nodes to temp labels */
      if((newdist=fromdist+arcdist)<(to->outcost)){

	/* if to node is already in a bucket, remove it */
	if(to->group==INBUCKET){
	  if(to->outcost<bkts->maxind){
	    BucketRemove(to,to->outcost,bkts);
	  }else{
	    BucketRemove(to,bkts->maxind,bkts);
	  }
	}
		
	/* update to node */
	to->outcost=newdist;
	to->pred=from;

	/* insert to node into appropriate bucket */
	if(newdist<bkts->maxind){
	  BucketInsert(to,newdist,bkts);
	  if(newdist<bkts->curr){
	    bkts->curr=newdist;
	  }
	}else{
	  BucketInsert(to,bkts->maxind,bkts);
	}
	
      } /* end if newdist < old dist */
      
    } /* end loop over outgoing arcs */
  } /* end while ClosestNode()!=NULL */

}


/* function: DischargeTree()
 * -------------------------
 * Does depth-first search on result tree from SolveMST.  Integrates
 * charges from tree leaves back up to set arc flows.  This implementation
 * is non-recursive; a recursive implementation might be faster, but 
 * would also use much more stack memory.  This method is equivalent to 
 * walking the tree, so it should be nore more than a factor of 2 slower.
 */
long DischargeTree(nodeT *source, short **mstcosts, short **flows,
		   signed char **residue, signed char **arcstatus, 
		   nodeT **nodes, nodeT *ground, long nrow, long ncol){

  long row, col, todir, arcrow, arccol, arcdir;
  long arcnum, upperarcnum, ngroundarcs;
  nodeT *from, *to, *nextnode;
  nodesuppT **nodesupp;


  /* set up */
  /* use group member of node structure to temporarily store charge */
  nextnode=source;
  ground->group=0;
  for(row=0;row<nrow-1;row++){
    for(col=0;col<ncol-1;col++){
      nodes[row][col].group=residue[row][col];
      ground->group-=residue[row][col];
    }
  }
  ngroundarcs=2*(nrow+ncol-2)-4;
  nodesupp=NULL;

  /* keep looping unitl we've walked the entire tree */
  while(TRUE){
    
    from=nextnode;
    nextnode=NULL;

    /* loop over outgoing arcs from this node */
    if(from->row!=GROUNDROW){
      arcnum=-5;
      upperarcnum=-1;
    }else{
      arcnum=-1;
      upperarcnum=ngroundarcs-1;
    }
    while(arcnum<upperarcnum){

      /* get row, col indices and distance of next node */
      to=NeighborNode(from,++arcnum,&upperarcnum,nodes,ground,
		      &arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
      
      /* see if the arc is on the tree and if it has been followed yet */
      if(arcstatus[arcrow][arccol]==-1){

	/* arc has not yet been followed: move down the tree */
	nextnode=to;
	row=arcrow;
	col=arccol;
	break;

      }else if(arcstatus[arcrow][arccol]==-2){

	/* arc has already been followed and leads back up the tree: */
	/* save it, but keep looking for downwards arc */
	nextnode=to;
	row=arcrow;
	col=arccol;
	todir=arcdir;

      }
    }

    /* break if no unfollowed arcs (ie, we are done examining the tree) */
    if(nextnode==NULL){
      break;
    }

    /* if we found leaf and we're moving back up the tree, do a push */
    /* otherwise, just mark the path by decrementing arcstatus */
    if((--arcstatus[row][col])==-3){
      flows[row][col]+=todir*from->group;
      nextnode->group+=from->group;
      from->group=0;
    }
  }

  /* finish up */
  return(from->group);
  
} /* end of DischargeTree() */


/* function: ClipFlow()
 * ---------------------
 * Given a flow, clips flow magnitudes to a computed limit, resets 
 * residues so sum of solution of network problem with new residues 
 * and solution of clipped problem give total solution.  Upper flow limit
 * is 2/3 the maximum flow on the network or the passed value maxflow, 
 * whichever is greater.  Clipped flow arcs get costs of passed variable 
 * maxcost.  Residues should have been set to zero by DischargeTree().
 */
signed char ClipFlow(signed char **residue, short **flows, 
		     short **mstcosts, long nrow, long ncol, 
		     long maxflow){

  long row, col, cliplimit, maxcol, excess, tempcharge, sign;
  long mostflow, maxcost;


  /* find maximum flow */
  mostflow=Short2DRowColAbsMax(flows,nrow,ncol);

  /* if there is no flow greater than the maximum, return TRUE */
  if(mostflow<=maxflow){
    return(TRUE);
  }
  fprintf(sp2,"Maximum flow on network: %ld\n",mostflow);

  /* set upper flow limit */
  cliplimit=(long )ceil(mostflow*CLIPFACTOR)+1;
  if(maxflow>cliplimit){
    cliplimit=maxflow;
  }

  /* find maximum cost (excluding ineligible corner arcs) */
  maxcost=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(mstcosts[row][col]>maxcost && mstcosts[row][col]<LARGESHORT){
	maxcost=mstcosts[row][col];
      }
    }
  }

  /* set the new maximum cost and make sure it doesn't overflow short int */
  maxcost+=INITMAXCOSTINCR;
  if(maxcost>=LARGESHORT){
    fprintf(sp0,"WARNING: escaping ClipFlow loop to prevent cost overflow\n");
    return(TRUE);
  }

  /* clip flows and do pushes */
  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(labs(flows[row][col])>cliplimit){
	if(flows[row][col]>0){
	  sign=1;
	  excess=flows[row][col]-cliplimit;
	}else{
	  sign=-1;
	  excess=flows[row][col]+cliplimit;
	}
	if(row<nrow-1){
	  if(col!=0){
	    tempcharge=residue[row][col-1]+excess;
	    if(tempcharge>MAXRES || tempcharge<MINRES){
	      fprintf(sp0,"Overflow of residue data type\nAbort\