range_obs.cpp

Go to the documentation of this file.

#include <set>
#include <fstream>
#include <CGAL/IO/Qt_widget.h>
#include "range_obs.hpp"
#include "jet.hpp"

using namespace Arak;
using namespace Arak::Geometry;

#define REGISTER_QUERY_POINTS

ArakPosteriorRangeObs::ArakPosteriorRangeObs(const ArakProcess& prior,
               const Arak::Util::PropertyMap& props)
  : ArakProcess(prior.getColoring()), prior(prior), updateId(0), lp(0.0) {
  using namespace Arak::Util;
  assert(parse(getp(props, "arak.range_obs.max_range"), maxRange));
  assert(parse(getp(props, "arak.range_obs.range_var"), rangeVar));
  assert(parse(getp(props, "arak.range_obs.prob_outlier"), pOutlier));
  assert(parse(getp(props, "arak.range_obs.prob_max_range_false_pos"), 
         pMaxFalsePos));
  assert(parse(getp(props, "arak.range_obs.prob_max_range_false_neg"), 
         pMaxFalseNeg));
  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;
#ifdef REGISTER_QUERY_POINTS
  std::vector<Geometry::Point> sources;
  sources.reserve(n);
#endif
  obs.reserve(n);
  const Geometry::Rectangle& bd = prior.getColoring().boundary();
  for (int i = 0; i < n; i++) {
    Geometry::Point p, q;
    in >> p >> q;
    /*
    // Ignore observations that are not contained in the coloring's
    // boundary. (This isn't correct, but oh well.)
    if (prior.getColoring().outside(p) || 
  prior.getColoring().outside(q)) 
      continue;
    */
    RangeObs* o = new RangeObs(p, q, maxRange);
#ifdef REGISTER_QUERY_POINTS
    sources.push_back(p);
#endif
    obs.push_back(o);
  }
  assert(!in.bad());
  // Build the index.
  int rows, cols;
  if (hasp(props, "arak.range_obs.rows") &&
      hasp(props, "arak.range_obs.cols")) {
    assert(parse(getp(props, "arak.range_obs.rows"), rows));
    assert(parse(getp(props, "arak.range_obs.cols"), cols));
  } else {
    // Choose the size of the grid based upon the number of
    // measurements and the dimensions of the boundary.
    const double obsPerCell = 3.0;
    const double numCells = double(obs.size()) / obsPerCell;
    const double ar = 
      CGAL::to_double((c.boundary().xmax() - c.boundary().xmin()) /
          (c.boundary().ymax() - c.boundary().ymin()));
    rows = int(sqrt(numCells / ar));
    cols = int(numCells) / rows;
    // std::cerr << "Using " << rows << "x" << cols << " grid." << std::endl;
  }
  obsIndex = new RangeObsIndex(bd, rows, cols);
  // Initialize the likelihood.
  for (std::vector<RangeObs*>::iterator it = obs.begin(); 
       it != obs.end(); it++)
    lp += updateObsLP(*(*it), true);
  // Register with the coloring for updates.
  c.addListener(*this);
#ifdef REGISTER_QUERY_POINTS
  QueryPointIndex index(sources, obs);
  c.addQueryPoints(index);
#endif
}

ArakPosteriorRangeObs::RangeObs::RangeObs(const Geometry::Point& source, 
            const Geometry::Point& target, 
            const double maxRange) {
  this->source = source;
  this->target = target;
  const Geometry::Vector v = target - source;
  range = sqrt(CGAL::to_double(v.squared_length()));
  max = source + v * (maxRange / range);
  isMaxRange = (range > maxRange - 1e-2);
  curContact = source;
  curRange = 0.0;
  lp = 0.0;
  updateId = 0;
}

bool ArakPosteriorRangeObs::retraceObs(RangeObs& o, bool init) const {
  Coloring::IntEdgeHandle e;
  Geometry::Kernel::FT sd;
  Geometry::Point oldContact = o.curContact;
  if (c.trace(o.source, o.max, e, o.curContact, sd)) {
    o.curRange = sqrt(CGAL::to_double(sd));
  } else {
    o.curContact = o.max;
    o.curRange = maxRange;
  }
  if (oldContact != o.curContact) {
    // Remove the observation's old entries in the index.
    for (RangeObs::CellEntryList::iterator it = o.entries.begin();
   it != o.entries.end(); it++)
      it->remove();
    o.unused.splice(o.unused.begin(), o.entries, 
        o.entries.begin(), o.entries.end());
    // Use a line cell iterator to find all cells the line currently
    // intersects.
    typedef RangeObsIndex::LineCellIterator Iterator;
    Iterator it, end;
    if (obsIndex->boundary().has_on_unbounded_side(o.curContact))
      it = Iterator(*obsIndex, o.source, o.curContact, Iterator::RAY());
    else
      it = Iterator(*obsIndex, o.source, o.curContact, Iterator::SEGMENT());
    while (it != end) {
      /*
      assert(std::find(it->getItemList().begin(), 
           it->getItemList().end(), 
           RangeObsHandle(o)) == it->getItemList().end());
      */
      RangeObsIndex::Cell::Entry entry = it->add(RangeObsHandle(o));
      // Add the entry to this observation's list, using an unused
      // list entry if possible.
      if (o.unused.empty())
  o.entries.push_front(entry);
      else {
  RangeObs::CellEntryList::iterator jt = o.unused.begin();
  *jt = entry;
  o.entries.splice(o.entries.begin(), o.unused, jt);
      }
      ++it;
    }
    return true;
  } else
    return false;
}

