sonar_obs.hpp

Go to the documentation of this file.

#ifndef _SONAR_OBS_HPP
#define _SONAR_OBS_HPP

#include <iostream>
#include "properties.hpp"
#include "coloring.hpp"
#include "query_point.hpp"
#include "arak.hpp"
#include "grid.hpp"

namespace Arak {

  struct SonarImpact {
    
    double proj_angle;
    
    double min_range;
    
    double visible_angle;
    
    Geometry::Direction dmin;
    
    Geometry::Direction dmax;

    Geometry::Point pmin;
    
    Geometry::Point pmax;
    
    Geometry::Point projection;

    SonarImpact() : proj_angle(0.0), 
        min_range(0.0), 
        visible_angle(0.0),
        pmin(CGAL::ORIGIN), 
        pmax(CGAL::ORIGIN),
        projection(CGAL::ORIGIN) { }
    
    SonarImpact(const Geometry::Point& source, 
    const Geometry::Direction& dmin,
    const Geometry::Direction& dmax,
    const Geometry::Point& pmin,
    const Geometry::Point& pmax) : 
      dmin(dmin), dmax(dmax), pmin(pmin), pmax(pmax) { 
      // Compute the point on the impact face closest to the source.
      Geometry::Line face_line(pmin, pmax);
      projection = face_line.projection(source);
      // Project the projection into the segment.
      if (CGAL::collinear_are_ordered_along_line(projection, pmin, pmax)) 
  projection = pmin;
      else if (CGAL::collinear_are_ordered_along_line(pmin, pmax, projection))
  projection = pmax;
      // Compute the impact information.
      Geometry::Direction fd = face_line.direction();
      Geometry::Direction psd = Geometry::Line(projection, source).direction();
      this->proj_angle = (atan2(CGAL::to_double(psd.dy()), 
        CGAL::to_double(psd.dx())) - 
        atan2(CGAL::to_double(fd.dy()), 
        CGAL::to_double(fd.dx())));
      this->min_range = 
  sqrt(CGAL::to_double(CGAL::squared_distance(source, projection)));
      if (dmin != dmax) {
  // Compute the visible angle.  First convert the directions to
  // angles.
  double amin = atan2(CGAL::to_double(dmin.dy()), 
          CGAL::to_double(dmin.dx()));
  double amax = atan2(CGAL::to_double(dmax.dy()), 
          CGAL::to_double(dmax.dx()));
  // Now compute the smaller of the two angles between them.
  double adiff1 = amax - amin;
  if (adiff1 < 0.0) adiff1 += 2 * M_PI;
  if (adiff1 >= 2 * M_PI) adiff1 -= 2 * M_PI;
  double adiff2 = amin - amax;
  if (adiff2 < 0.0) adiff2 += 2 * M_PI;
  if (adiff2 >= 2 * M_PI) adiff2 -= 2 * M_PI;
  this->visible_angle = std::min(fabs(adiff1), fabs(adiff2));
      } else
  this->visible_angle = 0.0;
    }
      
    SonarImpact(const Geometry::Point& source, 
    const Geometry::Point& pmin,
    const Geometry::Direction& dir,
    const Geometry::Point& corner,
    const Geometry::Point& pmax) : 
      dmin(dir), dmax(dir), pmin(corner), pmax(corner), projection(corner) { 
      this->proj_angle = 0.0; // ?
      this->min_range = 
  sqrt(CGAL::to_double(CGAL::squared_distance(source, corner)));
      this->visible_angle = 0.0;
    }

    bool isCorner() const { return (visible_angle == 0.0); }

    bool operator<(const SonarImpact& other) const {
      if (min_range < other.min_range)
  return true;
      else if (min_range == other.min_range) {
  return (isCorner() && !other.isCorner());
      } else
  return false;
    } 
  }; // End of class: SonarImpact

  void computeSonarImpacts(const Coloring& coloring,
         const Geometry::Point& vertex,
         const Geometry::Direction& min,
         const Geometry::Direction& max,
         const Geometry::Kernel::FT maxRange,
         std::vector<SonarImpact>& impacts,
         double* maxImpactRange = NULL);

  bool computeFirstSonarImpact(const Coloring& coloring,
             const Geometry::Point& vertex,
             const Geometry::Direction& min,
             const Geometry::Direction& max,
             const Geometry::Kernel::FT maxRange,
             SonarImpact& impact);

