sim.hpp

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
*/

#ifndef _SIM_HPP
#define _SIM_HPP

#include <inttypes.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <list>
#include <limits>


// pgm.cpp
bool load_pgm(const char *file, uint8_t **grid, uint32_t *w, uint32_t *h);
bool write_pgm(const char *file, const uint8_t *grid, uint32_t w, uint32_t h);

struct point_t
{
  double x, y;
  point_t() {}
  point_t(double _x, double _y) : x(_x), y(_y) {}
};

struct pose_t
{
  double x, y, t;
  pose_t() : x(0), y(0), t(0) {}
  pose_t(const pose_t &p) : x(p.x), y(p.y), t(p.t) {}
  pose_t(double _x, double _y, double _t) : x(_x), y(_y), t(_t) {}
};

enum {
  F = 0,   // full
  E = 255, // empty
  U = 127  // unknown
};

struct sensor_t
{
  double   v;        // robot velocity [>=0]
  double   F;        // firing frequency [1/timestep]
  uint32_t rho;      // readings per scan [>=1]
  double   rmax;     // maximum sensing range [>=0]
  double   delta_b;  // half beam width [0..pi]
  double   sigma_b;  // bearing uncertainty [0..2pi]
  double   sigma_r;  // range uncertainty [0..rmax]
  double   e_e;      // false negative rate [0..1]
  double   e_f;      // false positive rate [0..1]
};

// 2x2 matrix of parameters for pairwise MRF psi function
struct mrf_psi_function
{
  double f_f; // "likelihood" of F "adjacent to" F
  double f_e; // "likelihood" of F "adjacent to" E
  double e_e; // "likelihood" of E "adjacent to" E
};
struct mrf_pair
{
  int dx, dy; // location of the neighbor relative to the cell
  mrf_psi_function psi;
};


inline double clamp_angle(double a)
{
  while(a < -M_PI)
    a += 2 * M_PI;
  while(a >= M_PI)
    a -= 2 * M_PI;
  return a;
}

inline double angular_distance(double a1, double a2)
{
  double a = clamp_angle(a1-a2);
  if(a > M_PI) a -= 2 * M_PI;
  return a;
}

inline double deg2rad(double a) { return a * M_PI / 180; }
inline double rad2deg(double a) { return a * 180 / M_PI; }

inline double sign(double d) { if(d < 0) return -1; else return 1; }

template <class F>
inline double squared_distance(const point_t &p, const F &f)
{
  return (p.x-f.x) * (p.x-f.x) + (p.y-f.y) * (p.y-f.y);
}

template <class F>
inline double distance(const point_t &p, const F &f)
{
  return ::sqrt(squared_distance(p, f));
}



// trajectory.cpp
bool load_waypoints(const char *file, std::list<point_t> &waypoints);
bool load_trajectory(const char *file, std::vector<pose_t> &trajectory);
void generate_perfect_trajectory_from_waypoints
  (const std::list<point_t> &waypoints, const sensor_t &s,
   std::vector<pose_t> &trajectory);
void generate_brownian_trajectory
  (double T, const sensor_t &s, std::vector<pose_t> &trajectory);
void generate_uncorrelated_trajectory
  (double T, const sensor_t &s, std::vector<pose_t> &trajectory);

// grid.cpp
extern uint8_t *grid;   // ground truth map grid
extern uint32_t W, H;   // width/height in cells
extern std::vector<mrf_pair> mrf; // if necessary
bool load_mrf_model(const char *file);
double grid_entropy(const uint8_t *grid, uint32_t w, uint32_t h);
double grid_density(const uint8_t *grid, uint32_t w, uint32_t h);
void generate_random_grid_unif(double density);
void generate_random_grid_mrf(double density, uint32_t N = 20); // uses mrf vector
void generate_best_possible_map(uint8_t *possible);

// sensor.cpp
extern double eps_scale;
extern bool fixed_update; // update by adding/subtracting a fixed probability
extern double p_0; // the fixed probability
void sense_and_update(const pose_t &p, const sensor_t &s);
void update_without_simulation
  (const pose_t &p, const sensor_t &s, const std::vector<double> &ranges,
   double amin, double amax, double ares);

// sim.cpp
extern double xmin, ymin, xmax, ymax;  // dimensions in world coordinates
extern double   *map;         // map generated by the simulation
extern uint32_t *real_freq;   // frequency of sensor observations of each cell
extern uint32_t *update_freq; // frequency of sensor updates of each cell
extern double   delta;        // cell size
extern double   density;      // base environment density

inline uint8_t opposite(uint8_t status) { return (status == F) ? E : F; }
inline uint32_t cell(int32_t x, int32_t y)
{
  while(x < 0) x += W;
  while(x >= int32_t(W)) x -= W;
  while(y < 0) y += H;
  while(y >= int32_t(H)) y -= H;
  return x*W+y;
}
inline point_t cell_to_point(int32_t x, int32_t y)
{
  return point_t(xmin+x*delta, ymin+y*delta);
}
inline point_t cell_to_point(uint32_t m)
{
  return cell_to_point(m/W, m%W);
}

#endif // _SIM_HPP