snaphu_cs2.c

phase unwrapping algorithm for SAR interferometry

	    rc = REDUCED_COST ( i, j, a );


	    if ( rc < 0 ) /* admissible arc */
	      {
		dr = ( - rc - 0.5 ) / epsilon;
		if (( j_rank = dr + i_rank ) < dlinf )
		  {
		    if ( j_rank > j -> rank )
		      j -> rank = j_rank;
		  }
	      }
	  }
      } /* all arcs from i are scanned */

    if ( i_rank > 0 )
      {
	if ( i_rank > bmax ) bmax = i_rank;
	b = buckets + i_rank;
	INSERT_TO_BUCKET ( i, b )
      }
  } /* end of while-cycle: all nodes are scanned
           - longest distancess are computed */


if ( bmax == 0 ) /* preflow is eps-optimal */
  { break; }

for ( b = buckets + bmax; b != buckets; b -- )
  {
    i_rank = b - buckets;
    dp     = (double)i_rank * epsilon;

    while ( NONEMPTY_BUCKET( b ) )
      {
	GET_FROM_BUCKET ( i, b );

	n_prscan++;
	FOR_ALL_ARCS_a_FROM_i 
	  {
	    if ( OPEN ( a ) )
	      {
		j = a -> head;
        	j_rank = j -> rank;
        	if ( j_rank < i_rank )
	          {
		    rc = REDUCED_COST ( i, j, a );
 
		    if ( rc < 0 ) 
		        j_new_rank = i_rank;
		    else
		      {
			dr = rc / epsilon;
			j_new_rank = ( dr < dlinf ) ? i_rank - ( (long)dr + 1 )
			                            : 0;
		      }
		    if ( j_rank < j_new_rank )
		      {
			if ( cc == 1 )
			  {
			    j -> rank = j_new_rank;

			    if ( j_rank > 0 )
			      {
				b_old = buckets + j_rank;
				REMOVE_FROM_BUCKET ( j, b_old )
				}

			    b_new = buckets + j_new_rank;
			    INSERT_TO_BUCKET ( j, b_new )  
			  }
			else
			  {
			   df = a -> r_cap;
			    INCREASE_FLOW ( i, j, a, df ) 
			  }
		      }
		  }
	      } /* end if opened arc */
	  } /* all arcs are scanned */

	    i -> price -= dp;

      } /* end of while-cycle: the bucket is scanned */
  } /* end of for-cycle: all buckets are scanned */

if ( cc == 0 ) break;

} /* end of main loop */

/* finish: */

/* if refine needed - saturate non-epsilon-optimal arcs */

if ( cc == 0 )
{ 
FOR_ALL_NODES_i 
  {
    FOR_ALL_ARCS_a_FROM_i 
      {
	if ( REDUCED_COST ( i, a -> head, a ) < -epsilon )
	  {
	    if ( ( df = a -> r_cap ) > 0 )
	      {
		INCREASE_FLOW ( i, a -> head, a, df )
	      }
	  }

      }
  }
}


/*neg_cyc();*/

return ( cc );

} /* end of price_refine */



void compute_prices ()

