utils.c.svn-base

采用网格地图SLAM算法

{
  double val, value;
  if ( pos.x>=0 && pos.x<map.mapsize.x &&
       pos.y>=0 && pos.y<map.mapsize.y ) {
    if (map.mapsum[pos.x][pos.y]>0) {
      value = (double) (map.mapprob[pos.x][pos.y]); 
      if (value>0.2)
	val = 0.99;
      else
	val = 0.70;
    } else {
      val = 0.80;
    }
  } else {
    val = 0.80;
  }
  return(val);
}

double
get_map_val2( logtools_ivector2_t pos, MAP2 map )
{
  double val;
  if ( pos.x>=0 && pos.x<map.mapsize.x &&
       pos.y>=0 && pos.y<map.mapsize.y ) {
    if (map.mapsum[pos.x][pos.y]>0) {
      val = EPSILON + (double) (map.mapprob[pos.x][pos.y]); 
    } else {
      val = 1.0-EPSILON;
    }
  } else {
    val = 1.0-EPSILON;
  }
  if (val>=1.0) 
    return(1.0-EPSILON);
  else
    return(val);
}

double
stretch_function( double x )
{
  return( (settings.max_likelihood-settings.min_likelihood) * x +
	  settings.min_likelihood );
}

double
get_beam_prob( logtools_rpos2_t pos, double val, double angle,
	       double max_range, MAP2 * map )
{
  logtools_ivector2_t              end;
  logtools_vector2_t               abspt;
  logtools_rmove2_t                nomove = {0.0, 0.0, 0.0};
  
  if (val>=max_range) {
    abspt = logtools_compute_laser_points( pos, max_range-20.0, nomove, angle );
    map_pos_from_vec2( abspt, map, &end ); 
    mark_map_cell( end, map );
    if ( end.x>=0 && end.x<map->mapsize.x &&
	 end.y>=0 && end.y<map->mapsize.y ) {
      /* maxrange and in map and previous seen */
      if (map->mapsum[end.x][end.y]>0) {
	return(stretch_function(1.0-map->mapprob[end.x][end.y]));
      } else {
	return(settings.unknown_likelihood); 
      }
    } else {
      /* maxrange and point out of map */
      return(settings.unknown_likelihood); 
    }
  } else {
    /* no maxrange */
    abspt = logtools_compute_laser_points( pos, val,  nomove, angle );
    map_pos_from_vec2( abspt, map, &end );
    mark_map_cell( end, map );
    if ( end.x>=0 && end.x<map->mapsize.x &&
	 end.y>=0 && end.y<map->mapsize.y ) {
      /* no maxrange and in map and previous seen */
      if (map->mapsum[end.x][end.y]>0) {
	return(stretch_function(map->mapprob[end.x][end.y]));
      } else {
	return(settings.unknown_likelihood); 
      }
    } else {
      /* maxrange and point out of map */
      return(settings.unknown_likelihood); 
    }
  }
}

