snaphu_solver.c

phase unwrapping algorithm for SAR interferometry

  /* return the number of nodes in the network */
  return(nnodes);
      
}


/* function: FindApex()
 * --------------------
 * Given pointers to two nodes on a spanning tree, the function finds
 * and returns a pointer to their deepest common ancestor, the apex of
 * a cycle formed by joining the two nodes with an arc.
 */
nodeT *FindApex(nodeT *from, nodeT *to){

  if(from->level > to->level){
    while(from->level != to->level){
      from=from->pred;
    }
  }else{
    while(from->level != to->level){
      to=to->pred;
    }
  }
  while(from != to){
    from=from->pred;
    to=to->pred;
  }
  return(from);
}


/* function: CandidateCompare()
 * ----------------------------
 * Compares the violations of candidate arcs for sorting.  First checks
 * if either candidate has an arcdir magnitude greater than 1, denoting 
 * an augmenting cycle.  Augmenting candidates are always placed before 
 * non-augmenting candidates.  Otherwise, returns positive if the first  
 * candidate has a greater (less negative) violation than the second, 0 
 * if they are the same, and negative otherwise.  
 */
int CandidateCompare(const void *c1, const void *c2){

  if(labs(((candidateT *)c1)->arcdir) > 1){
    if(labs(((candidateT *)c2)->arcdir) < 2){
      return(-1);
    }
  }else if(labs(((candidateT *)c2)->arcdir) > 1){
    return(1);
  }

  return(((candidateT *)c1)->violation - ((candidateT *)c2)->violation);

  /*
  if(((candidateT *)c1)->violation > ((candidateT *)c2)->violation){
    return(1);
  }else if(((candidateT *)c1)->violation < ((candidateT *)c2)->violation){
    return(-1);
  }else{
    return(0);
  }
  */
}


/* function: NeighborNodeGrid()
 * ----------------------------
 * Return the neighboring node of the given node corresponding to the
 * given arc number for a grid network with a ground node.
 */
nodeT *NeighborNodeGrid(nodeT *node1, long arcnum, long *upperarcnumptr,
			nodeT **nodes, nodeT *ground, long *arcrowptr, 
			long *arccolptr, long *arcdirptr, long nrow, 
			long ncol, nodesuppT **nodesupp){
  long row, col;

  row=node1->row;
  col=node1->col;

  switch(arcnum){
  case -4:
    *arcrowptr=row;
    *arccolptr=col+1;
    *arcdirptr=1;
    if(col==ncol-2){
      return(ground);
    }else{
      return(&nodes[row][col+1]);
    }
    break;
  case -3:
    *arcrowptr=nrow+row;
    *arccolptr=col;
    *arcdirptr=1;
    if(row==nrow-2){
      return(ground);
    }else{
      return(&nodes[row+1][col]);
    }
    break;
  case -2:
    *arcrowptr=row;
    *arccolptr=col;
    *arcdirptr=-1;
    if(col==0){
      return(ground);
    }else{
      return(&nodes[row][col-1]);
    }
    break;
  case -1:
    *arcrowptr=nrow-1+row;
    *arccolptr=col;
    *arcdirptr=-1;
    if(row==0){
      return(ground);
    }else{
      return(&nodes[row-1][col]);
    }
    break;
  default:
    if(arcnum<nrow-1){
      *arcrowptr=arcnum;
      *arccolptr=0;
      *arcdirptr=1;
      return(&nodes[*arcrowptr][0]);
    }else if(arcnum<2*(nrow-1)){           
      *arcrowptr=arcnum-(nrow-1);
      *arccolptr=ncol-1;
      *arcdirptr=-1;
      return(&nodes[*arcrowptr][ncol-2]);
    }else if(arcnum<2*(nrow-1)+ncol-3){   
      *arcrowptr=nrow-1;
      *arccolptr=arcnum-2*(nrow-1)+1;    
      *arcdirptr=1;
      return(&nodes[0][*arccolptr]);
    }else{
      *arcrowptr=2*nrow-2;
      *arccolptr=arcnum-(2*(nrow-1)+ncol-3)+1;
      *arcdirptr=-1;
      return(&nodes[nrow-2][*arccolptr]);
    }
    break;
  }

}


