snaphu_solver.c

phase unwrapping algorithm for SAR interferometry

	    oldmntpt=to;

	    /* for each node on the path from to node to leaving child */
	    while(oldmntpt!=leavingparent){

	      /* remount the subtree at the new mount point */
	      mntpt=root;
	      root=oldmntpt;
	      oldmntpt=root->pred;
	      root->pred=mntpt;
	      GetArc(mntpt,root,&arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
	      
	      /* calculate differences for updating potentials and levels */
	      dlevel=mntpt->level-root->level+1;
	      doutcost=mntpt->outcost - root->outcost 
		+ GetCost(incrcosts,arcrow,arccol,arcdir);
	      dincost=mntpt->incost - root->incost 
		+ GetCost(incrcosts,arcrow,arccol,-arcdir);

	      /* update all children */
	      /* group of each remounted tree used to reset apexes below */
	      node1=root;
	      startlevel=root->level;
	      groupcounter++;
	      while(TRUE){
		
		/* update the level, potentials, and group of the node */
		node1->level+=dlevel;
		node1->outcost+=doutcost;
		node1->incost+=dincost;
		node1->group=groupcounter;
		
		/* break when node1 is no longer descendent of the root */
		if(node1->next->level <= startlevel){
		  break;
		}
		node1=node1->next;
	      }

	      /* update threads */
	      root->prev->next=node1->next;
	      node1->next->prev=root->prev;
	      node1->next=mntpt->next;  
	      mntpt->next->prev=node1;
	      mntpt->next=root;       
	      root->prev=mntpt;

	    }
	    skipthread=node1->next;

	    /* reset apex pointers for entering and leaving arcs */
	    GetArc(from,to,&arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);
	    apexes[arcrow][arccol]=NULL;
	    GetArc(leavingparent,leavingchild,&arcrow,&arccol,
		   &arcdir,nrow,ncol,nodesupp);
	    apexes[arcrow][arccol]=cycleapex;

	    /* make sure we have enough memory for the apex list */
	    if(groupcounter-apexlistbase+1>apexlistlen){
	      apexlistlen=1.5*(groupcounter-apexlistbase+1); 
	      apexlist=ReAlloc(apexlist,apexlistlen*sizeof(nodeT *));
	    }
        
	    /* set the apex list */
	    node2=leavingchild;
	    for(group1=groupcounter;group1>=apexlistbase;group1--){
	      apexlist[group1-apexlistbase]=node2;
	      node2=node2->pred;
	    }
        
	    /* reset apex pointers on remounted tree */
	    /* only nodes which are in different groups need new apexes */
	    node1=to;
	    startlevel=to->level;
	    while(TRUE){
	      
	      /* loop over outgoing arcs */
	      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 on tree */
		if(node2->group>0){

		
		  /* if node2 is either not part of remounted tree or */
		  /*   it is higher on remounted tree than node1, */
		  /*   and arc isn't already on tree */
		  if(node2->group < node1->group 
		     && apexes[arcrow][arccol]!=NULL){

		    /* if new apex in apexlist */
		    /* node2 on remounted tree, if nonaugmenting pivot */
		    if(node2->group >= apexlistbase){
		      
		      apexes[arcrow][arccol]=apexlist[node2->group
						     -apexlistbase];
		      
		    }else{
			
		      /* if old apex below level of cycleapex, */
		      /*   node2 is on "to" node's side of tree */
		      /* implicitly, if old apex above cycleapex, */
		      /*   we do nothing since apex won't change */
		      if(apexes[arcrow][arccol]->level > cycleapex->level){
		      
			/* since new apex not in apexlist (tested above), */
			/* node2 above leaving arc so new apex is cycleapex */
			apexes[arcrow][arccol]=cycleapex;
			
		      }else{

			/* node2 not on "to" side of tree */
			/* if old apex is cycleapex, node2 is on "from" side */
			if(apexes[arcrow][arccol]==cycleapex){

			  /* new apex will be on cycle, so trace node2->pred */
			  /*   until we hit a node with group==fromgroup */
			  tempnode2=node2;
			  while(tempnode2->group != fromgroup){
			    tempnode2=tempnode2->pred;
			  }
                          apexes[arcrow][arccol]=tempnode2;

			}
		      }
		    }

		    /* check outgoing arcs for negative reduced costs */
                    CheckArcReducedCost(node1,node2,apexes[arcrow][arccol],
					arcrow,arccol,arcdir,nflow,nodes,
					ground,&candidatebag,
					&candidatebagnext,&candidatebagsize,
					incrcosts,iscandidate,params);

		  } /* end if node2 below node1 and arc not on tree */

		}else{

		  /* node2 is not on tree, so put it in correct bucket */
		  AddNewNode(node1,node2,arcdir,bkts,nflow,incrcosts,
			     arcrow,arccol,params);

		} /* end if node2 on tree */
	      } /* end loop over node1 outgoing arcs */


	      /* move to next node in thread, break if we left the subtree */
	      node1=node1->next;
	      if(node1->level <= startlevel){
		break;
	      }
	    }
	  } /* end if leavingchild!=NULL */

	  /* if we had an augmenting cycle */
	  /* we need to check outarcs from descendents of any cycle node */
	  /* (except apex, since apex potentials don't change) */
	  if(cyclecost<0){
	    
	    /* check descendents of cycle children of apex */
	    while(TRUE){
	      
	      /* firstfromnode, firsttonode may have changed */
	      if(firstfromnode!=NULL && firstfromnode->pred==cycleapex){
		node1=firstfromnode;
		firstfromnode=NULL;
	      }else if(firsttonode!=NULL && firsttonode->pred==cycleapex){
		node1=firsttonode;
		firsttonode=NULL;
	      }else{
		break;
	      }
	      startlevel=node1->level;

	      /* loop over all descendents */
	      while(TRUE){
	      
		/* loop over outgoing arcs */
		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);

		  /* check for outcost updates or negative reduced costs */
		  if(node2->group>0){
		    if(apexes[arcrow][arccol]!=NULL 
		       && (node2->group!=node1->group 
			   || node1->group==apexlistbase)){
		      CheckArcReducedCost(node1,node2,apexes[arcrow][arccol],
					  arcrow,arccol,arcdir,nflow,nodes,
					  ground,&candidatebag,
					  &candidatebagnext,&candidatebagsize,
					  incrcosts,iscandidate,params);
		    }
		  }else{
		    AddNewNode(node1,node2,arcdir,bkts,nflow,incrcosts,
			       arcrow,arccol,params);
		  }			
		}
		
		/* move to next node in thread, break if left the subtree */
		/*   but skip the remounted tree, since we checked it above */
		node1=node1->next;
		if(node1==to){
		  node1=skipthread;
		}
		if(node1->level <= startlevel){
		  break;
		}
	      }
	    }
	  }
	  ipivots++;
	} /* end if cyclecost<0 || outcostto<to->outcost */
      } /* end of for loop over candidates in list */

      /* this is needed only if we don't process all candidates above */
      /* copy remaining candidates into candidatebag */
      /*
      while(candidatebagnext+(candidatelistlen-ncandidates)>candidatebagsize){
	candidatebagsize+=CANDIDATEBAGSTEP;
	candidatebag=ReAlloc(candidatebag,candidatebagsize*sizeof(candidateT));
      }
      for(i=ncandidates;i<candidatelistlen;i++){
	candidatebag[candidatebagnext++]=candidatelist[i];
      }
      */

      /* display status */
      fprintf(sp3,"\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
	      "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
	      "\b\b\b\b\b\b"
	      "Treesize: %-10ld Pivots: %-11ld Improvements: %-11ld",
	      treesize,ipivots,inondegen);
      fflush(sp3);

    } /* end of while loop on candidatebagnext */    
  } /* end while treesize<number of total nodes */

  
  /* clean up: set pointers for outputs */
  fprintf(sp3,"\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
	  "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b"
	  "\b\b\b\b\b\b"
	  "Treesize: %-10ld Pivots: %-11ld Improvements: %-11ld\n",
	  treesize,ipivots,inondegen);
  fflush(sp3);
  *candidatelistptr=candidatelist;
  *candidatebagptr=candidatebag;
  *candidatelistsizeptr=candidatelistsize;
  *candidatebagsizeptr=candidatebagsize;
  free(apexlist);

  /* return the number of nondegenerate pivots (number of improvements) */
  return(inondegen);

}