double
compute_scan_probability( MAP2 * map, logtools_rpos2_t pos, 
			  logtools_lasersens2_data_t lsens,
			  double max_range, double max_usable )
{
  static int                   first_time = TRUE; 
  static logtools_grid_line_t  line;
  static int                   max_num_linepoints = 0;
  int                          i, j, unknown;
  logtools_ivector2_t          start, end, uknw;
  logtools_vector2_t           abspt, startv, endv;
  logtools_rmove2_t            nomove = {0.0, 0.0, 0.0};
  double                       v, val = 0.0, prob, multprob;
  
  if (first_time) {
    max_num_linepoints =
      10 * ( max_range / map->resolution );
    line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * sizeof(logtools_ivector2_t) );
    first_time = FALSE;
  }

  prob = 1.0; 
  for (j=0;j<lsens.laser.numvalues;j+=2) {
    if (lsens.laser.val[j] <= max_usable ) {
      if (0) {
	if (lsens.laser.val[j] >= max_range ) {
	    abspt = logtools_compute_laser_points( pos, max_range,
						   nomove, lsens.laser.angle[j] );
	    map_pos_from_vec2( abspt, map, &end );
	} else {
	  abspt = logtools_compute_laser_points( pos, lsens.laser.val[j],
						 nomove, lsens.laser.angle[j] );
	  map_pos_from_vec2( abspt, map, &end );
	}
	map_pos_from_rpos( pos, map, &start );
	grid_line( start, end, &line );
	multprob = 1.0;
	if (line.numgrids>0) {
	  unknown = FALSE;
	  for (i=0;i<line.numgrids;i++) {
	    
	    if ( line.grid[i].x>=0 && line.grid[i].x<map->mapsize.x &&
		 line.grid[i].y>=0 && line.grid[i].y<map->mapsize.y ) {
	      
	      if (map->mapsum[line.grid[i].x][line.grid[i].y]==0) {
		/* unknown */
		if (!unknown) {
		  uknw = line.grid[i];
		  unknown = TRUE;
		}
		if (i==line.numgrids-1) {
		  startv.x = uknw.x * map->resolution;
		  startv.y = uknw.y * map->resolution;
		  endv.x   = line.grid[i].x * map->resolution;
		  endv.y   = line.grid[i].y * map->resolution;
		  /* logprob +=
		     log10(prob_unknown_space(vector2_distance(startv, endv),1));*/
		  multprob *=
		    prob_unknown_space(logtools_vector2_distance(startv, endv),1);
		}
	      } else {
		/* known */
		if (unknown) {
		  unknown = FALSE;
		  startv.x = uknw.x * map->resolution;
		  startv.y = uknw.y * map->resolution;
		  endv.x   = line.grid[i].x * map->resolution;
		  endv.y   = line.grid[i].y * map->resolution;
		  /* logprob +=
		     log10(prob_unknown_space(vector2_distance(startv,endv),0)); */
		  multprob *=
		    prob_unknown_space(logtools_vector2_distance(startv, endv),0);
		}
		if (i<line.numgrids-1) {
		  multprob *= (1.0-get_map_val( line.grid[i], *map ));
		  /* logprob += log10((1.0-get_map_val( line.grid[i], *map ))); */
		} else {
		  if (1) {
		    if (lsens.laser.val[j] >= max_range ) {
		      multprob *= (1.0-get_map_val( line.grid[i], *map ));
		      /* logprob +=
			 log10((1.0-get_map_val( line.grid[i], *map ))); */
		    } else {
		    v = get_map_val2( line.grid[i], *map  );
		    multprob *= v;
		    /* logprob += log10(v); */
		    }
		  }
		}
	      }
	    }
	  }
	}
	//      logprob += log10(multprob);
	val = (1.0-settings.min_likelihood)*multprob+settings.min_likelihood;
      //      printf( "(beamprob:%d-%f-%f)", j, multprob, val );
      } else {
	val = log(get_beam_prob( pos, lsens.laser.val[j],
				 lsens.laser.angle[j], max_range, map ));
	prob += val;
      }
    }
  }
  return(exp(prob));
}


double
compute_beam_prob( MAP2 * map, logtools_rpos2_t pos, 
		   double length, double max_range,
		   double * prob1, double * prob2 )
{
  static int                    first_time = TRUE; 
  static logtools_grid_line_t   line;
  static int                    max_num_linepoints = 0;
  int                           i;
  logtools_ivector2_t           start, end;
  logtools_vector2_t            abspt;
  logtools_rmove2_t             nomove = {0.0, 0.0, 0.0};
  double                        lprob = 0.0;
  
  if (first_time) {
    max_num_linepoints =
      10 * ( max_range / map->resolution );
    line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * 
						sizeof(logtools_ivector2_t) );
    first_time = FALSE;
  }

  abspt = logtools_compute_laser_points( pos, length, nomove, 0.0 );
  map_pos_from_vec2( abspt, map, &end );
  map_pos_from_rpos( pos, map, &start );

  grid_line( start, end, &line );
  if (line.numgrids>0) {
    for (i=0;i<line.numgrids;i++) {
      if ( line.grid[i].x<0 || line.grid[i].x>=map->mapsize.x ||
	   line.grid[i].y<0 || line.grid[i].y>=map->mapsize.y) {
	   //	   map->mapsum[line.grid[i].x][line.grid[i].y]==0 ) {
      	return(FALSE);
      }
      if (i==line.numgrids-1) {
	*prob1 = lprob + log(get_map_val( line.grid[i], *map ));
	*prob2 = lprob + log(1.0-get_map_val( line.grid[i], *map ));
	map->calc[line.grid[i].x][line.grid[i].y] = 1;
      } else {
	lprob += log(1.0-get_map_val( line.grid[i], *map ));
	map->calc[line.grid[i].x][line.grid[i].y] = 1;
      }
    }
  } else {
    return( FALSE );
  }
  return(TRUE);
}

int
intersect_bboxes( logtools_bounding_box_t box1, logtools_bounding_box_t box2 )
{
  if (box1.min.x<=box2.min.x) {
    /* box1.min.x is smaller that box2 */
    if (box1.max.x>box2.min.x) {
      /* intersection in x */
      if (box1.min.y<=box2.min.y) {
	/* box1.min.y is smaller that box2 */
	if (box1.max.y>box2.min.y) {
	  /* intersection in y */
	  return(1);
	} else {
	  return(0);
	}
      } else {
	/* box2.min.y is smaller that box1 */
	if (box2.max.y>=box1.min.y) {
	  /* intersection in y */
	  return(1);
	} else {
	  return(0);
	}
      }
    } else {
      return(0);
    }
  } else {
    /* box2.min.x is smaller that box1 */
    if (box2.max.x>=box1.min.x) {
      /* intersection in x */
      if (box1.min.y<=box2.min.y) {
	/* box1.min.y is smaller that box2 */
	if (box1.max.y>box2.min.y) {
	  /* intersection in y */
	  return(1);
	} else {
	  return(0);
	}
      } else {
	/* box2.min.y is smaller that box1 */
	if (box2.max.y>=box1.min.y) {
	  /* intersection in y */
	  return(1);
	} else {
	  return(0);
	}
      }
    } else {
      return(0);
    }
  }
  return(0);
}