/* function: NeighborNodeNonGrid()
 * -------------------------------
 * Return the neighboring node of the given node corresponding to the
 * given arc number for a nongrid network (ie, arbitrary topology).
 */
nodeT *NeighborNodeNonGrid(nodeT *node1, long arcnum, long *upperarcnumptr,
			   nodeT **nodes, nodeT *ground, long *arcrowptr, 
			   long *arccolptr, long *arcdirptr, long nrow, 
			   long ncol, nodesuppT **nodesupp){

  long tilenum, nodenum;
  scndryarcT *outarc;

  /* set up */
  tilenum=node1->row;
  nodenum=node1->col;
  *upperarcnumptr=nodesupp[tilenum][nodenum].noutarcs-5;
  
  /* set the arc row (tilenumber) and column (arcnumber) */
  outarc=nodesupp[tilenum][nodenum].outarcs[arcnum+4];
  *arcrowptr=outarc->arcrow;
  *arccolptr=outarc->arccol;
  if(node1==outarc->from){
    *arcdirptr=1;
  }else{
    *arcdirptr=-1;
  }

  /* return the neighbor node */
  return(nodesupp[tilenum][nodenum].neighbornodes[arcnum+4]);

}


/* function: GetArcGrid()
 * ----------------------
 * Given a from node and a to node, sets pointers for indices into
 * arc arrays, assuming primary (grid) network.
 */
void GetArcGrid(nodeT *from, nodeT *to, long *arcrow, long *arccol, 
		long *arcdir, long nrow, long ncol, nodesuppT **nodesupp){

  long fromrow, fromcol, torow, tocol;

  fromrow=from->row;
  fromcol=from->col;
  torow=to->row;
  tocol=to->col;
  
  if(fromcol==tocol-1){           /* normal arcs (neither endpoint ground) */
    *arcrow=fromrow;
    *arccol=fromcol+1;
    *arcdir=1;
  }else if(fromcol==tocol+1){
    *arcrow=fromrow;
    *arccol=fromcol;
    *arcdir=-1;
  }else if(fromrow==torow-1){
    *arcrow=fromrow+1+nrow-1;
    *arccol=fromcol;
    *arcdir=1;
  }else if(fromrow==torow+1){
    *arcrow=fromrow+nrow-1;
    *arccol=fromcol;
    *arcdir=-1;
  }else if(fromcol==0){           /* arcs to ground */
    *arcrow=fromrow;
    *arccol=0;
    *arcdir=-1;
  }else if(fromcol==ncol-2){
    *arcrow=fromrow;
    *arccol=ncol-1;
    *arcdir=1;
  }else if(fromrow==0){
    *arcrow=nrow-1;
    *arccol=fromcol;
    *arcdir=-1;
  }else if(fromrow==nrow-2){
    *arcrow=2*(nrow-1);
    *arccol=fromcol;
    *arcdir=1;
  }else if(tocol==0){             /* arcs from ground */
    *arcrow=torow;
    *arccol=0;
    *arcdir=1;
  }else if(tocol==ncol-2){
    *arcrow=torow;
    *arccol=ncol-1;
    *arcdir=-1;
  }else if(torow==0){
    *arcrow=nrow-1;
    *arccol=tocol;
    *arcdir=1;
  }else{
    *arcrow=2*(nrow-1);
    *arccol=tocol;
    *arcdir=-1;
  }

}


/* function: GetArcNonGrid()
 * -------------------------
 * Given a from node and a to node, sets pointers for indices into
 * arc arrays, assuming secondary (arbitrary topology) network.
 */
void GetArcNonGrid(nodeT *from, nodeT *to, long *arcrow, long *arccol, 
		   long *arcdir, long nrow, long ncol, nodesuppT **nodesupp){

  long tilenum, nodenum, arcnum;
  scndryarcT *outarc;

  /* get tile and node numbers for from node */
  tilenum=from->row;
  nodenum=from->col;

  /* loop over all outgoing arcs of from node */
  arcnum=0;
  while(TRUE){
    outarc=nodesupp[tilenum][nodenum].outarcs[arcnum++];
    if(outarc->from==to){
      *arcrow=outarc->arcrow;
      *arccol=outarc->arccol;
      *arcdir=-1;
      return;
    }else if(outarc->to==to){
      *arcrow=outarc->arcrow;
      *arccol=outarc->arccol;
      *arcdir=1;
      return;
    }
  }
}


