Modules/ImageProcessor/GT2005ImageProcessor/GT2005EdgeSpecialist.cpp

Go to the documentation of this file.

/**
* @file GT2005EdgeSpecialist.cpp
* This file contains a class for Image Processing.
* @author Dirk Thomas
*/

#include "GT2005EdgeSpecialist.h"
#include "Representations/Perception/Image.h"
#include "Representations/Perception/EdgesPercept.h"
#include "GT2005ImageProcessorTools.h"
#include <list>
#include "Tools/Math/Common.h"

GT2005EdgeSpecialist::GT2005EdgeSpecialist
(
)
{
  // used in checkPoint
  greenBefore = false;
  whiteBefore = false;
  numberOfContinuousNoColor = 0;

  // used in addCandidate
  gradientThreshold = 8;
  double c = cos(3*pi/4);
  double s = sin(3*pi/4);
  const Matrix2x2<double> rotation(
    Vector2<double>(c,s),
    Vector2<double>(-s,c)
  );
  const Matrix2x2<double> invertY(
    Vector2<double>(1,0),
    Vector2<double>(0,-1)
  );
  referenceChange = invertY*rotation;
  // used in getEdgesPercept
  normDistance = 1.0;
  normProjection = 0.98;
  multipleAverageDistance = 2.0;
}

void GT2005EdgeSpecialist::reset()
{
  numOfEdgePoints = 0;
}

void GT2005EdgeSpecialist::resetLine()
{
  greenBefore = false;
  whiteBefore = false;
  numberOfContinuousNoColor = 0;
}

void GT2005EdgeSpecialist::checkPoint(const Vector2<int> point, const colorClass color, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, const Image& image)
{
  Vector2<int> pointOnField; // position on field, relative to robot
  if(Geometry::calculatePointOnField(point.x, point.y, cameraMatrix, image.cameraInfo, pointOnField))
  {
    switch(color)
    {
      case green:
        lastGreen = point;
        lastGreenField = pointOnField;
        if(whiteBefore)
        {
          // trigger specialist (white2green)
          addCandidate(lastGreen, image);
        }
        greenBefore = true;
        whiteBefore = false;
        numberOfContinuousNoColor = 0;
        break;
      case white:
        lastWhite = pointOnField;
        if(greenBefore)
        {
          // trigger specialist (green2white)
          addCandidate(lastGreen, image);
        }
        greenBefore = false;
        whiteBefore = true;
        numberOfContinuousNoColor = 0;
        break;
      case noColor:
        numberOfContinuousNoColor++;
        break;
      default:
        greenBefore = false;
        whiteBefore = false;
        numberOfContinuousNoColor = 0;
        break;
    } // end switch
  } // end if

}

void GT2005EdgeSpecialist::addCandidate(const Vector2<int> point, const Image& image)
{
  gradientThreshold = 3;

  // 2x2 pixel gradient
  Vector2<double> grad = Vector2<double>(image.image[point.y][0][point.x] - image.image[(point.y)-1][0][(point.x)-1] ,
    image.image[(point.y)-1][0][point.x] - image.image[point.y][0][(point.x)-1]);
  /*
  // 3x3 pixel gradient - not better than 2x2
  double grad = Vector2<double>(
    image.image[point.y-1][0][point.x-1]
    + 2*image.image[point.y][0][point.x-1]
    + image.image[point.y+1][0][point.x-1]
    - image.image[point.y-1][0][point.x+1]
    - 2*image.image[point.y][0][point.x+1]
    - image.image[point.y+1][0][point.x+1],
    image.image[point.y-1][0][point.x-1]
    + 2*image.image[point.y-1][0][point.x]
    + image.image[point.y-1][0][point.x+1]
    - image.image[point.y+1][0][point.x-1]
    - 2*image.image[point.y+1][0][point.x]
    - image.image[point.y+1][0][point.x+1]
  );
  gradientThreshold *= 2;
  */

  if (grad.abs() > gradientThreshold)
  {
    if (numOfEdgePoints < maxNumberOfEdgePoints)
    {
      edgePoints[numOfEdgePoints].offset = point;
      Vector2<double> gradRotated = referenceChange * grad;
      edgePoints[numOfEdgePoints].line = Geometry::Line(point, gradRotated.normalize());
      edgePoints[numOfEdgePoints].weight = 0;
      edgePoints[numOfEdgePoints].belongsToLineNo = -1;
      DOT(imageProcessor_edges, point.x, point.y, 0, 1);
      NDOT("gt05ip:edges-candidate-points", point.x, point.y, 0, 1);
      numOfEdgePoints++;
    }
    else
    {
      OUTPUT(idText,text,"EdgeSpecialist: candidate-point drop (raise maxNumberOfEdgePoints)");
    }
  }
}

