LEDDetector.cpp

#include "LEDDetector.hh"
#include <Base/Image/ImageConvert.hh>

using namespace std;
using namespace BIAS;

#define MASK_X               17   // filter size
#define RADIUS_SPOT_RANGE    70   // max. space between spots
#define MASK_WHATISMINBRIGHT 150  // gray values for thresholding
#define MASK_WAHTISMAXDARK   50   // gray values for thresholding

LEDDetector::
LEDDetector()
{
  lookup = NULL;
  lookupsize = 0;
}

LEDDetector::
~LEDDetector()
{
  if (lookup) delete[] lookup;
}

void LEDDetector::
GetNeighbours(std::vector<std::vector<BIAS::Vector3<double> > > &neighbours)
{
  neighbours = nextpixel;
}

void LEDDetector::
GetAttributes (std::vector<double> &minX, std::vector<double> &maxX,
               std::vector<double> &maxValues)
{
  minX = minxcoords;
  maxX = maxxcoords;
  maxValues = maxvalues;
}

int LEDDetector::Compute(const BIAS::Image<unsigned char> &image,
                         std::vector<BIAS::Vector3<double> > &results)
{
  if (!results.empty()) {
    resultpixel_old.clear();
    for (unsigned int i = 0; i < results.size(); i++) {
      resultpixel_old.push_back(results[i]);
    }
  }

  BIAS::Image<unsigned char> tmpImage;
  BIAS::ImageConvert::ToGrey(image, tmpImage);
  BIAS::ImageConvert::BIAS2ipl(tmpImage, gray_threshold);
  BIAS::ImageConvert::BIAS2ipl(tmpImage, gray_orig);

  // initialize data
  width = gray_threshold->width;
  height = gray_threshold->height;
  if (lookupsize < width * height) {
    delete[] lookup;
    lookupsize = width * height;
    lookup = new unsigned char[lookupsize];
  }
  memset(lookup, 0, width * height * sizeof(unsigned char));
  minxcoords.clear();
  maxxcoords.clear();
  maxvalues.clear();

  CvScalar m = cvAvg(gray_orig, NULL);
  mean = (double)m.val[0];
  //mean = cvMean(gray_orig, 0);
  cvThreshold(gray_orig, gray_threshold, 210, 255, CV_THRESH_BINARY);
  step = gray_threshold->widthStep / sizeof(unsigned char);
  data = (unsigned char *)gray_threshold->imageData;
  resultpixel.clear();

  int res = 0;
  if (!resultpixel_old.empty())
    res = Recycle_();
  if (res != 0 || resultpixel_old.empty())
    TemplateMatching_();
  results = resultpixel;

  // release data
  tmpImage.Release();
  cvReleaseImage(&gray_threshold);
  cvReleaseImage(&gray_orig);

  return res;
}

int LEDDetector::Recycle_()
{
  bool foundatold = false;
  bool foundnn = true;
  for (unsigned int i = 0; i < resultpixel_old.size(); i++) {
    if (foundnn) {
      // if value in gray image is white and the loopup array is black
      BIAS::Vector3<double> f = resultpixel_old[i];
      int fx = (int)f[0], fy = (int)f[1];
      if (data[fy*step + fx] > mean-10 && lookup[fy*width+fx] == 0) {
        // find new neighbourhood
        Search_(fx, fy);
      } else {
        // if not a spot anymore
        foundatold = true;
        // check the pixel of neighborhood to find a bright pixel
        int a = -1, b = -1;
        int res = CheckNeighboursIfWhite_(fx, fy, a, b);
        if (res == 0 && a != -1 && b != -1) {
          Search_(a, b);
        } else {
          foundatold = false;
        }
        if (res == -1) {
          foundnn = false;
          foundatold = false;
        }
      }
    }
  }
  resultpixel_old.clear();
  return (!foundatold && !foundnn) ? -1 : 0;
}

int LEDDetector:: CheckNeighboursIfWhite_(int x, int y, int& a, int& b)
{
  int h = 1;
  bool found = false;
  while (!found && h < 100) { // @todo use maxblobsize instead?
    int res = CheckNeighboursIfWhite_inter_(x+h, y);
    if (res == 0) {
      found = true; a = x+h; b=y;
    } else {
      res = CheckNeighboursIfWhite_inter_(x+h, y-h);
      if (res == 0) {
        found = true; a = x+h; b=y-h;
      } else {
        res = CheckNeighboursIfWhite_inter_(x+h, y+h);
        if (res == 0) {
          found = true; a = x+h; b=y+h;
        } else {
          res = CheckNeighboursIfWhite_inter_(x, y+h);
          if (res == 0) {
            found = true; a = x; b = y+h;
          } else {
            res = CheckNeighboursIfWhite_inter_(x, y-h);
            if (res == 0) {
              found = true; a = x; b = y-h;
            } else {
              res = CheckNeighboursIfWhite_inter_(x-h, y+h);
              if (res == 0) {
                found = true; a=x-h;b=y+h;
              } else {
                res = CheckNeighboursIfWhite_inter_(x-h, y);
                if (res == 0) {
                  found = true; a = x-h; b=y;
                } else {
                  res = CheckNeighboursIfWhite_inter_(x-h, y-h);
                  if (res == 0) {
                    found = true; a = x-h; b = y-h;
                  }
                }
              }
            }
          }
        }
      }
    }
    h++;
  }
  return found ? 0 : -1;
}