/* function: AddNewNode()
 * ----------------------
 * Adds a node to a bucket if it is not already in a bucket.  Updates 
 * outcosts of to node if the new distance is less or if to's pred is
 * from (then we have to do the update).
 */
void AddNewNode(nodeT *from, nodeT *to, long arcdir, bucketT *bkts, 
		long nflow, incrcostT **incrcosts, long arcrow, long arccol, 
		paramT *params){
  
  long newoutcost;

  newoutcost=from->outcost
    +GetCost(incrcosts,arcrow,arccol,arcdir);
  if(newoutcost<to->outcost || to->pred==from){
    if(to->group==-1){      /* if to is already in a bucket */
      if(to->outcost<bkts->maxind){
	if(to->outcost>bkts->minind){
	  BucketRemove(to,to->outcost,bkts);
	}else{
	  BucketRemove(to,bkts->minind,bkts);
	}
      }else{
	BucketRemove(to,bkts->maxind,bkts);
      }
    }      
    to->outcost=newoutcost;
    to->pred=from;
    if(newoutcost<bkts->maxind){
      if(newoutcost>bkts->minind){
	BucketInsert(to,newoutcost,bkts);
	if(newoutcost<bkts->curr){
	  bkts->curr=newoutcost;
	}
      }else{
	BucketInsert(to,bkts->minind,bkts);
	bkts->curr=bkts->minind;
      }
    }else{
      BucketInsert(to,bkts->maxind,bkts);
    }
    to->group=-1;
  }	  
}


/* function: CheckArcReducedCost()
 * -------------------------------
 * Given a from and to node, checks for negative reduced cost, and adds
 * the arc to the entering arc candidate bag if one is found.
 */
