sonar_viz.cpp

Go to the documentation of this file.

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include "parsecl.hpp"
#include "jet.hpp"
#include "properties.hpp"
#include "geometry.hpp"
#include "coloring.hpp"
#include "cone_tracing.hpp"
#include "sonar_obs.hpp"
#include <CGAL/IO/Qt_widget.h>
#include <CGAL/IO/Qt_widget_layer.h>
#include <CGAL/IO/Qt_widget_standard_toolbar.h>
#include <CGAL/IO/Qt_widget_get_segment.h>
#include <qapplication.h>
#include <qmainwindow.h>
#include <CGAL/Aff_transformation_2.h>

using namespace Arak;

Coloring* coloring;
ArakPrior* prior;
ArakPosteriorSonarObs* process;

Geometry::Point point;
double maxRange;
double aperture;
bool firstImpactOnly = false;

std::vector<Arak::SonarImpact> impacts;
std::vector<std::pair<double,double> > likelihood_fn;

class My_Layer : public CGAL::Qt_widget_layer {
  void draw(){
    // First draw the map.
    coloring->visualize(*widget, false, false, true, false);
    // Now draw face impacts.
    double no_prev_returns_prob = 1.0;
    for (unsigned int i = 0; i < impacts.size(); i++) {
      const SonarImpact& impact = impacts[i];
      double return_prob = process->returnProb(impact);
      double first_return_prob = no_prev_returns_prob * return_prob;
      no_prev_returns_prob *= (1.0 - return_prob);
      unsigned char index = int(first_return_prob * double(Arak::jet_size));
      const CGAL::Color& c = Arak::jet_colors[index];
      if (!impact.isCorner()) 
  *widget << CGAL::LineWidth(0) << c << CGAL::FillColor(c) 
    << Geometry::Triangle(point, impact.pmin, impact.pmax);
    }
    // Now draw corner impacts.
    no_prev_returns_prob = 1.0;
    for (unsigned int i = 0; i < impacts.size(); i++) {
      const SonarImpact& impact = impacts[i];
      double return_prob = process->returnProb(impact);
      double first_return_prob = no_prev_returns_prob * return_prob;
      no_prev_returns_prob *= (1.0 - return_prob);
      unsigned char index = int(first_return_prob * double(Arak::jet_size));
      const CGAL::Color& c = Arak::jet_colors[index];
      if (impact.isCorner()) 
  *widget << CGAL::LineWidth(2) << c 
    << Geometry::Segment(point, impact.pmin);
    }
    // Visualize the likelihood function.
    double xmin = widget->x_min();
    double xmax = widget->x_max();
    double ymin = widget->y_min();
    double ymax = widget->y_max();

    typedef CGAL::Aff_transformation_2<Geometry::Kernel> Transformation;
    Arak::Geometry::Vector origin(xmin + 0.1 * (xmax - xmin),
          ymin + 0.1 * (ymax - ymin));
    Transformation trans(CGAL::TRANSLATION, origin);
    

    Transformation scale(CGAL::SCALING, (xmax - xmin) / (1.2 * maxRange));
    Transformation composed = trans * scale;
    *widget << CGAL::RED 
      << Geometry::Segment(CGAL::ORIGIN, Geometry::Point(maxRange, 0)).transform(composed)
      << Geometry::Segment(CGAL::ORIGIN, Geometry::Point(0, 1.0)).transform(composed);
    Geometry::Point prev = CGAL::ORIGIN;
    for (unsigned int i = 0; i < likelihood_fn.size(); i++) {
      double range = likelihood_fn[i].first;
      double likelihood = likelihood_fn[i].second;
      Geometry::Point next = Geometry::Point(range, likelihood);
      if (prev != CGAL::ORIGIN) 
  *widget << Geometry::Segment(prev, next).transform(composed);
      prev = next;
    }
    
  }
};