int LEDDetector::CheckNeighboursIfWhite_inter_(int x, int y)
{
  if (x > 0 && y > 0 && x < (gray_threshold->width)-1 && y < (gray_threshold->height)-1)
    if (data[y*step+x] > MASK_WHATISMINBRIGHT && lookup[y*width+x] == 0)
      return 0;
  return -1;
}

void LEDDetector::Search_(int x, int y)
{
  if (lookup[y*width+x] == 0) {
    recursivedepth = 0;
    minx = 2000;
    maxx = 0;
    maxvalue = 0;
    count = 0;
    pixelweightvaluex = 0;
    pixelweightvaluey = 0;
    valuesum = 0;
    blobsize = 0;

    int res = CheckNeighbours_(x, y);
    if (res == 0) {
      minxcoords.push_back(minx);
      maxxcoords.push_back(maxx);
      maxvalues.push_back(maxvalue);

      BIAS::Vector3<double> erg;
      double o = pixelweightvaluex / count; // = width
      double p = pixelweightvaluey / count; // = height
      double u = valuesum / count;
      valuesum = 0;
      o = o / u;
      p = p / u;
      erg[0] = p;
      erg[1] = o;

      //double b, c;
      //c = modf(muex, &b); muex=b; if (c > 0.5) muex++;

      int g = int(o);
      double d = o - g;
      if (d > 0.5) g++;
      int h = int(p);
      d = p - h;
      if (d > 0.5) h++;
      CvScalar w = cvGet2D(gray_orig, g, h);
      erg[2] = (double)w.val[0];
      CvScalar n;
      n.val[0] = 255; n.val[1] = 0; n.val[2] = 0; n.val[3] = 0;
      cvSet2D(gray_threshold, g, h, n);
      lookup[y*width+x] = 255;
      nextpixel.push_back(whitepixel);
      whitepixel.clear();
      resultpixel.push_back(erg);
      whitepixel.clear();
    }
  }
}

int LEDDetector::CheckNeighbours_(int x, int y)
{
  recursivedepth++;
  if (recursivedepth < 1000 && blobsize < 100) {
    if (x > 0 && y > 0 && x < (gray_threshold->width)-1 && y < (gray_threshold->height)-1) {
      if (data[y*step+x]> MASK_WAHTISMAXDARK && lookup[y*width+x] == 0) {
        if (x < minx)
          minx = x;
        if (x > maxx)
          maxx = x;
        blobsize++;
        lookup[y*width+x] = 127;
        BIAS::Vector3<double> a;
        a[0] = x;
        a[1] = y;
        double value;
        CvScalar s = cvGet2D(gray_orig, y, x);
        value = s.val[0];
        a[2]= value;
        valuesum += value;
        if (value > maxvalue)
          maxvalue = value;

        // set size, request max. value, backup
        whitepixel.push_back(a);
        count++;
        pixelweightvaluex+=value*y;
        pixelweightvaluey+=value*x;

        CheckNeighbours_( x-1, y);
        CheckNeighbours_( x+1, y);
        CheckNeighbours_( x, y-1);
        CheckNeighbours_( x, y+1);
      }
    }
    return 0;
  }
  return -1;
}

void LEDDetector::TemplateMatching_()
{
  // no recycling possible or first run, search pixel by pixel in whole image
  int y = gray_threshold->height;
  int x = gray_threshold->width*gray_threshold->nChannels;
  int s = (MASK_X-1) / 2;
  for (int i = s; i < y-s; i+=2) {
    for (int j = s; j < x-s; j+=2) {
      blobs = 0;
      bool repeat = true;
      int inside = 0;
      if (data[i*step+j] > MASK_WAHTISMAXDARK && lookup[i*width+j] == 0){
        int xh = i-s;
        int xd = i+s;
        int yh = j-s;
        int yd = j+s;
        for (; xh < xd; xh++) {
          // if not black outside
          if (data[xh*step+yh] > MASK_WAHTISMAXDARK) {
            repeat = false;
            break;
          }
        }
        if (repeat) {
          for (; yh < yd; yh++) {
            if (data[xh*step+yh] > MASK_WAHTISMAXDARK) {
              repeat = false;
              break;
            }
          }
          if (repeat) {
            xd = i - s;
            if (repeat) {
              for (; xh > xd; xh--) {
                if (data[xh*step+yh] > MASK_WAHTISMAXDARK) {
                  repeat = false;
                  break;
                }
              }
              yd = j-s;
              if (repeat) {
                for (; yh > yd; yh--) {
                  if (data[xh*step+yh] > MASK_WAHTISMAXDARK) {
                    repeat = false;
                    break;
                  }
                }
                if (repeat) {
                  // if white inside
                  for (int t = -3; t < 5; t++) {
                    for (int r = -3; r < 5; r++) {
                      if (data[(i+r)*step+(j+t)] > MASK_WAHTISMAXDARK) {
                        inside++;
                      }
                    }
                  }
                }
              }
            }
          }
        }
      }
      if (repeat && inside > 3) {
        // if not bright anymore search for neighbours
        Search_(j,i);
        lookup[i*width+j] = 255;
        inside = 0;
      }
    }
  }
}