void
write_map( MAP2 map, char *filename )
{
  int                 ok = TRUE;
  int                 i, j, idx;

  Image             * image;
  ImageInfo           image_info;
  double              c = 0.0;

#if defined MagickLibVersion && MagickLibVersion >= 0x500
  ExceptionInfo       exception;
  double            * pixel;
#else
  float             * red, * green, * blue, * opacity;
#endif

#if defined MagickLibVersion && MagickLibVersion >= 0x500
  fprintf( stderr, "alloc memory of pixel map (%d bytes) ... ",
	   map.mapsize.x * map.mapsize.y * 3 * sizeof(double) );
  if ( (pixel = (double *) malloc(map.mapsize.x * map.mapsize.y *
				  3 * sizeof(double)))==NULL )
    ok = FALSE;
  fprintf( stderr, "%s\n", ok?"yes":"no" );
#else
  fprintf( stderr, "alloc memory of pixel map (%d bytes) ... ",
	   map.mapsize.x * map.mapsize.y * 3 * sizeof(float) );
  if ( (red = (float *) malloc(map.mapsize.x * map.mapsize.y *
			       sizeof(float)))==NULL )
    ok = FALSE;
  fprintf( stderr, "red: %s,", ok?"yes":"no" );
  if ( (green = (float *) malloc(map.mapsize.x * map.mapsize.y *
				 sizeof(float)))==NULL )
    ok = FALSE;
  fprintf( stderr, "green: %s,", ok?"yes":"no" );
  if ( (blue = (float *) malloc(map.mapsize.x * map.mapsize.y *
				sizeof(float)))==NULL )
    ok = FALSE;
  fprintf( stderr, "blue: %s,", ok?"yes":"no" );
  if ( (opacity = (float *) malloc(map.mapsize.x * map.mapsize.y *
				   sizeof(float)))==NULL )
    ok = FALSE;
  fprintf( stderr, "opacity: %s,", ok?"yes":"no" );
#endif
  for (i=0;i<map.mapsize.x;i++) {
    for (j=0;j<map.mapsize.y;j++) {
      idx = j*map.mapsize.x+i;
      c = 1.0-map.mapprob[i][map.mapsize.y-j-1];
      if (map.mapsum[i][map.mapsize.y-j-1]>0) {
	if (c<0.0)
	  c = 0.0;
#if defined MagickLibVersion && MagickLibVersion >= 0x500
	pixel[idx*3]     = c;
	pixel[idx*3+1]   = c;
	pixel[idx*3+2]   = c;
#else
	red[idx]     = c;
	green[idx]   = c;
	blue[idx]    = c;
	opacity[idx] = 1.0;
#endif
      } else {
#if defined MagickLibVersion && MagickLibVersion >= 0x500
	pixel[idx*3]     = 200.0/255.0;
	pixel[idx*3+1]   = 200.0/255.0;
	pixel[idx*3+2]   = 255.0/255.0;
#else
	red[idx]     = 200.0/255.0;
	green[idx]   = 200.0/255.0;
	blue[idx]    = 255.0/255.0;
	opacity[idx] = 1.0;
#endif
      }
    }
  }
#if defined MagickLibVersion && MagickLibVersion >= 0x430
  GetExceptionInfo(&exception);
#endif
  GetImageInfo(&image_info);
  fprintf( stderr, "create image of map ... " );
#if defined MagickLibVersion && MagickLibVersion >= 0x430
  image = ConstituteImage ( map.mapsize.x, map.mapsize.y, "RGB",
			    DoublePixel, pixel, &exception );
  if (image == (Image *) NULL) {
    fprintf( stderr, "ERROR: no memory!!!\n" );
    exit(1);
  }
#else
#if defined MagickLibVersion && MagickLibVersion >= 0x420
  image = CreateImage( map.mapsize.x, map.mapsize.y,
		       red, green, blue, opacity );
#else
  image = CreateImage( map.mapsize.x, map.mapsize.y, "RGB",
		       red, green, blue, NULL );
  if (image == (Image *) NULL) {
    fprintf( stderr, "ERROR: no memory!!!\n" );
    exit(1);
  }
#endif
#endif
  strcpy( image->filename, filename );
  fprintf( stderr, "done\n" );
  fprintf( stderr, "write image in file %s ... ", filename );
  WriteImage( &image_info, image );
  fprintf( stderr, "done\n" );
  DestroyImage(image);
#if defined MagickLibVersion && MagickLibVersion >= 0x430
  DestroyConstitute();
  free(pixel);
#else
  free(red);
  free(green);
  free(blue);
#endif
  

}