houghDetect.cpp

/*
  ハフ変換による直線検出
  Satofumi KAMIMURA
  $Id$
*/

#include "vutils.h"
#include "mURGCtrl.h"
#include <stdio.h>
#include <stdlib.h>


static void drawUrgCapture(std::vector<VXV::Grid3D>& points) {
  vmonitor::drawPoints(points, White);
}


static void drawLines(std::vector<CoordinateCtrl::line_t>& lines) {
  for (std::vector<CoordinateCtrl::line_t>::iterator it = lines.begin();
       it != lines.end(); ++it) {
    VXV::Grid from = VXV::Grid(it->position.x, it->position.y);
    VXV::Grid to;
    double rad = it->position.zt.to_rad();
    to.x = it->position.x + static_cast<int>(it->length * cos(rad));
    to.y = it->position.y + static_cast<int>(it->length * sin(rad));
    vmonitor::drawLine(from, to, Green);
  }
}


int main(int argc, char *argv[]) {
  try {
    mURGCtrl urg;
    if (initConnection(&urg, argc, argv, false) < 0) {
      exit(1);
    }
    vmonitor::show();

    unsigned long firstTicks = vmonitor::getTicks();
    while (vmonitor::getTicks() - firstTicks < 1000*5) {
      urg.capture();

      vmonitor::clear();
      std::vector<CoordinateCtrl::line_t> lines;
      urg.convert();
      detectLines(lines, urg.crd_points);
      drawLines(lines);
      drawUrgCapture(urg.crd_points);

      VXV::Delay(100);
    }
  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}