sonar_obs.cpp

Go to the documentation of this file.

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

using namespace Arak;
using namespace Arak::Geometry;

#define REGISTER_QUERY_POINTS

// #define FIRST_IMPACT_ONLY

class ScanEventProcessor {

protected:

  SonarImpact impact;
  SonarImpact prevImpact;
  SonarImpact corner;
  Geometry::Point source;
  const double maxRange;
  std::vector<SonarImpact>* impacts;
  Geometry::Direction dir;

public:

  ScanEventProcessor(const Geometry::Point& source,
         const Geometry::Direction& min,
         const double maxRange,
         std::vector<SonarImpact>& impacts)
    : source(source), maxRange(maxRange), impacts(&impacts), dir(min) { 
    impact.min_range = -1.0; // invalid marker
    prevImpact.min_range = -1.0; // invalid marker
    corner.min_range = -1.0; // invalid marker
  }

  ScanEventProcessor() : maxRange(-1.0), impacts(NULL) { }

  const ScanEvent& operator=(const ScanEvent& event) {
    // First check for a corner.  We view a corner when there is a
    // previous impact and its max point is the same as the new
    // event's min point.
    if ((this->prevImpact.min_range >= 0.0) &&
  (this->prevImpact.pmax == event.pmin)) {
      // Populate the cached corner impact.
      corner = SonarImpact(this->source, 
         prevImpact.pmin, 
         event.dmin,
         event.pmin, 
         event.pmax);
    } else {
      // Invalidate the cached corner impact.
      corner.min_range = -1.0;
    }
    // Construct the sonar impact from the event information.  TODO:
    // pmin and pmax must be trimmed to lie inside the depth-truncated
    // sonar cone!
    Geometry::Point pmin = event.pmin;
    Geometry::Point pmax = event.pmax;
    SonarImpact tmp(this->source, event.dmin, event.dmax, pmin, pmax);
    // If the impact is close enough, update this iterator.
    if (tmp.min_range <= this->maxRange) {
      // Update the iterator.
      this->impact = tmp;
      dir = event.dmax;
    } else {
      // Invalidate the current impact.
      this->impact.min_range = -1.0;
    }
    return event;
  }

  ScanEventProcessor operator++() {
    // If there is a valid corner impact, record it first.
    if (corner.min_range >= 0.0) {
      impacts->push_back(corner);
      corner.min_range = -1.0;
    }
    if (impact.min_range >= 0.0) {
      // Record the current impact.
      impacts->push_back(impact);
      // Cache it as the previous impact.
      prevImpact = impact;
    }
    return *this;
  }

  ScanEventProcessor operator++(int) {
    ScanEventProcessor tmp(*this);
    ++(*this);
    return tmp;
  }
}; // End of class: ScanEventProcessor

void Arak::computeSonarImpacts(const Coloring& coloring,
             const Geometry::Point& vertex,
             const Geometry::Direction& dmin,
             const Geometry::Direction& dmax,
             const Geometry::Kernel::FT maxRange,
             std::vector<SonarImpact>& impacts,
             double* maxImpactRange) {
  impacts.clear();
  ScanEventProcessor processor(vertex, dmin, CGAL::to_double(maxRange), impacts);
  coneTrace(coloring, vertex, dmin, dmax, maxRange, processor);
  std::sort(impacts.begin(), impacts.end());
  // If required, compute the maximum impact range.
  if (maxImpactRange != NULL) {
    Geometry::Direction dir = dmin;
    typedef std::vector<SonarImpact>::const_iterator Iterator;
    *maxImpactRange = 0.0;
    for (Iterator it = impacts.begin(); it != impacts.end(); ++it) {
      if (it->dmin != dir) {
  *maxImpactRange = std::numeric_limits<double>::infinity();
  break;
      }
      *maxImpactRange = 
  std::max(*maxImpactRange, 
     CGAL::to_double(CGAL::squared_distance(vertex, it->pmax)));
      *maxImpactRange = 
  std::max(*maxImpactRange,
     CGAL::to_double(CGAL::squared_distance(vertex, it->pmin)));
      dir = it->dmax;
    }
    if (dir != dmax)
      *maxImpactRange = std::numeric_limits<double>::infinity();
    if (*maxImpactRange != std::numeric_limits<double>::infinity())
      *maxImpactRange = sqrt(*maxImpactRange);
  }
}

