sensor.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 <ext/hash_set>

// the sensing assumes a toroidal environment, i.e., sensor beams
// "wrap" from one edge of the grid to the opposite edge.  (actually,
// implementing this adds some complications but it's an assumption
// made in our analytical model for simplicity so we'll implement it
// that way here.)

double eps_scale; // computed in sim.cpp from as a function of delta
bool fixed_update = false; // set in sim.cpp
double p_0 = 0.01; // set in sim.cpp

struct tor_cell
{
  tor_cell(int32_t _x, int32_t _y) : x(_x), y(_y) {}
  bool operator==(const tor_cell &c) const { return cell(x,y)==cell(c.x,c.y); }
  int32_t x, y;
};
struct tor_cell_hash
{
  size_t operator()(const tor_cell &c) const { return cell(c.x,c.y); }
};
// toroidal 8-neighbors: the cells may not actually be on the grid
// (values may be < 0 or > W/H); they will be wrapped later when
// actually updating the grid
void get_cell_neighbors8(int32_t x, int32_t y, std::vector<tor_cell> &Nm)
{
  Nm.clear();
  Nm.push_back(tor_cell(x-1,y-1));
  Nm.push_back(tor_cell(x,y-1));
  Nm.push_back(tor_cell(x+1,y-1));
  Nm.push_back(tor_cell(x-1,y));
  Nm.push_back(tor_cell(x+1,y));
  Nm.push_back(tor_cell(x-1,y+1));
  Nm.push_back(tor_cell(x,y+1));
  Nm.push_back(tor_cell(x+1,y+1));
}

uint8_t sense(const sensor_t &s, uint32_t m)
{
  if(grid[m] == F)
    return (drand48() < s.e_e) ? E : F;
  else
    return (drand48() < s.e_f) ? F : E;
}

// returns range reading
double get_beam_cells
  (const pose_t &p, const sensor_t &s, double delta_b, double a, double rmax,
   bool dosense, __gnu_cxx::hash_set<uint32_t> &C)
{
  double min_occ = std::numeric_limits<double>::max();
  point_t n_point;
  double r;
  std::vector<tor_cell> Nm;

  // get the set C of cells that intersect the beam up to a given
  // range and if necessary simulate false neg/pos observations of
  // cells and only include cells that are not occluded
  std::list<tor_cell> Q;
  __gnu_cxx::hash_set<tor_cell, tor_cell_hash> been_in_Q;
  tor_cell m((int32_t)((p.x-xmin)/delta), (int32_t)((p.y-ymin)/delta));
  Q.push_back(m);
  been_in_Q.insert(m);
  C.clear();
  C.insert(cell(m.x,m.y));
  while(Q.size() != 0) {
    m = Q.front(); Q.pop_front();
    get_cell_neighbors8(m.x, m.y, Nm);
    for(uint32_t i = 0; i < Nm.size(); ++i) {
      const tor_cell &n = Nm[i];
      if(been_in_Q.find(n) != been_in_Q.end())
	continue;
      n_point = cell_to_point(n.x, n.y);
      r = distance(n_point, p);
      if(r > rmax)
	continue;
      if(dosense && r > min_occ + s.sigma_r + eps_scale)
	continue;
      // check if cell n intersects the beam
      if(r > eps_scale &&
	 fabs(angular_distance(atan2(n_point.y-p.y, n_point.x-p.x), a)) > delta_b + asin(eps_scale/r))
	continue;
      // yep, so add it to the set
      C.insert(cell(n.x,n.y));
      Q.push_back(n);
      been_in_Q.insert(n);
      // simulate sensing if necessary
      if(dosense && sense(s, cell(n.x,n.y)) == F)
	if(min_occ > r) min_occ = r;
    }
  }

  // add range error
  min_occ += s.sigma_r * (2*drand48()-1);
  min_occ = std::max(0.0, min_occ);

  return min_occ;
}

void beam_update(const pose_t &p, const sensor_t &s, double a, double r)
{
  static __gnu_cxx::hash_set<uint32_t> C, C_u; // static for speed
  __gnu_cxx::hash_set<uint32_t>::iterator i;
  get_beam_cells(p, s, s.delta_b, a, std::min(r,s.rmax)+s.sigma_r, false, C);
  get_beam_cells(p, s, s.delta_b+s.sigma_b, a, std::min(r,s.rmax)+s.sigma_r, false, C_u);

  // divide C_u into C_E and C_F, and count #cells in C_E^*
  static std::vector<uint32_t> C_E, C_F; // static for speed
  C_E.clear(); C_F.clear();
  uint32_t C_Estar = 0;
  for(i = C_u.begin(); i != C_u.end(); ++i) {
    ++update_freq[*i]; // update count of number of updates of this cell
    if(distance(cell_to_point(*i), p) <= std::max(0.0, r-s.sigma_r-eps_scale)) {
      C_E.push_back(*i);
      if(C.find(*i) != C.end()) ++C_Estar;
    } else if(distance(cell_to_point(*i), p) <= r+s.sigma_r+eps_scale)
      C_F.push_back(*i);
  }

  // do updates for all the cells
  double p_f = 0.5 + (0.5-s.e_f)/C_F.size();
  double p_e = 0.5 + (double(C_Estar)/double(C_E.size())) * (0.5-s.e_e);
  for(uint32_t j = 0; j < C_F.size(); ++j) {
    double &p = map[C_F[j]];
    if(fixed_update)
      p = std::min(1.0, p+p_0);
    else {
      p = p*p_f;
      if(p > 0) p /= ((1-p)*(1-p_f) + p*p_f); // normalize
    }
  }
  for(uint32_t j = 0; j < C_E.size(); ++j) {
    double &p = map[C_E[j]];
    if(fixed_update)
      p = std::max(0.0, p-p_0);
    else {
      p = p*(1-p_e);
      if(p > 0) p /= ((1-p)*p_e + p*(1-p_e)); // normalize
    }
  }
}

void beam_sense_and_update
  (const pose_t &p, const sensor_t &s, double expected_a, double noisy_a)
{
  static __gnu_cxx::hash_set<uint32_t> C; // static for speed
  __gnu_cxx::hash_set<uint32_t>::iterator i;
  double r = get_beam_cells(p, s, s.delta_b, noisy_a, s.rmax, true, C);
  for(i = C.begin(); i != C.end(); ++i)
    ++real_freq[*i]; // update count of number of actual observations of this cell

  beam_update(p, s, expected_a, r);
}

void sense_and_update(const pose_t &p, const sensor_t &s)
{
  if(grid[cell((uint32_t)((p.x-xmin)/delta), (uint32_t)((p.y-ymin)/delta))] == F)
    return; // can't sense if the robot is in an occupied cell
  double a = p.t, noisy_a;
  const double da = 2*M_PI / double(s.rho);
  for(uint32_t i = 0; i < s.rho; ++i, a += da) {
    noisy_a = a + s.sigma_b * (2*drand48()-1); // add bearing error
    beam_sense_and_update(p, s, a, noisy_a);
  }
}

void update_without_simulation
  (const pose_t &p, const sensor_t &s, const std::vector<double> &ranges,
   double amin, double amax, double ares)
{
  assert(p.x >= xmin && p.x <= xmax && p.y >= ymin && p.y <= ymax);
  double a = p.t + amin;
  for(uint32_t i = 0; i < s.rho; ++i, a += ares) {
    if(ranges[i] < s.rmax)
      beam_update(p, s, a, ranges[i]);
  }
}