Esempio n. 1
0
/* computes current energy */
int Match::BVZ_ComputeEnergy()
{
	int k;
	Coord p, d, q, dq;

	E = 0;

	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));

		E += BVZ_data_penalty(p, d);

		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];

			if (q>=Coord(0,0) && q<im_size)
			{
				dq = Coord(IMREF(x_left, q), IMREF(y_left, q));
				E += BVZ_smoothness_penalty(p, q, d, dq);
			}
		}
	}

	return E;
}
int Match::data_penalty_GRAY(Coord l, Coord r)
{
	int d;

	d = IMREF(im_left, l) - IMREF(im_right, r);
	if (params.data_cost==Parameters::L1) { if (d<0) d = -d; } else d = d*d;
	if (d>CUTOFF) d = CUTOFF;

	return d;
}
int Match::smoothness_penalty2_GRAY(Coord p, Coord np, Coord disp)
{
	int dl, dr;

	dl = IMREF(segm_left, p) - IMREF(segm_left, np);   
	dr = IMREF(segm_right, p+disp) - IMREF(segm_right, np+disp); 

	if (dl<0) dl = -dl; if (dr<0) dr = -dr;

	if (dl<params.I_threshold2 && dr<params.I_threshold2) return params.lambda1;
	else                                                  return params.lambda2;
}
Esempio n. 4
0
int Match::smoothness_penalty_right_GRAY(Coord p, Coord np, Coord disp, Coord ndisp)
{
	int d;

	if (disp == ndisp) return 0;

	d = IMREF(segm_right, p) - IMREF(segm_right, np);
	if (d<0) d = -d;

	if (d<params.I_threshold) return params.lambda1;
	else                      return params.lambda2;
}
Esempio n. 5
0
void PortraitCut::saveComp(char* name) {
  RGBImage im = (RGBImage) imNew(IMAGE_RGB, _w, _h);
    int i,  j,index=0;
  for (j=0; j<_h; j++)
    for (i=0; i<_w; i++, ++index) { 
      Coord p(i,j);
      unsigned char* cref = _imptr(_labels[index], p);
      IMREF(im,p).r = cref[0];
      IMREF(im,p).g = cref[1];
      IMREF(im,p).b = cref[2];
    }

  int res = imSave(im,name);
  assert(res==0);
  imFree(im);
}
int Match::smoothness_penalty_right_GRAY(Coord p, Coord np, Coord disp, Coord ndisp)
{
	int d, R;

	if (disp == ndisp) return 0;
	if (disp.x == OCCLUDED || ndisp.x == OCCLUDED) R = params.interaction_radius;
	else
	{
		int Rx = disp.x - ndisp.x; if (Rx < 0) Rx = -Rx;
		int Ry = disp.y - ndisp.y; if (Ry < 0) Ry = -Ry;
		R = Rx + Ry;
		if (R > params.interaction_radius) R = params.interaction_radius;
	}

	d = IMREF(segm_right, p) - IMREF(segm_right, np);
	if (d<0) d = -d;

	if (d<params.I_threshold) return R*params.lambda1;
	else                      return R*params.lambda2;
}
int Match::smoothness_penalty_right_COLOR(Coord p, Coord np, Coord disp, Coord ndisp)
{
	int d, d_max, R;

	if (disp == ndisp) return 0;
	if (disp.x == OCCLUDED || ndisp.x == OCCLUDED) R = params.interaction_radius;
	else
	{
		int Rx = disp.x - ndisp.x; if (Rx < 0) Rx = -Rx;
		int Ry = disp.y - ndisp.y; if (Ry < 0) Ry = -Ry;
		R = Rx + Ry;
		if (R > params.interaction_radius) R = params.interaction_radius;
	}

	d_max = 0;

	/* red component */
	d = IMREF(segm_color_right, p).r - IMREF(segm_color_right, np).r;
	if (d<0) d = -d; if (d_max<d) d_max = d;

	/* green component */
	d = IMREF(segm_color_right, p).g - IMREF(segm_color_right, np).g;
	if (d<0) d = -d; if (d_max<d) d_max = d;

	/* blue component */
	d = IMREF(segm_color_right, p).b - IMREF(segm_color_right, np).b;
	if (d<0) d = -d; if (d_max<d) d_max = d;

	if (d_max<params.I_threshold) return R*params.lambda1;
	else                          return R*params.lambda2;
}
Esempio n. 8
0
void PortraitCut::saveLabels(const char* name) {
  fprintf(_fp, "Trying to save file %s\n",name); fflush(_fp);
  GrayImage im = (GrayImage) imNew(IMAGE_GRAY, _w, _h);
  int i,  j,index=0;
  for (j=0; j<_h; j++)
    for (i=0; i<_w; i++, ++index) { 
      Coord p(i,j);
      IMREF(im,p) = (unsigned char) _labels[index];
    }

  imSave(im,name);
  imFree(im);
}
int Match::data_penalty_SUBPIXEL_GRAY(Coord l, Coord r)
{
	int dl, dr, d;
	int Il, Il_min, Il_max, Ir, Ir_min, Ir_max;

	Il     = IMREF(im_left,     l); Ir     = IMREF(im_right,     r);
	Il_min = IMREF(im_left_min, l); Ir_min = IMREF(im_right_min, r);
	Il_max = IMREF(im_left_max, l); Ir_max = IMREF(im_right_max, r);

	if      (Il < Ir_min) dl = Ir_min - Il;
	else if (Il > Ir_max) dl = Il - Ir_max;
	else return 0;

	if      (Ir < Il_min) dr = Il_min - Ir;
	else if (Ir > Il_max) dr = Ir - Il_max;
	else return 0;

	d = MIN(dl, dr); if (params.data_cost==Parameters::L2) d = d*d;
	if (d>CUTOFF) d = CUTOFF;

	return d;
}
Esempio n. 10
0
int Match::smoothness_penalty_right_COLOR(Coord p, Coord np, Coord disp, Coord ndisp)
{
	int d, d_max;

	if (disp == ndisp) return 0;

	d_max = 0;

	/* red component */
	d = IMREF(segm_color_right, p).r - IMREF(segm_color_right, np).r;
	if (d<0) d = -d; if (d_max<d) d_max = d;

	/* green component */
	d = IMREF(segm_color_right, p).g - IMREF(segm_color_right, np).g;
	if (d<0) d = -d; if (d_max<d) d_max = d;

	/* blue component */
	d = IMREF(segm_color_right, p).b - IMREF(segm_color_right, np).b;
	if (d<0) d = -d; if (d_max<d) d_max = d;

	if (d_max<params.I_threshold) return params.lambda1;
	else                          return params.lambda2;
}
int Match::data_penalty_COLOR(Coord l, Coord r)
{
	int d, d_sum = 0;

	/* red component */
	d = IMREF(im_color_left, l).r - IMREF(im_color_right, r).r;
	if (params.data_cost==Parameters::L1) { if (d<0) d = -d; } else d = d*d;
	if (d>CUTOFF) d = CUTOFF;
	d_sum += d;

	/* green component */
	d = IMREF(im_color_left, l).g - IMREF(im_color_right, r).g;
	if (params.data_cost==Parameters::L1) { if (d<0) d = -d; } else d = d*d;
	if (d>CUTOFF) d = CUTOFF;
	d_sum += d;

	/* blue component */
	d = IMREF(im_color_left, l).b - IMREF(im_color_right, r).b;
	if (params.data_cost==Parameters::L1) { if (d<0) d = -d; } else d = d*d;
	if (d>CUTOFF) d = CUTOFF;
	d_sum += d;

	return d_sum/3;
}
int Match::data_penalty_SUBPIXEL_COLOR(Coord l, Coord r)
{
	int dl, dr, d, d_sum = 0;
	int Il, Il_min, Il_max, Ir, Ir_min, Ir_max;

	/* red component */
	Il     = IMREF(im_color_left,     l).r; Ir     = IMREF(im_color_right,     r).r;
	Il_min = IMREF(im_color_left_min, l).r; Ir_min = IMREF(im_color_right_min, r).r;
	Il_max = IMREF(im_color_left_max, l).r; Ir_max = IMREF(im_color_right_max, r).r;

	if      (Il < Ir_min) dl = Ir_min - Il;
	else if (Il > Ir_max) dl = Il - Ir_max;
	else dl = 0;

	if      (Ir < Il_min) dr = Il_min - Ir;
	else if (Ir > Il_max) dr = Ir - Il_max;
	else dr = 0;

	d = MIN(dl, dr); if (params.data_cost==Parameters::L2) d = d*d;
	if (d>CUTOFF) d = CUTOFF;
	d_sum += d;

	/* green component */
	Il     = IMREF(im_color_left,     l).g; Ir     = IMREF(im_color_right,     r).g;
	Il_min = IMREF(im_color_left_min, l).g; Ir_min = IMREF(im_color_right_min, r).g;
	Il_max = IMREF(im_color_left_max, l).g; Ir_max = IMREF(im_color_right_max, r).g;

	if      (Il < Ir_min) dl = Ir_min - Il;
	else if (Il > Ir_max) dl = Il - Ir_max;
	else dl = 0;

	if      (Ir < Il_min) dr = Il_min - Ir;
	else if (Ir > Il_max) dr = Ir - Il_max;
	else dr = 0;

	d = MIN(dl, dr); if (params.data_cost==Parameters::L2) d = d*d;
	if (d>CUTOFF) d = CUTOFF;
	d_sum += d;

	/* blue component */
	Il     = IMREF(im_color_left,     l).b; Ir     = IMREF(im_color_right,     r).b;
	Il_min = IMREF(im_color_left_min, l).b; Ir_min = IMREF(im_color_right_min, r).b;
	Il_max = IMREF(im_color_left_max, l).b; Ir_max = IMREF(im_color_right_max, r).b;

	if      (Il < Ir_min) dl = Ir_min - Il;
	else if (Il > Ir_max) dl = Il - Ir_max;
	else dl = 0;

	if      (Ir < Il_min) dr = Il_min - Ir;
	else if (Ir > Il_max) dr = Ir - Il_max;
	else dr = 0;

	d = MIN(dl, dr); if (params.data_cost==Parameters::L2) d = d*d;
	if (d>CUTOFF) d = CUTOFF;
	d_sum += d;

	return d_sum/3;
}
Esempio n. 13
0
void Match::BVZ_Expand(Coord a)
{
	Coord p, d, q, dq;
	Energy::Var var, qvar;
	int E_old, E00, E0a, Ea0;
	int k;

	/* node_vars stores variables corresponding to nodes */

	Energy *e = new Energy(BVZ_error_function);

	/* initializing */
	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		if (a == d)
		{
			IMREF(node_vars, p) = VAR_ACTIVE;
			e -> add_constant(BVZ_data_penalty(p, d));
		}
		else
		{
			IMREF(node_vars, p) = var = e -> add_variable();
			e -> ADD_TERM1(var, BVZ_data_penalty(p, d), BVZ_data_penalty(p, a));
		}
	}

	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		var = (Energy::Var) IMREF(node_vars, p);

		/* smoothness term */
		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];
			if ( ! ( q>=Coord(0,0) && q<im_size ) ) continue;
			qvar = (Energy::Var) IMREF(node_vars, q);
			dq = Coord(IMREF(x_left, q), IMREF(y_left, q));

			if (var != VAR_ACTIVE && qvar != VAR_ACTIVE)
				E00 = BVZ_smoothness_penalty(p, q, d, dq);
			if (var != VAR_ACTIVE)
				E0a = BVZ_smoothness_penalty(p, q, d, a);
			if (qvar != VAR_ACTIVE)
				Ea0 = BVZ_smoothness_penalty(p, q, a, dq);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, E00, E0a, Ea0, 0);
				else                    e -> ADD_TERM1(var, E0a, 0);
			}
			else
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM1(qvar, Ea0, 0);
				else                    {}
			}
			
		}
	}

	E_old = E;
	E = e -> minimize();

	if (E < E_old)
	{
		for (p.y=0; p.y<im_size.y; p.y++)
		for (p.x=0; p.x<im_size.x; p.x++)
		{
			var = (Energy::Var) IMREF(node_vars, p);
			if (var!=VAR_ACTIVE && e->get_var(var)==VALUE1)
			{
				IMREF(x_left, p) = a.x; IMREF(y_left, p) = a.y;
			}
		}
	}

	delete e;
}
Esempio n. 14
0
double PortraitCut::BVZ_Expand(ushort a, double E_old) { 

  
  Coord p, np;
  ushort l, nl;
  Graph *g;
  float delta, P_00, P_0a, P_a0;
  Graph::node_id index, nindex;
  int k, ind=0;
  double E;
  
  /* indeces_a stores node indeces */
  /* D_a sto res penalties for assigning label a */
  
  g = new Graph(BVZ_error_function);
  
  /* initializing */
  E = 0.;
  for (p.y=0; p.y<_h; p.y++)
    for (p.x=0; p.x<_w; p.x++, ++ind)
      {
		l = _labels[ind];

		if (a == l) // 和原始的label一样
		  {
			IMREF(indeces_a, p) = INDEX_ACTIVE; //返回点p在graph中的节点索引,默认为0
			E += BVZ_data_penalty(p, l);
			continue;
		  }

		// 	而void *则不同,任何类型的指针都可以直接赋值给它,无需进行强制类型转换://void *p1; //int *p2; 		// 	p1 = p2	
		
		//label 不一样的话

		IMREF(indeces_a, p) = g -> add_node();
		delta = BVZ_data_penalty(p, l);
		IMREF(D_a, p) = BVZ_data_penalty(p, a) - delta;
		E += delta;
      }
  
  ind=0;
  for (p.y=0; p.y<_h; p.y++)
    for (p.x=0; p.x<_w; p.x++, ++ind)
      {
		  l = _labels[ind];
		  index = (Graph::node_id) IMREF(indeces_a, p);
	
			/* adding interactions */
			for (k=0; k<(int)NEIGHBOR_NUM; k++)
			  {
				np = p + NEIGHBORS[k];
				if ( ! ( np>=Coord(0,0) && np<_size ) ) continue; //HUM
				nl = _labels[CINDEX(np)];
				nindex = (Graph::node_id) IMREF(indeces_a, np);//获取在graph 中node的id
	    
				if (IS_NODE_ID(index))
				  {
						if (IS_NODE_ID(nindex))
						  {
							P_00 = BVZ_interaction_penalty(p, np, l, nl);
							P_0a = BVZ_interaction_penalty(p, np, l,  a);
							P_a0 = BVZ_interaction_penalty(p, np, a, nl);
							delta = (P_00 <  P_0a) ? P_00 : P_0a;
 							if (delta > 0)
							  {
								IMREF(D_a, p) -= delta; E += delta;
								P_00 -= delta;
								P_0a -= delta;
							  }
							delta = (P_00 < P_a0) ? P_00 : P_a0;
							if (delta > 0)
							  {
								  IMREF(D_a, np) -= delta; E += delta;
								  P_00 -= delta;
								  P_a0 -= delta;
							  }
							if (P_00 > 0.0001) { fprintf(_fp, "ERROR: BVZ_interaction_penalty() is non-metric %f!\n",P_00); fflush(_fp); /*assert(0);*/ }
							#ifdef BVZ_ALPHA_SINK
										g -> add_edge(index, nindex, P_0a, P_a0);
							#else
										g -> add_edge(index, nindex, P_a0, P_0a);
							#endif
						 }
						else
						  {
							delta = BVZ_interaction_penalty(p, np, l, a);
							IMREF(D_a, p) -= delta; E += delta;
						  }
				  }
				else
				  {
					if (IS_NODE_ID(nindex))
					  {
						delta = BVZ_interaction_penalty(p, np, a, nl);
						IMREF(D_a, np) -= delta; E += delta;
					  }
				  }
			  }// end for neighbor
     }// end for image-pixel
  
  
  /* adding source and sink edges */
  for (p.y=0; p.y<_h; p.y++)
    for (p.x=0; p.x<_w; p.x++)
      {
		index = (Graph::node_id) IMREF(indeces_a, p);
		if (IS_NODE_ID(index))
		  {
			delta = (float) IMREF(D_a, p);
			#ifdef BVZ_ALPHA_SINK
						if (delta > 0) { g -> set_tweights(index, delta, 0); }
						else           { g -> set_tweights(index, 0, -delta); E += delta; }
			#else
						if (delta > 0) { g -> set_tweights(index, 0, delta); }
						else           { g -> set_tweights(index, -delta, 0); E += delta; }
			#endif
		  }
      }
  
  E += g -> maxflow();

  //fprintf(_fp, "internal E: %f\n",E); fflush(_fp);
    if (E < E_old)
	{
		//fprintf(_fp,"Writing into _labels\n"); fflush(_fp);
		ind=0;
		for (p.y=0; p.y<_h; p.y++)
	      for (p.x=0; p.x<_w; p.x++, ++ind)
		    {
				index = (Graph::node_id) IMREF(indeces_a, p);
				if (IS_NODE_ID(index) && g->what_segment(index)==BVZ_TERM_B)
				{
					_labels[ind] = a; //进行交换操作
				}
		   }

		delete g;
		return E;
	}
  
  delete g;
  return E_old;
}
Esempio n. 15
0
/* computes current energy */
int Match::KZ1_ComputeEnergy()
{
	int k;
	Coord p, d, q, dq;

	E = 0;

	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		/* left image and data penalty */
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));

		if (d.x == OCCLUDED) E += KZ1_OCCLUSION_PENALTY;
		else
		{
			q = p + d;
			if (q>=Coord(0,0) && q<im_size)
			{
				dq = Coord(IMREF(x_right, q), IMREF(y_right, q));
				if (d == -dq)
				{
					E += KZ1_data_penalty(p, q);
				}
#ifndef NDEBUG
				/* check visibility constaint */
				if (dq.x != OCCLUDED && is_blocked(-dq, d))
				{
					fprintf(stderr, "KZ1: Visibility constraint is violated!\n");
					exit(1);
				}
#endif
			}
		}

		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];

			if (q>=Coord(0,0) && q<im_size)
			{
				dq = Coord(IMREF(x_left, q), IMREF(y_left, q));
				E += KZ1_smoothness_penalty_left(p, q, d, dq);
			}
		}


		/* right image */
		d = Coord(IMREF(x_right, p), IMREF(y_right, p));

		if (d.x == OCCLUDED) E += KZ1_OCCLUSION_PENALTY;