bool Arak::computeFirstSonarImpact(const Coloring& coloring,
           const Geometry::Point& vertex,
           const Geometry::Direction& min,
           const Geometry::Direction& max,
           const Geometry::Kernel::FT maxRange,
           SonarImpact& impact) {
  // TODO: this is hacked! clean it up.

  // Compute the set of edges that intersect the sonar cone.
  Geometry::Triangle outer = outerApprox(vertex, min, max, maxRange);
  const Coloring::InteriorEdgeIndex& index = 
    coloring.getInteriorEdgeIndex();
  Coloring::InteriorEdgeIndex::ConstTriangleCellIterator 
    it(index, outer), end;
  typedef std::set<Coloring::IntEdgeHandle> EdgeHandleSet;
  static EdgeHandleSet edges; // Warning: this is not thread-safe
  edges.clear();
  while (it != end) {
    const Coloring::InteriorEdgeIndex::Cell::ItemList& obsList = 
      it->getItemList();
    edges.insert(obsList.begin(), obsList.end());
    ++it;
  }
  // If there are no edges, we are done.
  if (edges.empty()) return false;
  // Otherwise, scan through the edges, looking for the closest
  // impact.
  bool found = false;
  ScanEdge se;
  double closest = std::numeric_limits<double>::infinity();
  for (EdgeHandleSet::iterator it = edges.begin(); it != edges.end(); it++) {
    // Create a scan edge object. TODO: this object should be moved
    // here from cone_tracing.hpp.
    se.init(vertex, min, max, *it);
    // If the scan edge does not intersect the scan cone, ignore it.
    if (!cones_intersect(min, max, se.dmin, se.dmax)) continue;
    // Construct a sonar impact from the scan edge.
    SonarImpact i = SonarImpact(vertex, se.dmin, se.dmax, se.pmin, se.pmax);
    if (i.min_range >= closest) continue;
    // This is the closest impact so far.  First transform it to a
    // corner if the projection point is an endpoint of the (full) face.
    if ((i.projection == se.pmin) &&
  ((se.pmin == (*it)->getNextVertex()->point()) ||
   (se.pmin == (*it)->getPrevVertex()->point()))) {
      i.pmax = i.pmin;
      i.dmax = i.dmin;
      i.visible_angle = 0.0;
    } else if ((i.projection == se.pmax) &&
         ((se.pmax == (*it)->getNextVertex()->point()) ||
    (se.pmax == (*it)->getPrevVertex()->point()))) {
      i.pmin = i.pmax;
      i.dmin = i.dmax;
      i.visible_angle = 0.0;
    }
    impact = i;
    closest = impact.min_range;
    found = true;
  }
  return found;
}