  class ArakPosteriorSonarObs : public ArakProcess, 
        public Coloring::Listener {

  protected:

    class SonarObs;
    typedef PointerHandle<SonarObs> SonarObsHandle;

    typedef Grid<SonarObsHandle> SonarObsIndex;

    class SonarObs : public QueryPointListener {

    public:
      
      Geometry::Point source;

      Geometry::Direction min;

      Geometry::Direction max;

      double range;

      Color sourceColor;

      double lp;

      Geometry::Triangle relevantRegion;

      typedef SonarObsIndex::Cell::Entry CellEntry;

      typedef std::list<CellEntry> CellEntryList;

      CellEntryList entries;

      CellEntryList unused;

      unsigned long int updateId;

      SonarObs(const Geometry::Point& source, 
         const double angle,
         const double range,
         const double aperture,
         const double maxRange);

      virtual ~SonarObs() {};

      virtual void recolor(Color color) {
  sourceColor = color;
      }

    }; // End of class: ArakPosteriorSonarObs::SonarObs

    const ArakProcess& prior;

    std::vector<SonarObs*> obs;

    SonarObsIndex* obsIndex;

    unsigned long int updateId;

    double aperture;

    double maxRange;

    double prob_max_range;

    double unif_outlier_weight;

    double exp_outlier_coef;

    double face_const_coef;

    double face_proj_angle_coef;

    double face_min_range_coef;

    double face_vis_angle_coef;

    double corner_const_coef;

    double corner_range_coef;

    double faceRelErrStDev;

    double cornerRelErrStDev;

    double lp;
    
    double updateObsLP(SonarObs& o, bool init = false) const;

  public:

    ArakPosteriorSonarObs(const ArakProcess& prior, 
        const Arak::Util::PropertyMap& props);

    virtual ~ArakPosteriorSonarObs();

    const Geometry::Rectangle& boundary() const { 
      return obsIndex->boundary(); 
    }

    virtual double scale() const { return prior.scale(); }

    virtual double logMeasure() const;

    virtual double potential() const;

    virtual void visualize(CGAL::Qt_widget& widget) const;

    virtual void update(const Geometry::Point& a,
      const Geometry::Point& b,
      const Geometry::Point& c);
      
    virtual void recolored(const Geometry::Point& a,
         const Geometry::Point& b,
         const Geometry::Point& c);
      
    virtual void recolored(const Geometry::Point& a,
         const Geometry::Point& b,
         const Geometry::Point& c,
         const Geometry::Point& d);
    
    inline double returnProb(const SonarImpact& impact) const {
      if (impact.isCorner()) 
  return 1.0 / (1.0 + 
          exp(-(corner_const_coef + 
          (corner_range_coef * impact.min_range))));
      else
  return 1.0 / (1.0 + 
          exp(-(face_const_coef + 
          (face_proj_angle_coef * log(sin(impact.proj_angle))) +
          (face_min_range_coef * impact.min_range) +
          (face_vis_angle_coef * impact.visible_angle))));
    }

    inline double distLikelihood(const SonarImpact& impact,
         const double obs) const {
      double stdev = impact.min_range * 
  (impact.isCorner() ? cornerRelErrStDev : faceRelErrStDev);
      double var = stdev * stdev;
      double dist = (obs - impact.min_range);
      // TODO: this should be a truncated Gaussian
      return exp(- (dist * dist) / (2.0 * var)) / (stdev * sqrt(2.0 * M_PI));
    } 

    template <typename ImpactIterator>
    inline double likelihood(double range,
           ImpactIterator begin,
           ImpactIterator end) const {
      bool is_max_range = (range >= maxRange);
      double no_prev_returns_prob = 1.0;
      double likelihood = 0.0;
      // Step through the sonar contacts in sorted order, adding in
      // the likelihood of the observation given the contact.
      while (begin != end) {
  const SonarImpact& impact = *begin;
  double return_prob = returnProb(impact);
  if (!is_max_range)
    likelihood += return_prob * no_prev_returns_prob *
      distLikelihood(impact, range);
  no_prev_returns_prob *= (1.0 - return_prob);
  ++begin;
      }
      // Add in the likelihood that comes from the background model,
      // which is an outlier density with a spike at the max-range
      // value.
      if (is_max_range)
  likelihood += no_prev_returns_prob * prob_max_range;
      else
  likelihood += no_prev_returns_prob * (1.0 - prob_max_range)
    * (// uniform outlier model
       (unif_outlier_weight / maxRange)
             +
       // exponential outlier model
       (1.0 - unif_outlier_weight) * 
       exp_outlier_coef * exp(-exp_outlier_coef * (maxRange - range)));
      // Return the likelihood.
      return likelihood;
    }

  }; // End of class: ArakPosteriorSonarObs 

  CGAL::Qt_widget& operator<<(CGAL::Qt_widget& widget, 
            ArakPosteriorSonarObs& p);

}

#endif

---