/*  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 parametersdouble 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 stepstd::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;}