void CheckArcReducedCost(nodeT *from, nodeT *to, nodeT *apex, 
			 long arcrow, long arccol, long arcdir, 
			 long nflow, nodeT **nodes, nodeT *ground, 
			 candidateT **candidatebagptr, 
			 long *candidatebagnextptr, 
			 long *candidatebagsizeptr, incrcostT **incrcosts, 
			 signed char **iscandidate, paramT *params){

  long apexcost, fwdarcdist, revarcdist, violation;
  nodeT *temp;
  
  /* do nothing if already candidate */
  /* illegal corner arcs have iscandidate=TRUE set ahead of time */
  if(iscandidate[arcrow][arccol]){
    return;
  }

  /* set the apex cost */
  apexcost=apex->outcost+apex->incost;

  /* check forward arc */
  fwdarcdist=GetCost(incrcosts,arcrow,arccol,arcdir);
  violation=fwdarcdist+from->outcost+to->incost-apexcost;
  if(violation<0){
    arcdir*=2;  /* magnitude 2 for sorting */
  }else{
    revarcdist=GetCost(incrcosts,arcrow,arccol,-arcdir);
    violation=revarcdist+to->outcost+from->incost-apexcost;
    if(violation<0){
      arcdir*=-2;  /* magnitude 2 for sorting */
      temp=from;
      from=to;
      to=temp;
    }else{
      violation=fwdarcdist+from->outcost-to->outcost;
      if(violation>=0){
	violation=revarcdist+to->outcost-from->outcost;
	if(violation<0){
	  arcdir=-arcdir;
	  temp=from;
	  from=to;
	  to=temp;
	}
      }
    }
  }

  /* see if we have a violation, and if so, add arc to candidate bag */
  if(violation<0){
    if((*candidatebagnextptr)>=(*candidatebagsizeptr)){
      (*candidatebagsizeptr)+=CANDIDATEBAGSTEP;
      (*candidatebagptr)=ReAlloc(*candidatebagptr,
				 (*candidatebagsizeptr)*sizeof(candidateT));
    }
    (*candidatebagptr)[*candidatebagnextptr].violation=violation;
    (*candidatebagptr)[*candidatebagnextptr].from=from;
    (*candidatebagptr)[*candidatebagnextptr].to=to;
    (*candidatebagptr)[*candidatebagnextptr].arcrow=arcrow;
    (*candidatebagptr)[*candidatebagnextptr].arccol=arccol;
    (*candidatebagptr)[*candidatebagnextptr].arcdir=arcdir;
    (*candidatebagnextptr)++;
    iscandidate[arcrow][arccol]=TRUE;
  }

}


/* function: InitTree()
 * --------------------
 */
long InitTree(nodeT *source, nodeT **nodes, nodesuppT **nodesupp, 
	      nodeT *ground, long ngroundarcs, bucketT *bkts, long nflow, 
	      incrcostT **incrcosts, nodeT ***apexes, 
	      signed char **iscandidate, long nnoderow, short *nnodesperrow, 
	      long narcrow, short *narcsperrow, long nrow, long ncol, 
	      paramT *params){

  long row, col, arcnum, upperarcnum, arcrow, arccol, arcdir, nnodes;
  nodeT *to;


  /* loop over each node and initialize values */
  nnodes=0;
  for(row=0;row<nnoderow;row++){
    for(col=0;col<nnodesperrow[row];col++){
      nodes[row][col].group=0;
      nodes[row][col].outcost=VERYFAR;
      nodes[row][col].pred=NULL;
      nnodes++;
    }
  }

  /* initialize the ground node */
  if(ground!=NULL){
    ground->group=0;
    ground->outcost=VERYFAR;
    ground->pred=NULL;
    nnodes++;
  }

  /* initialize arcs */
  for(row=0;row<narcrow;row++){
    for(col=0;col<narcsperrow[row];col++){
      apexes[row][col]=NONTREEARC;
      iscandidate[row][col]=FALSE;
    }
  }

  /* if in grid mode, ground will exist */
  if(ground!=NULL){
    
    /* set iscandidate=TRUE for illegal corner arcs so they're never used */
    iscandidate[nrow-1][0]=TRUE;
    iscandidate[2*nrow-2][0]=TRUE;
    iscandidate[nrow-1][ncol-2]=TRUE;
    iscandidate[2*nrow-2][ncol-2]=TRUE;

  }

  /* put source on tree */
  source->group=1;
  source->outcost=0;
  source->incost=0;
  source->pred=NULL;
  source->prev=source;
  source->next=source;
  source->level=0;

  /* loop over outgoing arcs and add to buckets */
  if(source->row!=GROUNDROW){
    arcnum=-5;
    upperarcnum=-1;
  }else{
    arcnum=-1;
    upperarcnum=ngroundarcs-1;
  }
  while(arcnum<upperarcnum){
    
    /* get node reached by outgoing arc */
    to=NeighborNode(source,++arcnum,&upperarcnum,nodes,ground,
		    &arcrow,&arccol,&arcdir,nrow,ncol,nodesupp);

    /* add to node to bucket */
    AddNewNode(source,to,arcdir,bkts,nflow,incrcosts,arcrow,arccol,params);

  }