ArakPosteriorSonarObs::ArakPosteriorSonarObs(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.sonar_obs.aperture"), aperture));
  assert(parse(getp(props, "arak.sonar_obs.max_range"), maxRange));
  assert(parse(getp(props, "arak.sonar_obs.prob_max_range"), 
         prob_max_range));
  assert(parse(getp(props, "arak.sonar_obs.unif_outlier_weight"), 
         unif_outlier_weight));
  assert(parse(getp(props, "arak.sonar_obs.exp_outlier_coef"), 
         exp_outlier_coef));
  assert(parse(getp(props, "arak.sonar_obs.face_rel_err_std_dev"), faceRelErrStDev));
  assert(parse(getp(props, "arak.sonar_obs.corner_rel_err_std_dev"), cornerRelErrStDev));
  assert(parse(getp(props, "arak.sonar_obs.return_model.face.const_coef"), 
         face_const_coef));
  assert(parse(getp(props, "arak.sonar_obs.return_model.face.proj_angle_coef"), 
         face_proj_angle_coef));
  assert(parse(getp(props, "arak.sonar_obs.return_model.face.min_range_coef"), 
         face_min_range_coef));
  assert(parse(getp(props, "arak.sonar_obs.return_model.face.vis_angle_coef"), 
         face_vis_angle_coef));
  assert(parse(getp(props, "arak.sonar_obs.return_model.corner.const_coef"), 
         corner_const_coef));
  assert(parse(getp(props, "arak.sonar_obs.return_model.corner.range_coef"), 
         corner_range_coef));
  // Read in the number of observations.
  const std::string& datafile_path = getp(props, "arak.sonar_obs.data_file");
  std::ifstream in(datafile_path.data());
  assert(in.good());
  int n;
  in >> n;
  // Build the index.
  int rows, cols;
  Geometry::Rectangle bd = getColoring().boundary();
  if (hasp(props, "arak.sonar_obs.rows") &&
      hasp(props, "arak.sonar_obs.cols")) {
    assert(parse(getp(props, "arak.sonar_obs.rows"), rows));
    assert(parse(getp(props, "arak.sonar_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(n) / obsPerCell;
    const double ar = 
      CGAL::to_double((bd.xmax() - bd.xmin()) /
          (bd.ymax() - bd.ymin()));
    rows = int(sqrt(numCells / ar));
    cols = int(numCells) / rows;
    // std::cerr << "Using " << rows << "x" << cols << " grid." << std::endl;
  }
  obsIndex = new SonarObsIndex(bd, rows, cols);
#ifdef REGISTER_QUERY_POINTS
  std::vector<Geometry::Point> sources;
#endif
  // Read in the observations.
  for (int i = 0; i < n; i++) {
    Geometry::Point source;
    double angle, range;
    in >> source >> angle >> range;
#ifdef REGISTER_QUERY_POINTS
    sources.push_back(source);
#endif
    obs.push_back(new SonarObs(source, angle, range, aperture, maxRange));
  }
  assert(!in.bad());
  // Initialize the likelihood.
  for (std::vector<SonarObs*>::iterator it = obs.begin(); 
       it != obs.end(); it++)
    lp += updateObsLP(*(*it), true);
  // Register with the coloring for updates.
  getColoring().addListener(*this);
#ifdef REGISTER_QUERY_POINTS
  QueryPointIndex index(sources, obs);
  c.addQueryPoints(index);
#endif
}

ArakPosteriorSonarObs::SonarObs::SonarObs(const Geometry::Point& source, 
            const double angle,
            const double range,
            const double aperture,
            const double maxRange) {
  this->source = source;
  double minAngle = angle - aperture / 2.0;
  double maxAngle = angle + aperture / 2.0;
  this->min = Geometry::Direction(cos(minAngle), sin(minAngle));
  this->max = Geometry::Direction(cos(maxAngle), sin(maxAngle));
  this->range = range;
  lp = 0.0;
  updateId = 0;
}

double ArakPosteriorSonarObs::updateObsLP(SonarObs& o, bool init) const {
  const double oldLP = o.lp;
  // Update the sequence number.
  o.updateId = updateId;
  double maxImpact;
#ifdef FIRST_IMPACT_ONLY
  SonarImpact impact;
  bool isImpact = computeFirstSonarImpact(c, o.source, o.min, o.max, 
            Geometry::Kernel::FT(maxRange),
            impact);
  maxImpact = 
    isImpact ? impact.min_range : std::numeric_limits<double>::infinity();
#else
  static std::vector<SonarImpact> impacts; // WARNING: static is not threadsafe
  computeSonarImpacts(c, o.source, o.min, o.max, 
          Geometry::Kernel::FT(maxRange), impacts,
          &maxImpact);
#endif
  // std::cerr << impacts.size() << " " << maxImpact << std::endl;
  // Remove the observation's old entries in the index.
  for (SonarObs::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 triangle cell iterator to find all cells the cone currently
  // intersects.  First determine the maximum range to which the
  // observation is sensitive.  This is the least of the following
  // quantities:
  // (1) the maximum range measurement
  // (2) the maximum impact obtained in the previous scan (which 
  //     may coincide with (1))
  // (3) the range at which the likelihood of the observation is 
  //     effectively zero.
  double sensitiveRange = std::min(maxImpact, maxRange);
  double effMaxRange = o.range + 5.0 * std::max(cornerRelErrStDev, 
            faceRelErrStDev);
  sensitiveRange = std::min(sensitiveRange, effMaxRange);
  // std::cerr << sensitiveRange << std::endl;
  o.relevantRegion = outerApprox(o.source, o.min, o.max, sensitiveRange);
  SonarObsIndex::TriangleCellIterator it(*obsIndex, o.relevantRegion), end;
  while (it != end) {
    SonarObsIndex::Cell::Entry entry = it->add(SonarObsHandle(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 {
      SonarObs::CellEntryList::iterator jt = o.unused.begin();
      *jt = entry;
      o.entries.splice(o.entries.begin(), o.unused, jt);
    }
    ++it;
  }
  /*
  if (impacts.size() > 0) {
    std::cerr << "*** " << o.source << std::endl;
    for (unsigned int i = 0; i < impacts.size(); i++) {
      std::cerr << "(" << impacts[i].proj_angle << ", " 
    << impacts[i].min_range << ", " 
    << impacts[i].visible_angle << ") ";
    }
    std::cerr << std::endl;
  }
  */
  // First check that the source is not colored black.
  if (c.color(o.source) == BLACK) {
    o.lp = -1e10;
    return o.lp - oldLP;
  }
  // Compute the likelihood of the range measurement.
#ifdef FIRST_IMPACT_ONLY
  o.lp = ln(likelihood(o.range, &impact, 
           isImpact ? &impact + 1 : &impact));
#else
  o.lp = ln(likelihood(o.range, impacts.begin(), impacts.end()));
#endif
  return o.lp - oldLP;
}

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

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

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

void ArakPosteriorSonarObs::visualize(CGAL::Qt_widget& widget) const {
  /*
  // Visualize the density of the index.
  const int maxSize = 500;
  for (int i = 0; i < obsIndex->numRows(); i++) 
    for (int j = 0; j < obsIndex->numCols(); j++) {
      const SonarObsIndex::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;
    }
  */
  for (std::vector<SonarObs*>::const_iterator it = obs.begin(); 
       it != obs.end(); it++) {
    const SonarObs* o = *it;
    if (o->range >= maxRange) continue;
    /*
    // Visualize the sensitive regions.
    widget << CGAL::noFill << CGAL::PURPLE << o->relevantRegion;
    */
    Geometry::Point p = o->source;
    const double one_deg = (M_PI / 180.0); 
    const double min_angle = atan2(CGAL::to_double(o->min.dy()), 
           CGAL::to_double(o->min.dx()));
    const double max_angle = atan2(CGAL::to_double(o->max.dy()), 
           CGAL::to_double(o->max.dx()));
    // CGAL::Color color = (o->range >= maxRange) ? CGAL::ORANGE : CGAL::RED;
    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);
    CGAL::Color color = jet_colors[n];
    for (double angle = min_angle + one_deg; 
   angle < max_angle; angle += one_deg) {
      Geometry::Point p = 
  o->source + Geometry::Vector(o->range * cos(angle - one_deg),
             o->range * sin(angle - one_deg));
      Geometry::Point q = 
  o->source + Geometry::Vector(o->range * cos(angle),
             o->range * sin(angle));
      widget << color << Geometry::Segment(p, q);
    }
  }
}

void ArakPosteriorSonarObs::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);
  SonarObsIndex::TriangleCellIterator it(*obsIndex, t), end;
  // int count = 0;
  do {
    SonarObsIndex::Cell::ItemList& obsList = it->getItemList();
    typedef SonarObsIndex::Cell::ItemList::iterator Iterator;
    Iterator jt = obsList.begin();
    bool done = (jt == obsList.end());
    while (!done) {
      SonarObsHandle o = *jt;
      done = (++jt == obsList.end());
      if ((o->updateId != updateId) &&
    (CGAL::do_intersect(t, o->relevantRegion))) {
  lp += updateObsLP(*o);
  // count++;
      }
    }
  } while (++it != end);
  // std::cerr << "Updated " << count << " sonar likelihoods." << std::endl;
}

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

void ArakPosteriorSonarObs::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);
}

---