snaphu_solver.c

phase unwrapping algorithm for SAR interferometry

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

  snaphu network-flow solver 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: TreeSolve()
 * ---------------------
 * Solves the nonlinear network optimization problem.
 */
long TreeSolve(nodeT **nodes, nodesuppT **nodesupp, nodeT *ground, 
	       nodeT *source, candidateT **candidatelistptr, 
	       candidateT **candidatebagptr, long *candidatelistsizeptr,
	       long *candidatebagsizeptr, bucketT *bkts, short **flows, 
	       void **costs, incrcostT **incrcosts, nodeT ***apexes, 
	       signed char **iscandidate, long ngroundarcs, long nflow, 
	       float **mag, float **wrappedphase, char *outfile, 
	       long nnoderow, short *nnodesperrow, long narcrow, 
	       short *narcsperrow, long nrow, long ncol,
	       outfileT *outfiles, paramT *params){

  long i, row, col, arcrow, arccol, arcdir, arcnum, upperarcnum;
  long arcrow1, arccol1, arcdir1, arcrow2, arccol2, arcdir2;
  long treesize, candidatelistsize, candidatebagsize;
  long violation, groupcounter, fromgroup, group1, apexlistbase, apexlistlen;
  long cyclecost, outcostto, startlevel, dlevel, doutcost, dincost;
  long candidatelistlen, candidatebagnext;
  long inondegen, ipivots, nnodes, nnewnodes, maxnewnodes, templong;
  signed char fromside;
  candidateT *candidatelist, *candidatebag, *tempcandidateptr;
  nodeT *from, *to, *cycleapex, *node1, *node2, *leavingparent, *leavingchild;
  nodeT *root, *mntpt, *oldmntpt, *skipthread, *tempnode1, *tempnode2;
  nodeT *firstfromnode, *firsttonode;
  nodeT **apexlist;
  float **unwrappedphase;


  /* dereference some pointers and store as local variables */
  candidatelist=(*candidatelistptr);
  candidatebag=(*candidatebagptr);
  candidatelistsize=(*candidatelistsizeptr);
  candidatebagsize=(*candidatebagsizeptr);
  candidatelistlen=0;
  candidatebagnext=0;

  /* set up */
  bkts->curr=bkts->maxind;
  nnodes=InitTree(source,nodes,nodesupp,ground,ngroundarcs,bkts,nflow,
		  incrcosts,apexes,iscandidate,nnoderow,nnodesperrow,
		  narcrow,narcsperrow,nrow,ncol,params);
  apexlistlen=INITARRSIZE;
  apexlist=MAlloc(apexlistlen*sizeof(nodeT *));
  groupcounter=2;
  ipivots=0;
  inondegen=0;
  maxnewnodes=ceil(nnodes*params->maxnewnodeconst);
  nnewnodes=0;
  treesize=1;
  fprintf(sp3,"Treesize: %-10ld Pivots: %-11ld Improvements: %-11ld",
	  treesize,ipivots,inondegen);

  /* loop over each entering node (note, source already on tree) */
  while(treesize<nnodes){

    nnewnodes=0;
    while(nnewnodes<maxnewnodes && treesize<nnodes){

      /* get node with lowest outcost */
      to=MinOutCostNode(bkts);
      from=to->pred;

      /* add new node to the tree */
      GetArc(from,to,&arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
      to->group=1;
      to->level=from->level+1;
      to->incost=from->incost+GetCost(incrcosts,arcrow,arccol,-arcdir);
      to->next=from->next;
      to->prev=from;
      to->next->prev=to;
      from->next=to;
    
      /* scan new node's neighbors */
      from=to;
      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);
	
	/* if to node is on tree */
	if(to->group>0){
	  if(to!=from->pred){
	    cycleapex=FindApex(from,to);
	    apexes[arcrow][arccol]=cycleapex;
	    CheckArcReducedCost(from,to,cycleapex,arcrow,arccol,arcdir,nflow,
				nodes,ground,&candidatebag,&candidatebagnext,
				&candidatebagsize,incrcosts,iscandidate,
				params);
	  }else{
	    apexes[arcrow][arccol]=NULL;
	  }

	}else{

	  /* if to is not on tree, update outcost and add to bucket */
	  AddNewNode(from,to,arcdir,bkts,nflow,incrcosts,arcrow,arccol,params);
	  
	}
      }
      nnewnodes++;
      treesize++;
    }

    /* keep looping until no more arcs have negative reduced costs */
    while(candidatebagnext){

      /* if we received SIGINT or SIGHUP signal, dump results */
      /* keep this stuff out of the signal handler so we don't risk */
      /* writing a non-feasible solution (ie, if signal during augment) */
      /* signal handler disabled for all but primary (grid) networks */
      if(dumpresults_global){
	fprintf(sp0,"\n\nDumping current solution to file %s\n",
		outfile);
	if(requestedstop_global){
	  Free2DArray((void **)costs,2*nrow-1);
	}
	unwrappedphase=(float **)Get2DMem(nrow,ncol,sizeof(float *),
					  sizeof(float));
	IntegratePhase(wrappedphase,unwrappedphase,flows,nrow,ncol);
	FlipPhaseArraySign(unwrappedphase,params,nrow,ncol);	
	WriteOutputFile(mag,unwrappedphase,outfiles->outfile,outfiles,
			nrow,ncol);  
	if(requestedstop_global){
	  fprintf(sp0,"Program exiting\n");
	  exit(ABNORMAL_EXIT);
	}
	Free2DArray((void **)unwrappedphase,nrow);
	dumpresults_global=FALSE;
	fprintf(sp0,"\n\nProgram continuing\n");
      }

      /* swap candidate bag and candidate list pointers and sizes */
      tempcandidateptr=candidatebag;
      candidatebag=candidatelist;
      candidatelist=tempcandidateptr;
      templong=candidatebagsize;
      candidatebagsize=candidatelistsize;
      candidatelistsize=templong;
      candidatelistlen=candidatebagnext;
      candidatebagnext=0;

      /* sort candidate list by violation, with augmenting arcs always first */
      qsort((void *)candidatelist,candidatelistlen,sizeof(candidateT),
	    CandidateCompare);

      /* set all arc directions to be plus/minus 1 */
      for(i=0;i<candidatelistlen;i++){
	if(candidatelist[i].arcdir>1){
	  candidatelist[i].arcdir=1;
	}else if(candidatelist[i].arcdir<-1){
	  candidatelist[i].arcdir=-1;
	}
      }      

      /* this doesn't seem to make it any faster, so just do all of them */
      /* set the number of candidates to process */
      /* (must change candidatelistlen to ncandidates in for loop below) */
      /*
      maxcandidates=MAXCANDIDATES;
      if(maxcandidates>candidatelistlen){
	ncandidates=candidatelistlen;
      }else{
	ncandidates=maxcandidates;
      }
      */

      /* now pivot for each arc in the candidate list */
      for(i=0;i<candidatelistlen;i++){

	/* get arc info */
	from=candidatelist[i].from;
	to=candidatelist[i].to;
	arcdir=candidatelist[i].arcdir;
	arcrow=candidatelist[i].arcrow;
	arccol=candidatelist[i].arccol;

	/* unset iscandidate */
	iscandidate[arcrow][arccol]=FALSE;

	/* make sure the next arc still has a negative violation */
	outcostto=from->outcost+
	  GetCost(incrcosts,arcrow,arccol,arcdir);
	cyclecost=outcostto + to->incost 
	  -apexes[arcrow][arccol]->outcost
	  -apexes[arcrow][arccol]->incost;

	/* if violation no longer negative, check reverse arc */
	if(!((outcostto < to->outcost) || (cyclecost < 0))){
	  from=to;
	  to=candidatelist[i].from;
	  arcdir=-arcdir;
	  outcostto=from->outcost+
	    GetCost(incrcosts,arcrow,arccol,arcdir);
	  cyclecost=outcostto + to->incost 
	    -apexes[arcrow][arccol]->outcost
	    -apexes[arcrow][arccol]->incost;
	}

	/* see if the cycle is negative (see if there is a violation) */
	if((outcostto < to->outcost) || (cyclecost < 0)){

	  /* make sure the group counter hasn't gotten too big */
	  if(++groupcounter>MAXGROUPBASE){
	    for(row=0;row<nnoderow;row++){
	      for(col=0;col<nnodesperrow[row];col++){
		if(nodes[row][col].group>0){
		  nodes[row][col].group=1;
		}
	      }
	    }
	    if(ground!=NULL && ground->group>0){
	      ground->group=1;
	    }
	    groupcounter=2;
	  }

	  /* if augmenting cycle (nondegenerate pivot) */
	  if(cyclecost<0){

	    /* augment flow along cycle and select leaving arc */
	    /* if we are augmenting non-zero flow, any arc with zero flow */
	    /* after the augmentation is a blocking arc */
	    while(TRUE){
	      fromside=TRUE;
	      node1=from;
	      node2=to;
	      leavingchild=NULL;
	      flows[arcrow][arccol]+=arcdir*nflow;
	      ReCalcCost(costs,incrcosts,flows[arcrow][arccol],arcrow,arccol,
			 nflow,nrow,params);
	      violation=GetCost(incrcosts,arcrow,arccol,arcdir);
	      if(node1->level > node2->level){
		while(node1->level != node2->level){
		  GetArc(node1->pred,node1,&arcrow1,&arccol1,&arcdir1,
			 nrow,ncol,nodesupp);
		  flows[arcrow1][arccol1]+=(arcdir1*nflow);
		  ReCalcCost(costs,incrcosts,flows[arcrow1][arccol1],
			     arcrow1,arccol1,nflow,nrow,params);
		  if(leavingchild==NULL 
		     && !flows[arcrow1][arccol1]){
		    leavingchild=node1;
		  }
		  violation+=GetCost(incrcosts,arcrow1,arccol1,arcdir1);
		  node1->group=groupcounter+1;
		  node1=node1->pred;
		}
	      }else{
		while(node1->level != node2->level){
		  GetArc(node2->pred,node2,&arcrow2,&arccol2,&arcdir2,
			 nrow,ncol,nodesupp);
		  flows[arcrow2][arccol2]-=(arcdir2*nflow);
		  ReCalcCost(costs,incrcosts,flows[arcrow2][arccol2],
			     arcrow2,arccol2,nflow,nrow,params);
		  if(!flows[arcrow2][arccol2]){
		    leavingchild=node2;
		    fromside=FALSE;
		  }
		  violation+=GetCost(incrcosts,arcrow2,arccol2,-arcdir2);
		  node2->group=groupcounter;
		  node2=node2->pred;
		}
	      }
	      while(node1!=node2){
		GetArc(node1->pred,node1,&arcrow1,&arccol1,&arcdir1,nrow,ncol,
		       nodesupp);
		GetArc(node2->pred,node2,&arcrow2,&arccol2,&arcdir2,nrow,ncol,
		       nodesupp);
		flows[arcrow1][arccol1]+=(arcdir1*nflow);
		flows[arcrow2][arccol2]-=(arcdir2*nflow);
		ReCalcCost(costs,incrcosts,flows[arcrow1][arccol1],
			   arcrow1,arccol1,nflow,nrow,params);
		ReCalcCost(costs,incrcosts,flows[arcrow2][arccol2],
			   arcrow2,arccol2,nflow,nrow,params);
		violation+=(GetCost(incrcosts,arcrow1,arccol1,arcdir1)
			    +GetCost(incrcosts,arcrow2,arccol2,-arcdir2));
		if(!flows[arcrow2][arccol2]){
		  leavingchild=node2;
		  fromside=FALSE;
		}else if(leavingchild==NULL 
			 && !flows[arcrow1][arccol1]){
		  leavingchild=node1;
		}
		node1->group=groupcounter+1;
		node2->group=groupcounter;
		node1=node1->pred;
		node2=node2->pred;
	      }
	      if(violation>=0){
		break;
	      }
	    }
	    inondegen++;

	  }else{

	    /* We are not augmenting flow, but just updating potentials. */
	    /* Arcs with zero flow are implicitly directed upwards to */
	    /* maintain a strongly feasible spanning tree, so arcs with zero */
	    /* flow on the path between to node and apex are blocking arcs. */
	    /* Leaving arc is last one whose child's new outcost is less */
	    /* than its old outcost.  Such an arc must exist, or else */
	    /* we'd be augmenting flow on a negative cycle. */
	    
	    /* trace the cycle and select leaving arc */
	    fromside=FALSE;
	    node1=from;
	    node2=to;
	    leavingchild=NULL;
	    if(node1->level > node2->level){
	      while(node1->level != node2->level){
		node1->group=groupcounter+1;
		node1=node1->pred;
	      }
	    }else{
	      while(node1->level != node2->level){
		if(outcostto < node2->outcost){
		  leavingchild=node2;
		  GetArc(node2->pred,node2,&arcrow2,&arccol2,&arcdir2,
			 nrow,ncol,nodesupp);
		  outcostto+=GetCost(incrcosts,arcrow2,arccol2,-arcdir2);
		}else{
		  outcostto=VERYFAR;
		}
		node2->group=groupcounter;
		node2=node2->pred;
	      }
	    }
	    while(node1!=node2){
	      if(outcostto < node2->outcost){
		leavingchild=node2;
		GetArc(node2->pred,node2,&arcrow2,&arccol2,&arcdir2,nrow,ncol,
		       nodesupp);
		outcostto+=GetCost(incrcosts,arcrow2,arccol2,-arcdir2);
	      }else{
		outcostto=VERYFAR;
	      }
	      node1->group=groupcounter+1;
	      node2->group=groupcounter;
	      node1=node1->pred;
	      node2=node2->pred;
	    }
	  }
	  cycleapex=node1;

          /* set leaving parent */ 
          if(leavingchild==NULL){
	    fromside=TRUE;
	    leavingparent=from;
	  }else{
	    leavingparent=leavingchild->pred;
	  }

          /* swap from and to if leaving arc is on the from side */
	  if(fromside){
	    groupcounter++;
	    fromgroup=groupcounter-1;
	    tempnode1=from;
	    from=to;
	    to=tempnode1;
	  }else{
	    fromgroup=groupcounter+1;
	  }

	  /* if augmenting pivot */
	  if(cyclecost<0){

	    /* find first child of apex on either cycle path */
	    firstfromnode=NULL;
	    firsttonode=NULL;
	    if(cycleapex->row!=GROUNDROW){
	      arcnum=-5;
	      upperarcnum=-1;
	    }else{
	      arcnum=-1;
	      upperarcnum=ngroundarcs-1;
	    }
	    while(arcnum<upperarcnum){
	      tempnode1=NeighborNode(cycleapex,++arcnum,&upperarcnum,nodes,
				     ground,&arcrow,&arccol,&arcdir,nrow,ncol,
				     nodesupp);
	      if(tempnode1->group==groupcounter
		 && apexes[arcrow][arccol]==NULL){
		firsttonode=tempnode1;
		if(firstfromnode!=NULL){
		  break;
		}
	      }else if(tempnode1->group==fromgroup 
		       && apexes[arcrow][arccol]==NULL){
		firstfromnode=tempnode1;
		if(firsttonode!=NULL){
		  break;
		}
	      }
	    }

	    /* update potentials, mark stationary parts of tree */
	    cycleapex->group=groupcounter+2;
	    if(firsttonode!=NULL){
	      NonDegenUpdateChildren(cycleapex,leavingparent,firsttonode,0,
				     ngroundarcs,nflow,nodes,nodesupp,ground,
				     apexes,incrcosts,nrow,ncol,params); 
	    }
	    if(firstfromnode!=NULL){
	      NonDegenUpdateChildren(cycleapex,from,firstfromnode,1,
				     ngroundarcs,nflow,nodes,nodesupp,ground,
				     apexes,incrcosts,nrow,ncol,params);
	    }
	    groupcounter=from->group;
	    apexlistbase=cycleapex->group;

	    /* children of cycleapex are not marked, so we set fromgroup */
	    /*   equal to cycleapex group for use with apex updates below */
	    /* all other children of cycle will be in apexlist if we had an */
	    /*   augmenting pivot, so fromgroup only important for cycleapex */
	    fromgroup=cycleapex->group;

	  }else{

	    /* set this stuff for use with apex updates below */
	    cycleapex->group=fromgroup;
	    groupcounter+=2;
	    apexlistbase=groupcounter+1;
	  }

	  /* remount subtree at new mount point */
	  if(leavingchild==NULL){
	    
	    skipthread=to;

	  }else{

	    root=from;