{

node   *i,              /* current node */
       *j;              /* opposite node */
arc    *a,              /* arc (i,j) */
       *a_stop;         /* first arc from the next node */

long   bmax;            /* number of farest nonempty bucket */
long   i_rank,          /* rank of node i */
       j_rank,          /* rank of node j */
       j_new_rank;      /* new rank of node j */
bucket *b,              /* current bucket */
       *b_old,          /* old and new buckets of current node */
       *b_new;
double rc,              /* reduced cost of a */
       dr,              /* ranks difference */
       dp;
int    cc;              /* return code: 1 - flow is epsilon optimal
                                        0 - refine is needed        */


n_prefine ++;

cc=1;

/* main loop */

while ( 1 )
{ /* while negative cycle is found or eps-optimal solution is constructed */


FOR_ALL_NODES_i 
  {
    i -> rank    = 0;
    i -> inp     = WHITE;
    i -> current = i -> first;
  }

RESET_STACKQ

FOR_ALL_NODES_i 
  {
    if ( i -> inp == BLACK ) continue;

    i -> b_next = NULL;

    /* deapth first search */
    while ( 1 )
      {
	i -> inp = GREY;

	/* scanning arcs from node i */
	FOR_ALL_ARCS_a_FROM_i 
	  {
	    if ( OPEN ( a ) )
	      {
		j = a -> head;
		if ( REDUCED_COST ( i, j, a ) < 0 )
		  {
		    if ( j -> inp == WHITE )
		      { /* fresh node  - step forward */
			i -> current = a;
			j -> b_next  = i;
			i = j;
			a = j -> current;
                        a_stop = (j+1) -> suspended;
			break;
		      }

		    if ( j -> inp == GREY )
		      { /* cycle detected; should not happen */
			cc = 0;
		      }                     
		  }
		/* if j-color is BLACK - continue search from i */
	      }
	  } /* all arcs from i are scanned */

	if ( a == a_stop )
	  {
	    /* step back */
	    i -> inp = BLACK;
	    n_prscan1++;
	    j = i -> b_next;
	    STACKQ_PUSH ( i );

	    if ( j == NULL ) break;
	    i = j;
	    i -> current ++;
	  }

      } /* end of deapth first search */
  } /* all nodes are scanned */

/* no negative cycle */
/* computing longest paths */

if ( cc == 0 ) break;

bmax = 0;

while ( NONEMPTY_STACKQ )
  {
    n_prscan2++;
    STACKQ_POP ( i );
    i_rank = i -> rank;
    FOR_ALL_ARCS_a_FROM_i 
      {
	if ( OPEN ( a ) )
	  {
	    j  = a -> head;
	    rc = REDUCED_COST ( i, j, a );


	    if ( rc < 0 ) /* admissible arc */
	      {
		dr = - rc;
		if (( j_rank = dr + i_rank ) < dlinf )
		  {
		    if ( j_rank > j -> rank )
		      j -> rank = j_rank;
		  }
	      }
	  }
      } /* all arcs from i are scanned */

    if ( i_rank > 0 )
      {
	if ( i_rank > bmax ) bmax = i_rank;
	b = buckets + i_rank;
	INSERT_TO_BUCKET ( i, b )
      }
  } /* end of while-cycle: all nodes are scanned
           - longest distancess are computed */


if ( bmax == 0 )
  { break; }

for ( b = buckets + bmax; b != buckets; b -- )
  {
    i_rank = b - buckets;
    dp     = (double) i_rank;

    while ( NONEMPTY_BUCKET( b ) )
      {
	GET_FROM_BUCKET ( i, b )

	  n_prscan++;
	FOR_ALL_ARCS_a_FROM_i 
	  {
	    if ( OPEN ( a ) )
	      {
		j = a -> head;
        	j_rank = j -> rank;
        	if ( j_rank < i_rank )
	          {
		    rc = REDUCED_COST ( i, j, a );
 
		    if ( rc < 0 ) 
		        j_new_rank = i_rank;
		    else
		      {
			dr = rc;
			j_new_rank = ( dr < dlinf ) ? i_rank - ( (long)dr + 1 )
			                            : 0;
		      }
		    if ( j_rank < j_new_rank )
		      {
			if ( cc == 1 )
			  {
			    j -> rank = j_new_rank;

			    if ( j_rank > 0 )
			      {
				b_old = buckets + j_rank;
				REMOVE_FROM_BUCKET ( j, b_old )
				}

			    b_new = buckets + j_new_rank;
			    INSERT_TO_BUCKET ( j, b_new )  
			  }
		      }
		  }
	      } /* end if opened arc */
	  } /* all arcs are scanned */

	    i -> price -= dp;

      } /* end of while-cycle: the bucket is scanned */
  } /* end of for-cycle: all buckets are scanned */

if ( cc == 0 ) break;

} /* end of main loop */

} /* end of compute_prices */


/***************************************************** price_out ************/

static void price_out ()

{
node     *i;                /* current node */

arc      *a,                /* current arc from i */
         *a_stop,           /* first arc from the next node */
         *b;                /* arc to be exchanged with suspended */

double   n_cut_off,         /* -cut_off */
         rc;                /* reduced cost */

n_cut_off = - cut_off;

FOR_ALL_NODES_i 
  {
    FOR_ALL_ARCS_a_FROM_i 
      {
	rc = REDUCED_COST ( i, a -> head, a );

	if (((rc > cut_off) && (CLOSED(a -> sister)))
             ||
             ((rc < n_cut_off) && (CLOSED(a)))
           )
	  { /* suspend the arc */
	    b = ( i -> first ) ++ ;

	    EXCHANGE ( a, b );
	  }
      }
  }

} /* end of price_out */


