sim.cpp

SLAM Gridsim use with sparer sensor

/*
  gridsim2 (c) 2006 Kris Beevers

  This file is part of gridsim2.

  gridsim2 is free software; you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  gridsim2 is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with gridsim2; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include "sim.hpp"
#include "options.hpp"
#include <fstream>

// environment parameters
double delta;   // size of a grid cell side (m)
double density; // probability a cell is occupied (for random generation)
double xmin, ymin, xmax, ymax;

double *map;
uint32_t *real_freq, *update_freq;

sensor_t sensor;

std::vector<pose_t> trajectory;

FILE *ranges = 0;  // for use with pre-generated logs of range reports for each step
std::vector<double> z_t;
double amin = 0, amax = 360, ares = 1;


int parse_command_line(int argc, char **argv);

double error_prob(const uint8_t *grid, const double *map)
{
  double e = 0;
  for(uint32_t i = 0; i < W*H; ++i) {
    if(grid[i] == F)
      e += (1.0-map[i])*(1.0-map[i]); // squared error
    else if(grid[i] == E)
      e += map[i]*map[i];
  }
  return e;
}

double error_ml(const uint8_t *grid, const double *map)
{
  double e = 0;
  for(uint32_t i = 0; i < W*H; ++i) {
    uint8_t argmax;
    if(fabs(map[i]-0.5) < 1e-5) // flip a fair coin
      argmax = (drand48() < map[i]) ? F : E;
    else
      argmax = (map[i] > 0.5) ? F : E;
    if(grid[i] != U && argmax != grid[i])
      e += 1.0;
  }
  return e;
}

template <class T>
double mean(const T *grid)
{
  double mu = 0;
  for(uint32_t i = 0; i < W*H; ++i)
    mu += double(grid[i]) / double(W*H);
  return mu;
}

template <class T>
T max(const T *grid)
{
  T m = std::numeric_limits<T>::min();
  for(uint32_t i = 0; i < W*H; ++i)
    if(grid[i] > m) m = grid[i];
  return m;
}

bool read_ranges(uint32_t rho)
{
  z_t.clear();
  double r;
  for(uint32_t i = 0; i < rho; ++i) {
    if(fscanf(ranges, "%lg", &r) != 1)
      return false;
    z_t.push_back(r);
  }
  return true;
}

int main(int argc, char **argv)
{
  srand48(time(0));

  parse_command_line(argc, argv);

  // run the simulation
  for(uint32_t t = 0; t < trajectory.size(); ++t) {
    if(ranges != 0) {
      assert(read_ranges(sensor.rho));
      update_without_simulation(trajectory[t], sensor, z_t, amin, amax, ares);
    } else
      sense_and_update(trajectory[t], sensor);
  }

  // compute interesting statistics
  uint8_t *possible = new uint8_t[W*H];
  generate_best_possible_map(possible);
  double d = grid_density(grid, W, H);
  double ent = grid_entropy(grid, W, H);
  double nu_prob = error_prob(possible, map);
  double nu_ml = error_ml(possible, map);
  double mean_real_freq = mean(real_freq);
  double mean_update_freq = mean(update_freq);
  double mean_real_update_ratio = 0;

  // ratio of real observations to updates (alternatively we could
  // just use the two means to compute this)
  uint32_t m_count = 0;
  for(uint32_t i = 0; i < W*H; ++i)
    if(update_freq[i] > 0) {
      ++m_count;
      mean_real_update_ratio += double(real_freq[i])/double(update_freq[i]);
    }
  mean_real_update_ratio /= double(m_count);

  printf("%g %g %g %g %g %g %g\n", d, ent, nu_prob, nu_ml,
	 mean_real_freq, mean_update_freq, mean_real_update_ratio);

  if(!options::quickget<bool>("nofiles")) { // make some images
    write_pgm("grid.pgm", grid, W, H);
    write_pgm("possible.pgm", possible, W, H);

    uint8_t *map_out = new uint8_t[W*H];
    uint8_t *map_ml_out = new uint8_t[W*H];
    uint8_t *real_freq_out = new uint8_t[W*H];
    uint8_t *update_freq_out = new uint8_t[W*H];
    uint32_t max_real_freq = max(real_freq);
    uint32_t max_update_freq = max(update_freq);

    for(uint32_t i = 0; i < W*H; ++i) {
      map_out[i] = (uint8_t)((1.0-map[i])*255);
      map_ml_out[i] = (update_freq[i] == 0) ? U : ((map[i] > 0.5) ? 0 : 255);
      real_freq_out[i] = (uint8_t)(255*double(real_freq[i])/double(max_real_freq));
      update_freq_out[i] = (uint8_t)(255*double(update_freq[i])/double(max_update_freq));
    }

    write_pgm("map.pgm", map_out, W, H);
    write_pgm("map-ml.pgm", map_ml_out, W, H);
    write_pgm("real-freq.pgm", real_freq_out, W, H);
    write_pgm("update-freq.pgm", update_freq_out, W, H);

    // save the trajectory
    FILE *traj = fopen("trajectory.phist", "w");
    for(uint32_t t = 0; t < trajectory.size(); ++t) {
      const pose_t &p = trajectory[t];
      fprintf(traj, "%lg %lg %lg\n", p.x, p.y, rad2deg(p.t));
    }
    fclose(traj);
  }

  return 0;
}

int parse_command_line(int argc, char **argv)
{
  // set up commandline/configuration file options
  options::add<bool>("help", 0, "Print usage information", 0, false, options::nodump);
  options::set_cf_options("config", "c");
  options::add<std::string>("save-config", 0, "Save configuration file", 0, "", options::nodump);

  options::add<bool>("nofiles", 0, "Don't save output files", 0, false, options::nodump);

  options::add<uint32_t>("width", "w", "Workspace width (cells)", "Environment", 400);
  options::add<uint32_t>("height", "h", "Workspace height (cells)", "Environment", 400);
  options::add<double>("delta", "D", "Grid cell size (m)", "Environment", 0.1);
  options::add<double>("density", "d", "Base environment density [0..1]", "Environment", 0.5);
  options::add<std::string>("grid", "g", "Grid file (pgm)", "Environment", "", options::nodump);
  options::add<std::string>("mrf", "m", "Pairwise MRF file", "Environment", "", options::nodump);
  options::add<uint32_t>("mrf-gibbs-iterations", "N", "Gibbs sampler iterations for MRF model",
			 "Environment", 20);

  options::add<double>("frequency", "F", "Firing frequency (Hz)", "Sensor", 5.0);
  options::add<uint32_t>("rho", "p", "Readings per scan", "Sensor", 180);
  options::add<double>("rmax", "r", "Maximum range (m)", "Sensor", 10.0);
  options::add<double>("beamwidth", "B", "Beam width (rad)", "Sensor", M_PI/180.0);
  options::add<double>("sigma-b", "b", "Bearing uncertainty [0..2pi]", "Sensor", M_PI/180.0);
  options::add<double>("sigma-r", "a", "Range uncertainty [0..rmax]", "Sensor", 0.2);
  options::add<double>("err-e", "e", "False negative rate [0..1]", "Sensor", 0.05);
  options::add<double>("err-f", "f", "False positive rate [0..1]", "Sensor", 0.01);
  options::add<std::string>("ranges", 0, "Range readings file", "Sensor", "", options::nodump);

  options::add<bool>("sampled-poses", "s", "Sample poses randomly (not a real trajectory)",
		     "Trajectory", false);
  options::add<double>("time-limit", "T", "Trajectory length (s)", "Trajectory", 50.0);
  options::add<std::string>("waypoints", 0, "Waypoint file", "Trajectory", "", options::nodump);
  options::add<std::string>("trajectory", "t", "Trajectory file", "Trajectory", "", options::nodump);
  options::add<double>("velocity", "v", "Velocity (m/s)", "Trajectory", 2.0);

  options::add<bool>("fixed-update", 0, "Fixed updates?", "Mapping", false);
  options::add<double>("update-prob", "u", "Fixed update prob. [0..0.5]", "Mapping", 0.01);

  // parse commandline (and read config file if it is specified)
  int inpidx = options::parse_cmdline(argc, argv);

  if(inpidx < 0) // some kind of error
    exit(1);    // relevant info printed by getopt

  // print usage information
  if(inpidx > argc || options::quickget<bool>("help") == true) {
    std::cerr << "Usage: " << argv[0] << " [options]" << std::endl;
    options::print_options(std::cout);
    exit(1);
  }

  // save a config file based on these options?
  std::string cfname = options::quickget<std::string>("save-config");
  if(cfname.length() > 0) {
    std::ofstream conf(cfname.c_str());
    if(!conf)
      std::cerr << "Can't write configuration file " << cfname << std::endl;
    else
      options::dump(conf);
  }

  // do initialization
  density = options::quickget<double>("density");
  std::string s = options::quickget<std::string>("grid");
  if(s.length() > 0) {
    assert(load_pgm(s.c_str(), &grid, &W, &H));
  } else {
    W = options::quickget<uint32_t>("width");
    H = options::quickget<uint32_t>("height");
    s = options::quickget<std::string>("mrf");
    if(s.length() > 0) {
      assert(load_mrf_model(s.c_str()));
      generate_random_grid_mrf(density, options::quickget<uint32_t>("mrf-gibbs-iterations"));
    } else
      generate_random_grid_unif(density);
  }
  map = new double[W*H];
  real_freq = new uint32_t[W*H];
  update_freq = new uint32_t[W*H];
  for(uint32_t i = 0; i < W*H; ++i) {
    map[i] = 0.5;
    real_freq[i] = update_freq[i] = 0;
  }
  delta = options::quickget<double>("delta");
  eps_scale = delta * M_SQRT2 / 2;
  xmin = -double(W)*delta/2;
  xmax = double(W)*delta/2;
  ymin = -double(H)*delta/2;
  ymax = double(H)*delta/2;

  sensor.v = options::quickget<double>("velocity");
  sensor.F = options::quickget<double>("frequency");
  sensor.rho = options::quickget<uint32_t>("rho");
  sensor.rmax = options::quickget<double>("rmax");
  sensor.delta_b = options::quickget<double>("beamwidth");
  sensor.sigma_b = options::quickget<double>("sigma-b");
  sensor.sigma_r = options::quickget<double>("sigma-r");
  sensor.e_e = options::quickget<double>("err-e");
  sensor.e_f = options::quickget<double>("err-f");

  fixed_update = options::quickget<bool>("fixed-update");
  p_0 = options::quickget<double>("update-prob");

  double T = options::quickget<double>("time-limit");

  s = options::quickget<std::string>("waypoints");
  if(s.length() > 0) {
    std::list<point_t> waypoints;
    assert(load_waypoints(s.c_str(), waypoints));
    generate_perfect_trajectory_from_waypoints(waypoints, sensor, trajectory);
  }
  s = options::quickget<std::string>("trajectory");
  if(s.length() > 0)
    assert(load_trajectory(s.c_str(), trajectory));
  if(trajectory.size() == 0) { // didn't load any trajectory
    if(options::quickget<bool>("sampled-poses"))
      generate_uncorrelated_trajectory(T, sensor, trajectory);
    else
      generate_brownian_trajectory(T, sensor, trajectory);
  }

  s = options::quickget<std::string>("ranges");
  if(s.length() > 0) {
    ranges = fopen(s.c_str(), "r");
    assert(ranges != 0);
    assert(fscanf(ranges, "%lg %lg %lg", &amin, &amax, &ares) == 3);
    amin = deg2rad(amin);
    amax = deg2rad(amax);
    ares = deg2rad(ares);
  }

  return 0;
}