double ArakPosteriorRangeObs::updateObsLP(RangeObs& o, bool init) const {
  // Update the sequence number.
  o.updateId = updateId;
  // Retrace the observation.
  if (!retraceObs(o, init)) return 0.0;
  // Record the old log probability.
  const double oldLP = o.lp;
  // First check that the source is not colored black.
#ifdef REGISTER_QUERY_POINTS
  Color sourceColor = o.sourceColor;
#else
  Color sourceColor = c.color(o.source);
#endif
  if (sourceColor == BLACK) {
    o.lp = -1e10; // ln(1e-100);
  } else {
    if (o.curRange < maxRange) {
      // The correct observation is o.curRange.
      if (o.isMaxRange)
  // The probability of reflection.
  o.lp = ln(pMaxFalsePos);
      else {
  // Noisy measurement.
  double gl = exp(-pow(o.range - o.curRange, 2) / (2.0 * rangeVar)) /
    sqrt(rangeVar * 2.0 * M_PI);
  o.lp = ln((1.0 - pMaxFalsePos) * 
       ((pOutlier / maxRange) + (1.0 - pOutlier) * gl));
      }
      
    } else {
      // The correct observation is maxRange.
      if (o.isMaxRange)
  // The probability of getting a correct maxRange reading.
  o.lp = ln(1.0 - pMaxFalseNeg);
      else 
  // The probability of failing to get a maxRange reading.
  o.lp = ln(pMaxFalseNeg) - ln(maxRange);
    }
  }
  return o.lp - oldLP;
}

double ArakPosteriorRangeObs::logMeasure() const {
  return prior.logMeasure();
}

double ArakPosteriorRangeObs::potential() const {
  return prior.potential() - lp;
}

ArakPosteriorRangeObs::~ArakPosteriorRangeObs() { 
  for (std::vector<RangeObs*>::iterator it = obs.begin(); 
       it != obs.end(); it++)
    delete *it;
  delete obsIndex;
}

void ArakPosteriorRangeObs::visualize(CGAL::Qt_widget& widget) const {
  /*
  // Visualize the density of the index.
  const int maxSize = 100;
  for (int i = 0; i < obsIndex->numRows(); i++) 
    for (int j = 0; j < obsIndex->numCols(); j++) {
      const RangeObsIndex::Cell& cell = obsIndex->getCell(i, j);
      int size = cell.getItemList().size();
      // std::cerr << size << std::endl;
      unsigned char intensity = 
  int(std::min(double(size) / double(maxSize), 1.0) * 255.0);
      CGAL::Color fill(intensity, intensity, intensity);
      widget << fill << CGAL::FillColor(fill) << cell;
    }
  */
  // widget << CGAL::RED;
  for (std::vector<RangeObs*>::const_iterator it = obs.begin(); 
       it != obs.end(); it++) {
    const RangeObs* o = *it;
    const double max_likelihood = 5.0; // likelihoods can be greater than 1
    double normalized = std::min(exp(o->lp), max_likelihood) / max_likelihood;
    int n = int(double(jet_size - 1) * normalized);
    const CGAL::Color& color = jet_colors[n];
    /*
    if (c.color(o->source) == BLACK)
      widget << CGAL::YELLOW << Segment(o->source, o->target); 
    else {
      widget << CGAL::GREEN << Segment(o->source, o->target);
      widget << CGAL::RED << Segment(o->target, o->curContact);
    }
    */
    widget << color << Segment(o->source, o->target);
  }
}

void ArakPosteriorRangeObs::update(const Geometry::Point& a,
           const Geometry::Point& b,
           const Geometry::Point& c) {
  // Compute a set of observations that contains all observations
  // sensitive to the coloring in the supplied triangle.
  Geometry::Triangle t(a, b, c);
  RangeObsIndex::TriangleCellIterator it(*obsIndex, t), end;
  do {
    RangeObsIndex::Cell::ItemList& obsList = it->getItemList();
    typedef RangeObsIndex::Cell::ItemList::iterator Iterator;
    Iterator jt = obsList.begin();
    bool done = (jt == obsList.end());
    while (!done) {
      RangeObsHandle o = *jt;
      done = (++jt == obsList.end());
      if (o->updateId != updateId)
  lp += updateObsLP(*o);
    }
  } while (++it != end);
}

void ArakPosteriorRangeObs::recolored(const Geometry::Point& a,
              const Geometry::Point& b,
              const Geometry::Point& c) {
  updateId++;
  update(a, b, c);
}

void ArakPosteriorRangeObs::recolored(const Geometry::Point& a,
              const Geometry::Point& b,
              const Geometry::Point& c,
              const Geometry::Point& d) {
  updateId++;
  update(a, b, c);
  update(a, d, c);
}

---