occupancy_grid.cpp

Go to the documentation of this file.

#include <iostream>
#include <fstream>
#include <algorithm>
#include <queue>
#include "properties.hpp"
#include "occupancy_grid.hpp"
#include "cone_tracing.hpp"

using namespace Arak;

OccupancyGrid::OccupancyGrid(const Arak::Util::PropertyMap& props) {
  using namespace Arak::Util;
  // Set up the grid.
  int n = 1;
  if (hasp(props, "arak.occupancy_grid.grid_size")) {
    parse(getp(props, "arak.occupancy_grid.grid_size"), n);
    assert(n > 0);
  }
  double x, y, width, height;
  assert(parse(getp(props, "arak.occupancy_grid.xmin"), x));
  assert(parse(getp(props, "arak.occupancy_grid.ymin"), y));
  assert(parse(getp(props, "arak.occupancy_grid.width"), width));
  assert(parse(getp(props, "arak.occupancy_grid.height"), height));
  Geometry::Rectangle r(x, y, x + width, y + height);
  double aspect_ratio = width / height;
  int rows = std::max(1, int(sqrt(double(n) / aspect_ratio)));
  int cols = n / rows;
  grid = new EstimatorGrid(r, rows, cols);
  for (int i = 0; i < rows; i++)
    for (int j = 0; j < cols; j++) {
      EstimatorGrid::Cell& cell = grid->getCell(i, j);
      OccupancyEstimator* est = new OccupancyEstimator();
      cell.add(est);
    }
  // Process any measurements.
  if (hasp(props, "arak.obs_type")) {
    const std::string& obs_type = getp(props, "arak.obs_type");    
    if (obs_type == "range") {
      // Laser data.  Get the parameters.
      double maxRange, obstacleWidth, p_given_occ, p_given_free;
      assert(parse(getp(props, "arak.range_obs.max_range"), maxRange));
      assert(parse(getp(props, "arak.occupancy_grid.obstacle_width"), obstacleWidth));
      assert(parse(getp(props, "arak.occupancy_grid.p_given_occ"), p_given_occ));
      assert(parse(getp(props, "arak.occupancy_grid.p_given_free"), p_given_free));
      // Process the data.
      const std::string& datafile_path = 
  getp(props, "arak.range_obs.data_file");
      std::ifstream in(datafile_path.data());
      assert(in.good());
      int n;
      in >> n;
      std::vector<Geometry::Point> impacts;
      Geometry::Point source, impact;
      in >> source >> impact;
      impacts.push_back(impact);
      for (int i = 1; i < n; i++) {
  Geometry::Point new_source;
  in >> new_source >> impact;
  if (new_source == source)
    impacts.push_back(impact);
  else {
    laser_scan(source, impacts, maxRange, 
         obstacleWidth, p_given_occ, p_given_free);
    source = new_source;
    impacts.clear();
    impacts.push_back(impact);
  }
      }
      laser_scan(source, impacts, maxRange, obstacleWidth, p_given_occ, p_given_free);
      /*
      for (int i = 0; i < n; i++) {
  Geometry::Point p, q;
  in >> p >> q;
  laser(p, q, maxRange, obstacleWidth, p_given_occ, p_given_free);
      }
      */
    } else if (obs_type == "sonar") {
      // Sonar data.  Get the parameters.
      double width, maxRange, obstacleWidth;
      assert(parse(getp(props, "arak.sonar_obs.width"), width));
      assert(parse(getp(props, "arak.sonar_obs.max_range"), maxRange));
      assert(parse(getp(props, "arak.occupancy_grid.obstacle_width"), obstacleWidth));
      double p_given_occ, p_given_free;
      assert(parse(getp(props, "arak.occupancy_grid.p_given_occ"), p_given_occ));
      assert(parse(getp(props, "arak.occupancy_grid.p_given_free"), p_given_free));
      // Process the data.
      const std::string& datafile_path = 
  getp(props, "arak.range_obs.data_file");
      std::ifstream in(datafile_path.data());
      assert(in.good());
      int n;
      in >> n;
      for (int i = 0; i < n; i++) {
  Geometry::Point source;
  double angle, range;
  in >> source >> angle >> range;
  sonar(source, angle, range, maxRange, obstacleWidth, width, 
        p_given_occ, p_given_free);
      }
    } else {
      std::cerr << "Unrecognized observation type: " 
    << obs_type << std::endl;
    }
  }
}

OccupancyGrid::~OccupancyGrid() {
  for (int i = grid->numRows() - 1; i >= 0; i--) {
    for (int j = 0; j < grid->numCols(); j++) {
      EstimatorGrid::Cell& cell = grid->getCell(i, j);
      delete *(cell.getItemList().begin());
    }
  }
  delete grid;
}