#ifndef NDEBUG
		else
		{
			/* check visibility constaint */
			q = p + d;
			if (q>=Coord(0,0) && q<im_size)
			{
				dq = Coord(IMREF(x_left, q), IMREF(y_left, q));
				if (dq.x != OCCLUDED && is_blocked(dq, -d))
				{
					fprintf(stderr, "KZ1: Visibility constraint is violated!\n");
					exit(1);
				}
			}
		}
#endif

		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];

			if (q>=Coord(0,0) && q<im_size)
			{
				dq = Coord(IMREF(x_right, q), IMREF(y_right, q));
				E += KZ1_smoothness_penalty_right(p, q, d, dq);
			}
		}
	}

	return E;
}
Esempio n. 16
0
/* computes the minimum a-expansion configuration */
void Match::KZ1_Expand(Coord a)
{
	Coord p, q, d, dq;
	Energy::Var var, qvar;
	int E_old, delta, E00, E0a, Ea0, Eaa;
	int k;

	Energy *e = new Energy(KZ1_error_function);

	/* initializing */
	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		if (a == d) IMREF(node_vars_left, p) = VAR_ACTIVE;
		else
		{
			IMREF(node_vars_left, p) = var = e -> add_variable();
			if (d.x == OCCLUDED) e -> ADD_TERM1(var, KZ1_OCCLUSION_PENALTY, 0);
		}

		d = Coord(IMREF(x_right, p), IMREF(y_right, p));
		if (a == -d) IMREF(node_vars_right, p) = VAR_ACTIVE;
		else
		{
			IMREF(node_vars_right, p) = var = e -> add_variable();
			if (d.x == OCCLUDED) e -> ADD_TERM1(var, KZ1_OCCLUSION_PENALTY, 0);
		}
	}

	for (p.y=0; p.y<im_size.y; p.y++)
	for (p.x=0; p.x<im_size.x; p.x++)
	{
		/* data and visibility terms */
		d = Coord(IMREF(x_left, p), IMREF(y_left, p));
		var = (Energy::Var) IMREF(node_vars_left, p);
		if (d != a && d.x != OCCLUDED)
		{
			q = p + d;
			if (q>=Coord(0,0) && q<im_size)
			{
				qvar = (Energy::Var) IMREF(node_vars_right, q);
				dq = Coord(IMREF(x_right, q), IMREF(y_right, q));
				if (d == -dq)
				{
					delta = (is_blocked(a, d)) ? INFINITY : 0;
					e -> ADD_TERM2(var, qvar, KZ1_data_penalty(p, q), delta, delta, 0);
				}
				else if (is_blocked(a, d))
				{
					e -> ADD_TERM2(var, qvar, 0, INFINITY, 0, 0);
				}
			}
		}

		q = p + a;
		if (q>=Coord(0,0) && q<im_size)
		{
			qvar = (Energy::Var) IMREF(node_vars_right, q);
			dq = Coord(IMREF(x_right, q), IMREF(y_right, q));

			E0a = (is_blocked(d, a)) ? INFINITY : 0;
			Ea0 = (is_blocked(-dq, a)) ? INFINITY : 0;
			Eaa = KZ1_data_penalty(p, q);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, 0, E0a, Ea0, Eaa);
				else                    e -> ADD_TERM1(var, E0a, Eaa);
			}
			else
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM1(qvar, Ea0, Eaa);
				else                    e -> add_constant(Eaa);
			}
		}

		/* left smoothness term */
		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];
			if ( ! ( q>=Coord(0,0) && q<im_size ) ) continue;
			qvar = (Energy::Var) IMREF(node_vars_left, q);
			dq = Coord(IMREF(x_left, q), IMREF(y_left, q));

			if (var != VAR_ACTIVE && qvar != VAR_ACTIVE)
				E00 = KZ1_smoothness_penalty_left(p, q, d, dq);
			if (var != VAR_ACTIVE)
				E0a = KZ1_smoothness_penalty_left(p, q, d, a);
			if (qvar != VAR_ACTIVE)
				Ea0 = KZ1_smoothness_penalty_left(p, q, a, dq);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, E00, E0a, Ea0, 0);
				else                    e -> ADD_TERM1(var, E0a, 0);
			}
			else
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM1(qvar, Ea0, 0);
				else                    {}
			}
		}

		/* right smoothness term */
		d = Coord(IMREF(x_right, p), IMREF(y_right, p));
		var = (Energy::Var) IMREF(node_vars_right, p);
		for (k=0; k<NEIGHBOR_NUM; k++)
		{
			q = p + NEIGHBORS[k];
			if ( ! ( q>=Coord(0,0) && q<im_size ) ) continue;
			qvar = (Energy::Var) IMREF(node_vars_right, q);
			dq = Coord(IMREF(x_right, q), IMREF(y_right, q));

			if (var != VAR_ACTIVE && qvar != VAR_ACTIVE)
				E00 = KZ1_smoothness_penalty_right(p, q, d, dq);
			if (var != VAR_ACTIVE)
				E0a = KZ1_smoothness_penalty_right(p, q, d, -a);
			if (qvar != VAR_ACTIVE)
				Ea0 = KZ1_smoothness_penalty_right(p, q, -a, dq);

			if (var != VAR_ACTIVE)
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM2(var, qvar, E00, E0a, Ea0, 0);
				else                    e -> ADD_TERM1(var, E0a, 0);
			}
			else
			{
				if (qvar != VAR_ACTIVE) e -> ADD_TERM1(qvar, Ea0, 0);
				else                    {}
			}
		}

		/* visibility term */
		if (d.x != OCCLUDED && is_blocked(a, -d))
		{
			q = p + d;
			if (q>=Coord(0,0) && q<im_size)
			{
				if (d.x != -IMREF(x_left, q) || d.y != -IMREF(y_left, q))
					e -> ADD_TERM2(var, (Energy::Var) IMREF(node_vars_left, q),
					               0, INFINITY, 0, 0);
			}
		}
	}

	E_old = E;
	E = e -> minimize();

	if (E < E_old)
	{
		for (p.y=0; p.y<im_size.y; p.y++)
		for (p.x=0; p.x<im_size.x; p.x++)
		{
			var = (Energy::Var) IMREF(node_vars_left, p);

			if (var != VAR_ACTIVE && e->get_var(var)==VALUE1)
			{
				IMREF(x_left, p) = a.x; IMREF(y_left, p) = a.y;
			}

			var = (Energy::Var) IMREF(node_vars_right, p);

			if (var != VAR_ACTIVE && e->get_var(var)==VALUE1)
			{
				IMREF(x_right, p) = -a.x; IMREF(y_right, p) = -a.y;
			}
		}
	}

	delete e;
}