occupancy_grid.hpp

Go to the documentation of this file.

#ifndef _OCCUPANCY_GRID_HPP
#define _OCCUPANCY_GRID_HPP

#include <iostream>
#include <iomanip>
#include "properties.hpp"
#include "global.hpp"
#include "grid.hpp"

namespace Arak {

  class OccupancyGrid {

  protected:

    // Forward declaration
    class OccupancyEstimator;
    class StateComparator;

    typedef Grid<OccupancyEstimator*> EstimatorGrid;

    class OccupancyEstimator {

      friend class OccupancyGrid;
      friend class OccupancyGrid::StateComparator;

    protected:

      double prior_log_odds;

      double log_odds;

    public:
      
      OccupancyEstimator(double prior = 0.5) {
  this->log_odds = this->prior_log_odds = log(prior / (1 - prior));
      }

      void condition(double posterior) {
  double obs_log_odds = log(posterior / (1.0 - posterior));
  log_odds += obs_log_odds - prior_log_odds;
      }

      double estimate() const {
  return 1.0 - (1.0 / (1.0 + exp(log_odds)));
      }

    }; // End of class: OccupancyEstimator

    EstimatorGrid* grid;

  public:

    OccupancyGrid(const Util::PropertyMap& props);

    ~OccupancyGrid();

    void laser(const Geometry::Point& source,
         const Geometry::Point& impact,
         double maxRange,
         double obstacleWidth,
         double p_given_obs,
         double p_given_free);

    void laser_scan(const Geometry::Point& source,
        const std::vector<Geometry::Point>& impacts,
        double maxRange,
        double obstacleWidth,
        double p_given_obs,
        double p_given_free);
    void sonar(const Geometry::Point& source,
         double angle,
         double range,
         double maxRange,
         double obstacleWidth,
         double width,
         double p_given_obs,
         double p_given_free);

    template<typename charT, typename traits>
    void write(std::basic_ostream<charT,traits>& out) const {
      // Write out an arbitrary number (so the form is the same as
      // Arak grid estimates).
      out << 0 << std::endl;
      // Write out the number of rows and columns.
      out << grid->numRows() << " " << grid->numCols() << std::endl;
      // Write out the boundary of the grid.
      out << grid->boundary() << std::endl;
      // Now write out each point color estimate.
      out << std::scientific << std::setw(16) << std::setprecision(12);
      for (int i = grid->numRows() - 1; i >= 0; i--) {
  for (int j = 0; j < grid->numCols(); j++) {
    const EstimatorGrid::Cell& cell = grid->getCell(i, j);
    const OccupancyEstimator* est = *(cell.getItemList().begin());
    out << est->estimate() << " ";
  }
  out << std::endl;
      }
    }

    template<typename charT, typename traits>
    OccupancyGrid(std::basic_istream<charT,traits>& in) {
      in >> skipcomments;
      int unused, num_rows, num_cols;
      in >> unused >> num_rows >> num_cols;
      double xmin, ymin, xmax, ymax, prob;
      in >> xmin >> ymin >> xmax >> ymax;
      Geometry::Rectangle r(xmin, ymin, xmax, ymax);
      grid = new EstimatorGrid(r, num_rows, num_cols);
      for (int i = num_rows - 1; i >= 0; i--)
  for (int j = 0; j < num_cols; j++) {
    EstimatorGrid::Cell& cell = grid->getCell(i, j);
    OccupancyEstimator* est = new OccupancyEstimator();
    in >> prob;
    est->prior_log_odds = 0.0; // TODO: parameter
    est->log_odds = log(prob / (1.0 - prob));
    cell.add(est);
  }
      assert(in.good());
    }

  };

}

#endif

---