/**************************************************** update_epsilon *******/
/*----- decrease epsilon after epsilon-optimal flow is constructed */

static int update_epsilon()
{

if ( epsilon <= low_bound ) return ( 1 );

epsilon = ceil ( epsilon / f_scale );

cut_off        = cut_off_factor * epsilon;
cut_on         = cut_off * CUT_OFF_GAP;

return ( 0 );
}


/*************************************************** finishup ***********/
static void finishup ( obj_ad )

double *obj_ad;       /* objective */

{
arc   *a;            /* current arc */
long  na;            /* corresponding position in capacity array */
double  obj_internal;/* objective */
double cs;           /* actual arc cost */
long   flow;         /* flow through an arc */

obj_internal = 0;

for ( a = arcs, na = 0; a != sentinel_arc ; a ++, na ++ )
    {
      /*      cs = a -> cost / dn;  */
      cs = a -> cost;

      if ( cap[na]  > 0 && ( flow = cap[na] - (a -> r_cap) ) != 0 )
	obj_internal += cs * (double) flow; 

      /*       a -> cost = cs;  */
    }

*obj_ad = obj_internal;

}


/*********************************************** init_solution ***********/
/*  static void init_solution ( ) */


/*  { */
/*  arc   *a; */   /* current arc  (i,j) */ 
/*  node  *i, */   /* tail of  a  */ 
/*        *j; */   /* head of  a  */ 
/*  long  df; */   /* ricidual capacity */ 

/*  for ( a = arcs; a != sentinel_arc ; a ++ ) */
/*      { */
/*        if ( a -> r_cap > 0 && a -> cost < 0 ) */
/*  	{ */
/*  	  df = a -> r_cap; */
/*  	  i  = ( a -> sister ) -> head; */
/*            j  = a -> head; */
/*  	  INCREASE_FLOW ( i, j, a, df ); */
/*  	} */
/*      } */
/*  } */

  /* check complimentary slackness */ 
/*  int check_cs () */

/*  { */
/*    node *i; */
/*    arc *a, *a_stop; */

/*    FOR_ALL_NODES_i */
/*      FOR_ALL_ARCS_a_FROM_i */
/*        if (OPEN(a) && (REDUCED_COST(i, a->head, a) < 0)) */
/*  	assert(0); */

/*    return(1); */
/*  } */

/************************************************* cs2 - head program ***/

static void  cs2 ( n_p, m_p, nodes_p, arcs_p, f_sc, max_c, cap_p, obj_ad)

long    n_p,        /* number of nodes */
        m_p;        /* number of arcs */
node    *nodes_p;   /* array of nodes */
arc     *arcs_p;    /* array of arcs */
long    f_sc;       /* scaling factor */
double  max_c;      /* maximal cost */
short   *cap_p;     /* capacities (changed to short by CWC) */
double  *obj_ad;    /* objective */

{

int cc;             /* for storing return code */
cs_init ( n_p, m_p, nodes_p, arcs_p, f_sc, max_c, cap_p );

/*init_solution ( );*/
cc = 0;
update_epsilon ();

do{  /* scaling loop */

    refine ();

    if ( n_ref >= PRICE_OUT_START )
      {
	price_out ( );
      }

    if ( update_epsilon () ) break;

    while ( 1 )
      {
        if ( ! price_refine () ) break;

	if ( n_ref >= PRICE_OUT_START )
	  {
	    if ( price_in () ) 
	      { 
		break; 
	      }
	  }
	if ((cc = update_epsilon ())) break;
      }
  } while ( cc == 0 );

finishup ( obj_ad );

}

/*-----------------------------------------------------------------------*/

/* SolveCS2-- formerly main() */