void GT2005EdgeSpecialist::getEdgesPercept(EdgesPercept& percept, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, const Image& image)
{
  int i, j;

  NDECLARE_DEBUGDRAWING( "gt05ip:edges-candidate-points" , "drawingOnImage" , "show candidate edge points" );
  NDECLARE_DEBUGDRAWING( "gt05ip:edges-points" , "drawingOnImage" , "show selected edge points" );
  NDECLARE_DEBUGDRAWING( "gt05ip:edges" , "drawingOnImage" , "show detected edges" );

  // for all lines, calculate how many other lines are similar/near in sense of this special norm
  bool similar[maxNumberOfEdgePoints][maxNumberOfEdgePoints];
  int lineHeight[maxNumberOfEdgePoints];
  for(i = 1; i < numOfEdgePoints; i++)
  {
    lineHeight[i] = Geometry::calculateLineSize(edgePoints[i].offset, cameraMatrix, image.cameraInfo);
    for(j = 0; j < i; j++)
    {
      // similar/near if distance is small and normal is in the same direction
      bool sim = false;
      // test projection first - to remove unneeded calculations of "expensive" distance
      double projection = edgePoints[i].line.direction * edgePoints[j].line.direction;
      if(projection > normProjection)
      {
        // distance point to line:
        // http://mo.mathematik.uni-stuttgart.de/kurse/kurs8/seite44.html
        // NOTE: this is because in fact, the line.direction is here the *normal* to the real line
        double distance1 = (edgePoints[j].line.base - edgePoints[i].line.base) * edgePoints[i].line.direction;
        distance1 = distance1 * distance1;
        if(distance1 < normDistance * lineHeight[i])
        {
          double distance2 = (edgePoints[i].line.base - edgePoints[j].line.base) * edgePoints[j].line.direction;
          distance2 = distance2 * distance2;
          sim = (distance2 < normDistance * lineHeight[j]);
        }
      }
      similar[i][j] = sim;
      similar[j][i] = sim;
      // draw similarity-lines
      /*if(sim) LINE(imageProcessor_edges, 
        edgePoints[i].offset.x, 
        edgePoints[i].offset.y, 
        edgePoints[j].offset.x, 
        edgePoints[j].offset.y,
        0, 0, 2
      );*/
    }
  }

  // analyse detected points and extract edges
  int maxWeight = 0;
  int colorArrow = 0;
  int colorEdge = 0;
  for(int m = 0; m < 10; m++)
  {
    int edgePointWithHighestWeight = -1;
    maxWeight = 0;

    // for all points, calculate how many other are similar
    for(i = 0; i < numOfEdgePoints; i++)
    {
      if(edgePoints[i].belongsToLineNo == -1) // only if point is not yet matched to an edge
      for (j = i + 1; j < numOfEdgePoints; j++)
      {
        if(edgePoints[j].belongsToLineNo == -1) // only if point is not yet matched to an edge
        if(similar[i][j])
        {
          // weight added to both points because of reflexivity of distance-norm
          edgePoints[i].weight++;
          edgePoints[j].weight++;

          if(edgePoints[i].weight > maxWeight)
          {
            maxWeight = edgePoints[i].weight;
            edgePointWithHighestWeight = i;
          }
          if(edgePoints[j].weight > maxWeight)
          {
            maxWeight = edgePoints[j].weight;
            edgePointWithHighestWeight = j;
          }
        }
      }
    }

    // break detection if no more highest is found
    if(maxWeight<1) break;

    // draw point with highest weight
    DOT(imageProcessor_edges, edgePoints[edgePointWithHighestWeight].offset.x, edgePoints[edgePointWithHighestWeight].offset.y, colorArrow, colorArrow);
    NDOT("gt05ip:edges-points", edgePoints[edgePointWithHighestWeight].offset.x, edgePoints[edgePointWithHighestWeight].offset.y, colorArrow, colorArrow);

    // store points which belong to the current line
    Vector2<int>start, end;
    int numberOfEdges = 0;
    double averageDistance;
    struct Edge
    {
      //int numberOfEdgePoints;
      std::list <int> pointIndices;
    };
    std::list <int>::iterator it;
    Edge edges[30];
    for(i = 0; i < 30; i++) edges[i].pointIndices.clear();

    // show lines that belong to the line with highest weight
    for(i = 0; i < numOfEdgePoints; i++)
    {
      if(edgePoints[i].belongsToLineNo == -1)
      {
        if(similar[i][edgePointWithHighestWeight])
        {
          ARROW(imageProcessor_edges, 
            edgePoints[i].line.base.x,
            edgePoints[i].line.base.y,
            edgePoints[i].line.base.x + edgePoints[i].line.direction.x * 10,
            edgePoints[i].line.base.y + edgePoints[i].line.direction.y * 10, 1, 1, colorArrow
          );
          NARROW("gt05ip:edges-points",
            edgePoints[i].line.base.x,
            edgePoints[i].line.base.y,
            edgePoints[i].line.base.x + edgePoints[i].line.direction.x * 10,
            edgePoints[i].line.base.y + edgePoints[i].line.direction.y * 10, 1, 1, colorArrow
          );
          // store points in ordered collection based on x- or y-values
          for(it = edges[numberOfEdges].pointIndices.begin( ); it != edges[numberOfEdges].pointIndices.end( ); it++)
          {
            if(edgePoints[*it].offset.x > edgePoints[i].offset.x) break;
          }
          edges[numberOfEdges].pointIndices.insert(it, i);
        }
      }
    }
    colorArrow++;
    numberOfEdges++;

    // check points on the single edge and filter outsiders and/or split in multiple parts

    // compute avg-distance
    if(edges[0].pointIndices.size() <= 1) continue;
    start = edgePoints[edges[0].pointIndices.front()].offset;
    end = edgePoints[edges[0].pointIndices.back()].offset;
    //if(edges[0].numberOfEdgePoints > 0) averageDistance = Geometry::distance(start, end) / edges[0].numberOfEdgePoints;
    averageDistance = Geometry::distance(start, end) / edges[0].pointIndices.size();
    // split edge in multple parts if distance of two neighboured points is much bigger than avg-distance
    int current, last, size;
    
    it = edges[numberOfEdges-1].pointIndices.begin();
    bool finished = (it != edges[numberOfEdges-1].pointIndices.end());
    while(finished)
    {
      size = edges[numberOfEdges-1].pointIndices.size();
      last = *it;
      it++;
      if(it != edges[numberOfEdges-1].pointIndices.end())
      {
        current = *it;
        int x1 = edgePoints[last].offset.x;
        int x2 = edgePoints[current].offset.x;
        if(x1 + multipleAverageDistance * averageDistance < x2)
        {
          // split of *it and following points into next edge
          edges[numberOfEdges].pointIndices.splice(
            edges[numberOfEdges].pointIndices.begin(),
            edges[numberOfEdges-1].pointIndices,
            it,
            edges[numberOfEdges-1].pointIndices.end()
          );
          it = std::list <int>::iterator(edges[numberOfEdges].pointIndices.begin());
          numberOfEdges++;
        }
      }
      finished = (it != edges[numberOfEdges-1].pointIndices.end());
    }


    // for each resulting edge check size, draw and generate percept
    for(int j = 0; j < numberOfEdges; j++)
    {
      // draw edge (and generate edge-percept) if based on a couple of points
      if(edges[j].pointIndices.size() > 4)
      {
        // generate edge-point-percept, "disable" all points for any further computation
        for(it = edges[j].pointIndices.begin(); it != edges[j].pointIndices.end(); it++)
        {
          i = *it;
          Vector2<double> pointOnField; //position on field, relative to robot
          if(Geometry::calculatePointOnField(edgePoints[i].offset.x, edgePoints[i].offset.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, pointOnField))
          {
            edgePoints[i].belongsToLineNo = edgePointWithHighestWeight;
            edgePoints[i].weight = 0;
          }
        }
        start = edgePoints[edges[j].pointIndices.front()].offset;
        end = edgePoints[edges[j].pointIndices.back()].offset;
        LINE(imageProcessor_edges, 
          start.x, 
          start.y, 
          end.x, 
          end.y,
          1, 1, colorEdge
        );
        NLINE("gt05ip:edges",
          start.x, 
          start.y, 
          end.x, 
          end.y,
          1, 1, colorEdge
        );
        Vector2<double> point1OnField, point2OnField;
        Geometry::calculatePointOnField(start.x, start.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, point1OnField);
        Geometry::calculatePointOnField(end.x, end.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, point2OnField);
        Vector2<double> grad = edgePoints[edges[j].pointIndices.front()].line.direction;
        Vector2<double> dir(grad.y, -grad.x);
        if ((edgePoints[edges[j].pointIndices.back()].line.base - edgePoints[edges[j].pointIndices.front()].line.base) * dir > 0)
          percept.add(point2OnField, point1OnField, end, start);
        else
          percept.add(point1OnField, point2OnField, start, end);
      }
    }
    colorEdge++;
    if(edgePoints[edgePointWithHighestWeight].belongsToLineNo == -1) edgePoints[edgePointWithHighestWeight].belongsToLineNo = -2;
  }
}

---