class My_Window : public QMainWindow {
  Q_OBJECT
public:
  My_Window(int x, int y){
    widget = new CGAL::Qt_widget(this, "CGAL Qt_widget");
    setCentralWidget(widget);
    resize(x,y);
    widget->attach(&get_segment);
    widget->attach(&v);
    connect(widget, SIGNAL(new_cgal_object(CGAL::Object)), 
            this, SLOT(get_new_object(CGAL::Object)));
    std_toolbar = new CGAL::Qt_widget_standard_toolbar(widget, this,
                   "Standard Toolbar");
    Geometry::Rectangle bd = coloring->boundary();
    widget->set_window(CGAL::to_double(bd.xmin()), 
           CGAL::to_double(bd.xmax()), 
           CGAL::to_double(bd.ymin()), 
           CGAL::to_double(bd.ymax()));
  };
private:  //members
  CGAL::Qt_widget_get_segment<Geometry::Kernel> get_segment;
  My_Layer v;
  CGAL::Qt_widget *widget;
  CGAL::Qt_widget_standard_toolbar *std_toolbar;
private slots:
  void get_new_object(CGAL::Object obj) {
    Geometry::Segment seg;
    if (CGAL::assign(seg, obj)) { 
      point = seg.source();
      Geometry::Vector vec = seg.to_vector();
      double angle = atan2(CGAL::to_double(vec.y()), 
         CGAL::to_double(vec.x()));
      double angle_min = angle - aperture / 2.0;
      double angle_max = angle + aperture / 2.0;
      if (firstImpactOnly) {
  SonarImpact impact;
  impacts.clear();
  if (Arak::computeFirstSonarImpact(*coloring, point, 
            Geometry::Direction(cos(angle_min), 
                    sin(angle_min)), 
            Geometry::Direction(cos(angle_max), 
                    sin(angle_max)), 
            Geometry::Kernel::FT(maxRange), 
            impact)) 
    impacts.push_back(impact);
      } else {
  Arak::computeSonarImpacts(*coloring, point, 
          Geometry::Direction(cos(angle_min), 
                  sin(angle_min)), 
          Geometry::Direction(cos(angle_max), 
                  sin(angle_max)), 
          Geometry::Kernel::FT(maxRange), 
          impacts);
      }
      likelihood_fn.clear();
      for (double range = 0.0; range < maxRange; range += 0.01) {
  double likelihood = 
    process->likelihood(range, impacts.begin(), impacts.end());
  likelihood_fn.push_back(std::pair<double,double>(range, likelihood));
      }
      likelihood_fn.push_back(std::pair<double,double>(maxRange, 
                   process->likelihood(maxRange, impacts.begin(), impacts.end())));
      widget->redraw();
    }
  }
}; //endclass

#include <sonar_viz.moc> // MOC source file: sonar_viz.cpp

int sonar_viz(int argc, char **argv,
        const Arak::Util::PropertyMap& props) {
  // Allocate the coloring.
  coloring = new Arak::Coloring(props);
  // Read the coloring from standard input.
  std::cin >> *coloring;
  if (!std::cin.good()) {
    std::cerr << "Error reading coloring from standard input." << std::endl;
    return 1;
  }
  // Form the sonar observation posterior.
  prior = new ArakPrior(*coloring, props);
  process = new ArakPosteriorSonarObs(*prior, props);

  QApplication app(argc, argv);
  My_Window *w = new My_Window(600, 600);
  app.setMainWidget(w);
  w->show();
  int result = app.exec();
  delete process;
  delete prior;
  delete coloring;
  return result;
}

int main(int argc, char** argv) {
  using namespace Arak::Util;
  // Parse the command line.
  CommandLine cl;
  CommandLine::MultiParameter<std::string>
    propFiles("-p", "--prop-file", "specifies a property file");
  cl.add(propFiles);
  CommandLine::MultiParameter<std::string>
    propDefs("-D", "--define-prop", "defines a property value");
  cl.add(propDefs);
  CommandLine::Option
    firstOpt("-f", "--first-only", "use first sonar impact only");
  cl.add(firstOpt);

  if (!cl.parse(argc, argv, std::cerr)) {
    cl.printUsage(std::cerr);
    exit(1);
  }
  firstImpactOnly = firstOpt.supplied();

  Arak::Util::PropertyMap properties;

  // Parse the properties files.
  const std::vector<std::string>& propFilePaths = propFiles.values();
  for (std::vector<std::string>::const_iterator it = propFilePaths.begin();
       it != propFilePaths.end(); it++) {
    std::string path = *it;
    std::ifstream in(path.data());
    in >> properties;
  }
  // Parse the overriding property definitions.
  const std::vector<std::string>& propDefinitions = propDefs.values();
  for (std::vector<std::string>::const_iterator it = propDefinitions.begin();
       it != propDefinitions.end(); it++) {
    std::string def = *it;
    std::istringstream in(def);
    in >> properties;
  }

  std::cout << "Properties:" << std::endl << properties << std::endl;

  assert(parse(getp(properties, "arak.sonar_obs.max_range"), maxRange));
  assert(parse(getp(properties, "arak.sonar_obs.aperture"), aperture));

  // Invoke the program.
  return sonar_viz(argc, argv, properties);
}

---