void SolveCS2(signed char **residue, short **mstcosts, long nrow, long ncol, 
	      long cs2scalefactor, short ***flowsptr)
{

  /*  double t; */
  arc *arp;
  node *ndp;
  long n, m, m2, nmin; 
  node *i;
  long ni;
  arc *a;
  long nNrow, nNcol;
  long to, from, num, flow, ground;
  long f_sc;

  double cost,  c_max;
  short *cap;  /* cap changed to short by CWC */

  short **rowcost, **colcost;
  short **rowflow, **colflow;
  
  /* number of rows, cols, in residue network */
  nNrow=nrow-1;
  nNcol=ncol-1;
  ground=nNrow*nNcol+1;

  /* parse input, set up the problem */
  rowcost=mstcosts;
  colcost=&(mstcosts[nrow-1]);
  f_sc=cs2scalefactor;
  cs2mcfparse( residue,rowcost,colcost,nNrow,nNcol,
	       &n,&m,&ndp,&arp,&nmin,&c_max,&cap );

  /* free memory that is no longer needed */
  Free2DArray((void **)residue,nrow-1);
  Free2DArray((void **)mstcosts,2*nrow-1);

  /* solve it! */
  fprintf(sp2,"Running cs2 MCF solver\n");
  m2 = 2 * m;
  cs2 ( n, m2, ndp, arp, f_sc, c_max, cap, &cost );


  /* parse flow solution and place into flow arrays */
  
  /* get memory for flow arrays */
  (*flowsptr)=(short **)Get2DRowColZeroMem(nrow,ncol,
					   sizeof(short *),sizeof(short));
  rowflow=(*flowsptr);
  colflow=&((*flowsptr)[nrow-1]);

  /* loop over nodes */
  for ( i = ndp; i < ndp + n; i ++ ){
    ni = N_NODE ( i );

    /* loop over arcs */
    for ( a = i -> suspended; a != (i+1)->suspended; a ++ ){

      /* if finite (non-zero) flow */
      if ( cap[ N_ARC (a) ]  > 0 &&  (cap[ N_ARC (a) ] - ( a -> r_cap ) ) ){
	
	/* get to, from nodes and flow amount */
	from=ni;
	to=N_NODE( a -> head );
	flow=cap[ N_ARC (a) ] - ( a -> r_cap );
      
	if(flow>LARGESHORT || flow<-LARGESHORT){
	  fprintf(sp0,"Flow will overflow short data type\nAbort\n");
	  exit(ABNORMAL_EXIT);
	}

	if(from==(to+1)){    
	  num=from+(int )((from-1)/nNrow);
	  colflow[(num-1) % (nNrow+1)][(int )(num-1)/(nNrow+1)]-=flow;
	}else if(from==(to-1)){
	  num=from+(int )((from-1)/nNrow)+1;
	  colflow[(num-1) % (nNrow+1)][(int )(num-1)/(nNrow+1)]+=flow;
	}else if(from==(to-nNrow)){
	  num=from+nNrow;
	  rowflow[(num-1) % nNrow][(int )((num-1)/nNrow)]+=flow;
	}else if(from==(to+nNrow)){
	  num=from;
	  rowflow[(num-1) % nNrow][(int )((num-1)/nNrow)]-=flow;
	}else if((from==ground) || (to==ground)){
	  if(to==ground){
	    num=to;
	    to=from;
	    from=num;
	    flow=-flow;
	  }
	  if(!((to-1) % nNrow)){
	    colflow[0][(int )((to-1)/nNrow)]+=flow;
	  }else if(to<=nNrow){
	    rowflow[to-1][0]+=flow;
	  }else if(to>=(ground-nNrow-1)){
	    rowflow[(to-1) % nNrow][nNcol]-=flow;
	  }else if(!(to % nNrow)){
	    colflow[nNrow][(int )((to/nNrow)-1)]-=flow;
	  }else{
	    fprintf(sp0,"Unassigned ground arc parsing cs2 solution\nAbort\n");
	    exit(ABNORMAL_EXIT);
	  }        
	}else{
	  fprintf(sp0,"Non-grid arc parsing cs2 solution\nAbort\n");
	  exit(ABNORMAL_EXIT);
	}
      } /* end if flow on arc */

    } /* end for loop over arcs of node */
  } /* end for loop over nodes */

  /* free memory */
  free(ndp-nmin);  
  free(arp);
  free(cap);
  free(buckets);

}

#endif  /* end #ifndef NO_CS2 */