示例#1
0
// - merges 's1' with 's2' with a line fitting method
// - points are uniformly distributed on the line segments
// - number and weight of the points are calculated from the weight of the line segment
void carmen_linemapping_line_fitting_uniformly_distribute(carmen_linemapping_segment_t *s1, 
							  const carmen_linemapping_segment_t *s2)
{
  // get uniformly distributed point set
  carmen_linemapping_dyn_tab_point_t *points;
  points = carmen_linemapping_uniformly_distribute_points_on_segment(s1, s2);

  double x_av = 0.0, y_av = 0.0;
  for(int i=0; i<points->numberOfElements(); i++){
    carmen_point_t *p = points->getElement(i);
    x_av += p->x * p->theta;
    y_av += p->y * p->theta;
  }
  int weight_of_points = s1->weight + s2->weight;
  x_av = x_av / (double)weight_of_points;
  y_av = y_av / (double)weight_of_points;

  double numerator = 0.0, denominator = 0.0;
  for(int i=0; i<points->numberOfElements(); i++){
    carmen_point_t *p = points->getElement(i);
    numerator   += p->theta * (y_av - p->y) * (x_av - p->x);
    denominator += p->theta * ( carmen_square( (y_av - p->y) ) - carmen_square( (x_av - p->x) ) );
  }
  numerator *= -2.0;
  double angle    = atan2( numerator, denominator ) / 2.0;
  double distance = ( x_av*cos(angle) ) + ( y_av*sin(angle) );

  double min_x = MAXDOUBLE, max_x = -MAXDOUBLE, min_y = MAXDOUBLE, max_y = -MAXDOUBLE;
  const carmen_point_t* pnts[4] = {&s1->p1, &s1->p2, &s2->p1, &s2->p2};
  for(int i=0; i<=3; i++){
    const carmen_point_t *p = pnts[i];
    if( min_x > p->x ){ min_x = p->x; } if( max_x < p->x ){ max_x = p->x; }
    if( min_y > p->y ){ min_y = p->y; } if( max_y < p->y ){ max_y = p->y; }
  }

  if(angle<M_PI/4.0 && angle>-M_PI/4.0){
    s1->p1.x = ( distance - (min_y)*sin(angle) ) / cos(angle);
    s1->p2.x = ( distance - (max_y)*sin(angle) ) / cos(angle);
    s1->p1.y = min_y;
    s1->p2.y = max_y;
  }
  else{
    s1->p1.y = ( distance - (min_x)*cos(angle) ) / sin(angle);
    s1->p2.y = ( distance - (max_x)*cos(angle) ) / sin(angle);
    s1->p1.x = min_x;
    s1->p2.x = max_x;
  }
  s1->weight = weight_of_points;

  points->setAutoDelete(true);
  delete points;

}
示例#2
0
// fits the line segment 's' to the point set 'pnts[i_low] ... pnts[i_high]'
void carmen_linemapping_line_fitting(carmen_linemapping_segment_t *s, 
				     const carmen_linemapping_dyn_tab_point_t *pnts, 
				     int i_low, int i_high)
{
  int num_of_points = i_high - i_low + 1;

  double x_av = 0.0, y_av = 0.0;
  for(int i=i_low; i<=i_high; i++){
    x_av += pnts->getElement(i)->x;
    y_av += pnts->getElement(i)->y;
  }
  x_av = x_av/(double)num_of_points;
  y_av = y_av/(double)num_of_points;

  double numerator = 0.0, denominator = 0.0;
  double min_x = MAXDOUBLE, max_x = -MAXDOUBLE, min_y = MAXDOUBLE, max_y = -MAXDOUBLE;
  for(int i=i_low; i<=i_high; i++){
    numerator   += (y_av - pnts->getElement(i)->y) * (x_av - pnts->getElement(i)->x);
    denominator += carmen_square( (y_av - pnts->getElement(i)->y) ) 
      - carmen_square( (x_av - pnts->getElement(i)->x)  );
    if( min_x > pnts->getElement(i)->x ){ min_x = pnts->getElement(i)->x; }
    if( max_x < pnts->getElement(i)->x ){ max_x = pnts->getElement(i)->x; }
    if( min_y > pnts->getElement(i)->y ){ min_y = pnts->getElement(i)->y; }
    if( max_y < pnts->getElement(i)->y ){ max_y = pnts->getElement(i)->y; }
  }
  numerator *= -2.0;

  double angle    = atan2( numerator, denominator ) / 2.0;
  double distance = ( x_av*cos(angle) ) + ( y_av*sin(angle) );
  if(angle<M_PI/4.0 && angle>-M_PI/4.0){
    s->p1.x = ( distance - (min_y)*sin(angle) ) / cos(angle);
    s->p2.x = ( distance - (max_y)*sin(angle) ) / cos(angle);
    s->p1.y = min_y;
    s->p2.y = max_y;
  }
  else{
    s->p1.y = ( distance - (min_x)*cos(angle) ) / sin(angle);
    s->p2.y = ( distance - (max_x)*cos(angle) ) / sin(angle);
    s->p1.x = min_x;
    s->p2.x = max_x;
  }

}
示例#3
0
// calculates the  max. distance 'max_dist' of a point 'pnt[max_index]' of the point set 'pnts[i_low] ... pnts[i_high]'
// to the line segment with the endpoints 'p1' and 'p2'
void carmen_linemapping_get_max_dist_max_index(const carmen_point_t *p1, 
					       const carmen_point_t *p2, 
					       const carmen_linemapping_dyn_tab_point_t *pnts, 
					       int i_low, int i_high, double *max_dist, int *max_index)
{
  double dx = p2->x - p1->x;
  double dy = p2->y - p1->y;
  double denominator = carmen_square(dx) + carmen_square(dy);
  *max_dist = 0.0;
  if(denominator < carmen_square(carmen_linemapping_epsilon)){ // line segment is a point
    for(int i=i_low; i<=i_high; i++){
      double distance = carmen_linemapping_distance_point_point(p1, pnts->getElement(i));
      if(distance>*max_dist){ *max_dist = distance; *max_index = i; }
    }
  }
  else{
    for(int i=i_low; i<=i_high; i++){
      double numerator = carmen_square(dx*(pnts->getElement(i)->y - p2->y) - dy*(pnts->getElement(i)->x - p2->x));
      double distance = sqrt( numerator/denominator );
      if(distance>*max_dist){ *max_dist = distance; *max_index = i; }
    }
  }
}
示例#4
0
void art_draw_ellipse(art_buffer_p buffer, art_context_p context, int filled,
		      float x, float y, float x_var, 
		      float xy_cov, float y_var, float k)
{
  float poly_x[MAX_POLY_POINTS], poly_y[MAX_POLY_POINTS], l11, l21, l22;
  int i;
  
  l11 = sqrt(x_var);
  l21 = xy_cov / l11;
  l22 = sqrt(y_var - carmen_square(l21));
  for(i = 0; i < MAX_POLY_POINTS; i++) {
    double t = i / (float)MAX_POLY_POINTS * 2 * M_PI, xt, yt;
    xt = cos(t);    yt = sin(t);
    poly_x[i] = x + l11 * xt * k;
    poly_y[i] = y + (l21 * xt + l22 * yt) * k;
  }
  art_draw_poly(buffer, context, filled, poly_x, poly_y, MAX_POLY_POINTS, 1);
}
示例#5
0
static double
dist2(carmen_ackerman_path_point_t v, carmen_ackerman_path_point_t w)
{
	return (carmen_square(v.x - w.x) + carmen_square(v.y - w.y));
}
示例#6
0
void art_draw_gaussian(art_buffer_p buffer, art_context_p context,
		       float x, float y, float x_var, 
		       float xy_cov, float y_var, float k)
{
  float poly_x[MAX_POLY_POINTS], poly_y[MAX_POLY_POINTS];
  double len, discriminant, eigval1, eigval2,
    eigvec1x, eigvec1y, eigvec2x, eigvec2y;
  int i, ex1, ey1, ex2, ey2;
  
  /* check for special case of axis-aligned */
  if (fabs(xy_cov) < (fabs(x_var) + fabs(y_var) + 1e-4) * 1e-4) {
    eigval1 = x_var;
    eigval2 = y_var;
    eigvec1x = 1.;
    eigvec1y = 0.;
    eigvec2x = 0.;
    eigvec2y = 1.;
  }
  else {
    /* compute axes and scales of ellipse */
    discriminant = sqrt(4 * carmen_square(xy_cov) + carmen_square(x_var - y_var));
    eigval1 = .5 * (x_var + y_var - discriminant);
    eigval2 = .5 * (x_var + y_var + discriminant);
    eigvec1x = (x_var - y_var - discriminant) / (2.0 * xy_cov);
    eigvec1y = 1.0;
    eigvec2x = (x_var - y_var + discriminant) / (2.0 * xy_cov);
    eigvec2y = 1.0;
    /* normalize eigenvectors */
    len = sqrt(carmen_square(eigvec1x) + 1.0);
    eigvec1x /= len;
    eigvec1y /= len;
    len = sqrt(carmen_square(eigvec2x) + 1.0);
    eigvec2x /= len;
    eigvec2y /= len;
  }

  /* take square root of eigenvalues and scale -- once this is
     done, eigvecs are unit vectors along axes and eigvals are
     corresponding radii */
  if (eigval1 < 0 || eigval2 < 0) {
    art_draw_circle(buffer, context, 0, x, y, 1);
    return;
  }
  eigval1 = sqrt(eigval1) * k;
  eigval2 = sqrt(eigval2) * k;
  if(eigval1 < 1)
    eigval1 = 1;
  if(eigval2 < 1)
    eigval2 = 1;
  
  /* compute points around edge of ellipse */
  for (i = 0; i < MAX_POLY_POINTS; i++) {
    double theta = M_PI * (-1.0 + 2.0 * i / (float)MAX_POLY_POINTS);
    double xi = cos(theta) * eigval1;
    double yi = sin(theta) * eigval2;
    poly_x[i] = xi * eigvec1x + yi * eigvec2x + x;
    poly_y[i] = xi * eigvec1y + yi * eigvec2y + y;
  }

  /* finally we can draw it */
  art_draw_poly(buffer, context, 0, poly_x, poly_y, MAX_POLY_POINTS, 1);
  
  ex1 = x + eigval1 * eigvec1x;
  ey1 = y + eigval1 * eigvec1y;
  ex2 = x - eigval1 * eigvec1x;
  ey2 = y - eigval1 * eigvec1y;
  art_draw_line(buffer, context, ex1, ey1, ex2, ey2);
  ex1 = x + eigval2 * eigvec2x;
  ey1 = y + eigval2 * eigvec2y;
  ex2 = x - eigval2 * eigvec2x;
  ey2 = y - eigval2 * eigvec2y;
  art_draw_line(buffer, context, ex1, ey1, ex2, ey2);
}
示例#7
0
// euclidean distance of two points 'p1' and 'p2'
double carmen_linemapping_distance_point_point(const carmen_point_t *p1, 
					       const carmen_point_t *p2)
{
  return sqrt( carmen_square( p1->x - p2->x) + carmen_square( p1->y - p2->y) );
}
示例#8
0
// - merges 'segment' with the best candidate of 'set', but not the candidate 'set[index_no_element]'
// - merged segment is stored in 'segment', weight of best canditate is set to '0'
int carmen_linemapping_merge_segment(carmen_linemapping_segment_set_t *set, 
				     carmen_linemapping_segment_t *segment, 
				     int index_no_element)
{
  int best_merge_index = -1;
  double best_similar_angle = MAXDOUBLE;;
  for(int i=0; i<set->num_segs; i++){
    if(i==index_no_element || set->segs[i].weight==0 || segment->weight==0){ continue; }

    carmen_linemapping_segment_t *s_set = &(set->segs[i]);

    double dx = segment->p2.x - segment->p1.x;
    double dy = segment->p2.y - segment->p1.y;
    double denominator = carmen_square(dx) + carmen_square(dy);

    double numerator1 = carmen_square( dx*(s_set->p1.y - segment->p2.y) - dy*(s_set->p1.x - segment->p2.x) );
    double numerator2 = carmen_square( dx*(s_set->p2.y - segment->p2.y) - dy*(s_set->p2.x - segment->p2.x) );
    double dis_set_p1 = sqrt( numerator1/denominator );
    double dis_set_p2 = sqrt( numerator2/denominator );

    if( dis_set_p1 < carmen_linemapping_params_global.merge_max_dist && 
	dis_set_p2 < carmen_linemapping_params_global.merge_max_dist ) { 
      // 's_set' is close enough to the line 'segment' (not line segment!!!)
      int overlap_set_p1 = carmen_linemapping_overlap_point_linesegment(segment, &s_set->p1);
      int overlap_set_p2 = carmen_linemapping_overlap_point_linesegment(segment, &s_set->p2);
      
      if( overlap_set_p1 && overlap_set_p2 ){ 
	// case 1: both endpoints of 's_set' overlaps -> merge
	double a_dif = carmen_linemapping_angle_difference(segment, s_set);
	if( a_dif < best_similar_angle ){
	  best_similar_angle = a_dif;
	  best_merge_index = i;
	  continue;
	}
      }
      
      if( overlap_set_p1 || overlap_set_p2 ){ 
	// case 2: only one endpoint of 's_set' overlaps -> test merge
	double overlap_dist;
	carmen_point_t point = carmen_linemapping_get_point_on_segment(segment, s_set);
	if( overlap_set_p1 ){ 
	  overlap_dist = carmen_linemapping_distance_point_point(&s_set->p1, &point); 
	}
	else                { 
	  overlap_dist = carmen_linemapping_distance_point_point(&s_set->p2, &point); 
	}
	
	double line_size = carmen_linemapping_distance_point_point(&s_set->p1, &s_set->p2);
	if( (overlap_dist/line_size) > carmen_linemapping_params_global.merge_min_relative_overlap || 
	    overlap_dist > carmen_linemapping_params_global.merge_overlap_min_length ){
	  double a_dif = carmen_linemapping_angle_difference(segment, s_set);
	  if( a_dif < best_similar_angle ){
	    best_similar_angle = a_dif;
	    best_merge_index = i;
	    continue;
	  }
	}
      }
    }
    
    dx = s_set->p2.x - s_set->p1.x;
    dy = s_set->p2.y - s_set->p1.y;
    denominator = carmen_square(dx) + carmen_square(dy);

    numerator1 = carmen_square( dx*(segment->p1.y - s_set->p2.y) - dy*(segment->p1.x - s_set->p2.x) );
    numerator2 = carmen_square( dx*(segment->p2.y - s_set->p2.y) - dy*(segment->p2.x - s_set->p2.x) );
    double dis_segment_p1 = sqrt( numerator1/denominator );
    double dis_segment_p2 = sqrt( numerator2/denominator );

    if( dis_segment_p1 < carmen_linemapping_params_global.merge_max_dist && 
	dis_segment_p2 < carmen_linemapping_params_global.merge_max_dist ){ 
      // 'segment' is close enough to the line 's_set' (not line segment!!!)
      int overlap_segment_p1 = carmen_linemapping_overlap_point_linesegment(s_set, &segment->p1);
      int overlap_segment_p2 = carmen_linemapping_overlap_point_linesegment(s_set, &segment->p2);

      if(overlap_segment_p1 && overlap_segment_p2){ // case 3: both endpoints of 's_old' overlaps -> merge
	double a_dif = carmen_linemapping_angle_difference(segment, s_set);
	if( a_dif < best_similar_angle ){
	  best_similar_angle = a_dif;
	  best_merge_index = i;
	  continue;
	}
      }

      if(overlap_segment_p1 || overlap_segment_p2){ // case 4: only one endpoint of 'segment' overlaps -> test merge
	double overlap_dist;
	carmen_point_t point = carmen_linemapping_get_point_on_segment(s_set, segment);
	if( overlap_segment_p1 ){ 
	  overlap_dist = carmen_linemapping_distance_point_point(&segment->p1, &point); 
	}
	else                    { 
	  overlap_dist = carmen_linemapping_distance_point_point(&segment->p2, &point); 
	}

	double line_size = carmen_linemapping_distance_point_point(&segment->p1, &segment->p2);
	if( (overlap_dist/line_size) > carmen_linemapping_params_global.merge_min_relative_overlap || 
	    overlap_dist > carmen_linemapping_params_global.merge_overlap_min_length ){
	  double a_dif = carmen_linemapping_angle_difference(segment, s_set);
	  if( a_dif < best_similar_angle ){
	    best_similar_angle = a_dif;
	    best_merge_index = i;
	    continue;
	  }
	}
      }
    }
  }
  
  if (best_merge_index>=0){ 
    // merge the best candidate with 'segment'
    carmen_linemapping_line_fitting_uniformly_distribute(segment, &(set->segs[best_merge_index]));
    set->segs[best_merge_index].weight = 0;
    return true;
  }
  else
    return false; 

}