void OccupancyGrid::laser(const Geometry::Point& source,
        const Geometry::Point& impact,
        double maxRange,
        double obstacleWidth,
        double p_given_occ,
        double p_given_free) {
  // Ignore readings over 50 meters.
  if (CGAL::to_double(CGAL::squared_distance(source, impact)) > 
      maxRange * maxRange) 
    return;
  // Compute the max range point.
  const Geometry::Vector v = impact - source;
  double range = sqrt(CGAL::to_double(v.squared_length()));
  const Geometry::Point max = source + v * (maxRange / range);
  // Compute the obstacle segment.
  Geometry::Segment obstacle(impact, impact + v * (obstacleWidth / range));
  // Use a line search from the source to the back of the obstacle.
  typedef EstimatorGrid::LineCellIterator Iterator; 
  Iterator it(*grid, source, obstacle.target(), Iterator::SEGMENT()), end;
  bool free = true;
  do {
    EstimatorGrid::Cell& cell = *it;
    OccupancyEstimator* est = *(cell.getItemList().begin());
    // Determine if we have entered the obstacle.
    if (free && !cell.has_on_unbounded_side(impact)) free = false;
    if (free) {
      est->condition(p_given_free);
    } else
      est->condition(p_given_occ);
  } while (++it != end);
}

void OccupancyGrid::laser_scan(const Geometry::Point& source,
             const std::vector<Geometry::Point>& impacts,
             double maxRange,
             double obstacleWidth,
             double p_given_occ,
             double p_given_free) {
  // std::cerr << "Processing scan with " << impacts.size() << " beams." << std::endl;
  // Compute a bounding box containing all cells that should be
  // updated by this scan.
  double xmin = source.x(), xmax = source.x(), 
    ymin = source.y(), ymax = source.y(); 
  for (unsigned int i = 0; i < impacts.size(); i++) {
    xmin = std::min(xmin, impacts[i].x() - obstacleWidth);
    xmax = std::max(xmax, impacts[i].x() + obstacleWidth);
    ymin = std::min(ymin, impacts[i].y() - obstacleWidth);
    ymax = std::max(ymax, impacts[i].y() + obstacleWidth);
  }
  // Iterate over these cells.
  typedef EstimatorGrid::RectangleCellIterator Iterator; 
  Iterator it(*grid, Geometry::Rectangle(xmin, ymin, xmax, ymax)), end;
  while (it != end) {
    EstimatorGrid::Cell& cell = *it;
    OccupancyEstimator* est = *(cell.getItemList().begin());
    // Find the center point of the cell.
    Geometry::Point center = CGAL::midpoint(cell[0], cell[2]);
    // Determine the laser beam closest to this cell (in angle).
    unsigned int closest = 0;
    double dist = std::numeric_limits<double>::infinity();
    for (unsigned int i = 0; i < impacts.size(); i++) {
      Geometry::Line line(source, impacts[i]);
      Geometry::Point projection = line.projection(center);
      double d = sqrt(CGAL::to_double(CGAL::squared_distance(center, projection)));
      if (d < dist) {
  closest = i;
  dist = d;
      }
    }
    // Determine if the angle to the closest beam is close enough.
    double angle_to_center = atan2(center.y() - source.y(),
           center.x() - source.x());
    double beam_angle = atan2(impacts[closest].y() - source.y(),
            impacts[closest].x() - source.x());
    // std::cerr << "Beam: " << beam_angle << " Center: " << angle_to_center << std::endl;
    if ((fabs(angle_to_center - beam_angle) < M_PI / 180.0)  // TODO: parameter
  &&
  (CGAL::to_double(CGAL::squared_distance(source, impacts[closest])) < 
   maxRange * maxRange)) { // ignore max-range readings
      // Update this cell with this beam.
      double obs_range = 
  sqrt(CGAL::to_double(CGAL::squared_distance(impacts[closest], source)));
      double center_range = 
  sqrt(CGAL::to_double(CGAL::squared_distance(center, source)));
      if (fabs(obs_range - center_range) < obstacleWidth / 2.0)
  est->condition(p_given_occ);
      else if (obs_range > center_range)
  est->condition(p_given_free);
    }
    ++it;
  }
}

void OccupancyGrid::sonar(const Geometry::Point& source,
        double angle,
        double range,
        double maxRange,
        double obstacleWidth,
        double width,
        double p_given_occ,
        double p_given_free) {
  double minAngle = angle - width / 2.0;
  double maxAngle = angle + width / 2.0;
  Geometry::Direction min(cos(minAngle), sin(minAngle));
  Geometry::Direction max(cos(maxAngle), sin(maxAngle));
  range = std::min(range, maxRange);
  Geometry::Triangle t = 
    outerApprox(source, min, max, Geometry::Kernel::FT(range));
  typedef EstimatorGrid::TriangleCellIterator Iterator; 
  for (Iterator it = Iterator(*grid, t); it != Iterator(); ++it) {
    EstimatorGrid::Cell& cell = *it;
    OccupancyEstimator* est = *(cell.getItemList().begin());
    // Compute the midpoint (representative) of the cell.
    Geometry::Point p = CGAL::midpoint(cell.min(), cell.max());
    // Compute the range and angle to this point.
    double r = sqrt(CGAL::to_double(CGAL::squared_distance(source, p)));
    Geometry::Direction d = (p - source).direction();
    // Determine if the point is inside the cone.
    if (d.counterclockwise_in_between(min, max)) {
      // Determine if the point is before contact, at contact, or
      // after contact.
      if (r < (range - (obstacleWidth / 2.0))) {
  // The point is before contact.
  est->condition(p_given_free);
      } else if ((range < maxRange) && 
     (r <= (range + (obstacleWidth / 2.0)))) {
  // The point is at contact.
  est->condition(p_given_occ);
      }
    }
  }
}

---