/* Function: NonDegenUpdateChildren()
 * ----------------------------------
 * Updates potentials and groups of all childredn along an augmenting path, 
 * until a stop node is hit.
 */
void NonDegenUpdateChildren(nodeT *startnode, nodeT *lastnode, 
			    nodeT *nextonpath, long dgroup, 
			    long ngroundarcs, long nflow, nodeT **nodes,
			    nodesuppT **nodesupp, nodeT *ground, 
			    nodeT ***apexes, incrcostT **incrcosts, 
			    long nrow, long ncol, paramT *params){

  nodeT *node1, *node2;
  long dincost, doutcost, arcnum, upperarcnum, startlevel;
  long group1, pathgroup, arcrow, arccol, arcdir;

  /* loop along flow path  */
  node1=startnode;
  pathgroup=lastnode->group;
  while(node1!=lastnode){

    /* update potentials along the flow path by calculating arc distances */
    node2=nextonpath;
    GetArc(node2->pred,node2,&arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
    doutcost=node1->outcost - node2->outcost
      + GetCost(incrcosts,arcrow,arccol,arcdir);
    node2->outcost+=doutcost;
    dincost=node1->incost - node2->incost
      + GetCost(incrcosts,arcrow,arccol,-arcdir);
    node2->incost+=dincost;
    node2->group=node1->group+dgroup;

    /* update potentials of children of this node in the flow path */
    node1=node2;
    if(node1->row!=GROUNDROW){
      arcnum=-5;
      upperarcnum=-1;
    }else{
      arcnum=-1;
      upperarcnum=ngroundarcs-1;
    }
    while(arcnum<upperarcnum){
      node2=NeighborNode(node1,++arcnum,&upperarcnum,nodes,ground,
                         &arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
      if(node2->pred==node1 && node2->group>0){
        if(node2->group==pathgroup){
          nextonpath=node2;
        }else{
          startlevel=node2->level;
          group1=node1->group;
          while(TRUE){
            node2->group=group1;
            node2->incost+=dincost;
            node2->outcost+=doutcost;
            node2=node2->next;
            if(node2->level <= startlevel){
              break;
            }
          }
        }
      }
    }
  }
}


/* function: InitNetowrk()
 * -----------------------
 */
void InitNetwork(short **flows, long *ngroundarcsptr, long *ncycleptr, 
		 long *nflowdoneptr, long *mostflowptr, long *nflowptr, 
		 long *candidatebagsizeptr, candidateT **candidatebagptr, 
		 long *candidatelistsizeptr, candidateT **candidatelistptr, 
		 signed char ***iscandidateptr, nodeT ****apexesptr, 
		 bucketT **bktsptr, long *iincrcostfileptr, 
		 incrcostT ***incrcostsptr, nodeT ***nodesptr, nodeT *ground, 
		 long *nnoderowptr, short **nnodesperrowptr, long *narcrowptr,
		 short **narcsperrowptr, long nrow, long ncol, 
		 signed char *notfirstloopptr, totalcostT *totalcostptr,
		 paramT *params){

  long i;


  /* get and initialize memory for nodes */
  if(ground!=NULL && *nodesptr==NULL){
    *nodesptr=(nodeT **)Get2DMem(nrow-1,ncol-1,sizeof(nodeT *),sizeof(nodeT));
    InitNodeNums(nrow-1,ncol-1,*nodesptr,ground);
  }

  /* take care of ambiguous flows to ground at corners */
  if(ground!=NULL){
    flows[0][0]+=flows[nrow-1][0];
    flows[nrow-1][0]=0;
    flows[0][ncol-1]-=flows[nrow-1][ncol-2];
    flows[nrow-1][ncol-2]=0;
    flows[nrow-2][0]-=flows[2*nrow-2][0];
    flows[2*nrow-2][0]=0;
    flows[nrow-2][ncol-1]+=flows[2*nrow-2][ncol-2];
    flows[2*nrow-2][ncol-2]=0;
  }

  /* initialize network solver variables */
  *ncycleptr=0;
  *nflowptr=1;
  *candidatebagsizeptr=INITARRSIZE;
  *candidatebagptr=MAlloc(*candidatebagsizeptr*sizeof(candidateT));
  *candidatelistsizeptr=INITARRSIZE;
  *candidatelistptr=MAlloc(*candidatelistsizeptr*sizeof(candidateT));
  if(ground!=NULL){
    *nflowdoneptr=0;
    *mostflowptr=Short2DRowColAbsMax(flows,nrow,ncol);
    if(*mostflowptr*params->nshortcycle>LARGESHORT){
      fprintf(sp1,"Maximum flow on network: %ld\n",*mostflowptr);
      fprintf(sp0,"((Maximum flow) * NSHORTCYCLE) too large\nAbort\n");
      exit(ABNORMAL_EXIT);
    }
    if(ncol>2){
      *ngroundarcsptr=2*(nrow+ncol-2)-4; /* don't include corner column arcs */
    }else{
      *ngroundarcsptr=2*(nrow+ncol-2)-2;
    }
    *iscandidateptr=(signed char **)Get2DRowColMem(nrow,ncol,
						   sizeof(signed char *),
						   sizeof(signed char));
    *apexesptr=(nodeT ***)Get2DRowColMem(nrow,ncol,sizeof(nodeT **),
					 sizeof(nodeT *));
  }

  /* set up buckets for TreeSolve (MSTInitFlows() has local set of buckets) */
  *bktsptr=MAlloc(sizeof(bucketT));
  if(ground!=NULL){
    (*bktsptr)->minind=-LRound((params->maxcost+1)*(nrow+ncol)
			       *NEGBUCKETFRACTION);
    (*bktsptr)->maxind=LRound((params->maxcost+1)*(nrow+ncol)
			      *POSBUCKETFRACTION);
  }else{
    (*bktsptr)->minind=-LRound((params->maxcost+1)*(nrow)
			       *NEGBUCKETFRACTION);
    (*bktsptr)->maxind=LRound((params->maxcost+1)*(nrow)
			      *POSBUCKETFRACTION);
  }
  (*bktsptr)->size=(*bktsptr)->maxind-(*bktsptr)->minind+1;
  (*bktsptr)->bucketbase=(nodeT **)MAlloc((*bktsptr)->size*sizeof(nodeT *));
  (*bktsptr)->bucket=&((*bktsptr)->bucketbase[-(*bktsptr)->minind]);
  for(i=0;i<(*bktsptr)->size;i++){
    (*bktsptr)->bucketbase[i]=NULL;
  }

  /* get memory for incremental cost arrays */
  *iincrcostfileptr=0;
  if(ground!=NULL){
    (*incrcostsptr)=(incrcostT **)Get2DRowColMem(nrow,ncol,sizeof(incrcostT *),
						 sizeof(incrcostT));
  }

  /* set number of nodes and arcs per row */
  if(ground!=NULL){
    (*nnoderowptr)=nrow-1;
    (*nnodesperrowptr)=(short *)MAlloc((nrow-1)*sizeof(short));
    for(i=0;i<nrow-1;i++){
      (*nnodesperrowptr)[i]=ncol-1;
    }
    (*narcrowptr)=2*nrow-1;
    (*narcsperrowptr)=(short *)MAlloc((2*nrow-1)*sizeof(short));
    for(i=0;i<nrow-1;i++){
      (*narcsperrowptr)[i]=ncol;
    }
    for(i=nrow-1;i<2*nrow-1;i++){
      (*narcsperrowptr)[i]=ncol-1;
    }
  }

  /* initialize variables for main optimizer loop */
  (*notfirstloopptr)=FALSE;
  (*totalcostptr)=INITTOTALCOST;
}


/* function: InitNodeNums()
 * ------------------------
 */
void InitNodeNums(long nrow, long ncol, nodeT **nodes, nodeT *ground){

  long row, col;

  /* loop over each element and initialize values */
  for(row=0;row<nrow;row++){
    for(col=0;col<ncol;col++){