<commit_before>#include "RansacLineDetector.h"

RansacLineDetector::RansacLineDetector()
{
  // initialize some stuff here
  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:draw_edgels_field", "", false);
  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:draw_lines_field", "", false);
  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:fit_and_draw_ellipse_field", "", false);

  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:draw_circle_field", "", false);

  getDebugParameterList().add(&params);
}

RansacLineDetector::~RansacLineDetector()
{
  getDebugParameterList().remove(&params);
}

void RansacLineDetector::execute()
{
  getLinePercept().reset();

  // copy the edgels
  // todo: can this be optimized?
  //outliers.assign(getLineGraphPercept().edgels.begin(), getLineGraphPercept().edgels.end());
  outliers.resize(getLineGraphPercept().edgelsOnField.size());
  getLinePercept().edgelLineIDs.resize(getLineGraphPercept().edgelsOnField.size());

  for(size_t i = 0; i < getLineGraphPercept().edgelsOnField.size(); ++i) {
    outliers[i] = i;
    getLinePercept().edgelLineIDs[i] = -1;
  }

  bool foundLines = false;

  std::vector<size_t> inliers;

  for(int i = 0; i < params.maxLines; ++i)
  {
    Math::LineSegment result;
    if(ransac(result, inliers) > 0)
    {
      foundLines = true;
      LinePercept::FieldLineSegment fieldLine;
      fieldLine.lineOnField = result;
      getLinePercept().lines.push_back(fieldLine);
    }
    else {
      break;
    }
    inliers.clear();
  }

  // circle
  Vector2d circResult;
  inliers.clear();
  int bestInlierCirc = ransacCircle(circResult, inliers);

  if (bestInlierCirc) {
    getLinePercept().middleCircleCenter = circResult;
  }

  DEBUG_REQUEST("Vision:RansacLineDetector:draw_circle_field",
    FIELD_DRAWING_CONTEXT;
    if (bestInlierCirc) {
      PEN("000099", 5);

      for(size_t i: inliers)
      {
        const Edgel& e = getLineGraphPercept().edgelsOnField[i];
        CIRCLE(e.point.x, e.point.y, 30);
      }

      PEN("009900", 10);

      CIRCLE(circResult.x, circResult.y, 750);
    }
  );

  DEBUG_REQUEST("Vision:RansacLineDetector:draw_edgels_field",
    FIELD_DRAWING_CONTEXT;

    for(size_t i=0; i<getLineGraphPercept().edgelsOnField.size(); i++)
    {
      const Edgel& e = getLineGraphPercept().edgelsOnField[i];

      if(getLinePercept().edgelLineIDs[i] > -1) {
        std::string color;
        switch(getLinePercept().edgelLineIDs[i]%3) {
          case 0: color = "00FF00"; break;
          case 1: color = "0000FF"; break;
          default: color = "00FFFF"; break;
        }
        PEN(color,2);
      } else {
        PEN("FF0000",2);
      }

      if(e.point.x > 0 || e.point.y > 0) {
        CIRCLE(e.point.x, e.point.y, 25);
      }
      PEN("000000",0.1);
      LINE(e.point.x, e.point.y, e.point.x + e.direction.x*100.0, e.point.y + e.direction.y*100.0);
    }

  );

  DEBUG_REQUEST("Vision:RansacLineDetector:draw_lines_field",
    FIELD_DRAWING_CONTEXT;

    if (foundLines) {
      for(size_t i=0; i<getLinePercept().lines.size(); i++)
      {
        std::string color;
        switch(i%3) {
          case 0: color = "00FF00"; break;
          case 1: color = "0000FF"; break;
          default: color = "00FFFF"; break;
        }
        PEN(color, 50);
        LINE(
          getLinePercept().lines[i].lineOnField.begin().x, getLinePercept().lines[i].lineOnField.begin().y,
          getLinePercept().lines[i].lineOnField.end().x, getLinePercept().lines[i].lineOnField.end().y);
      }
    }

  );

  DEBUG_REQUEST("Vision:RansacLineDetector:fit_and_draw_ellipse_field",
    FIELD_DRAWING_CONTEXT;
    // fit ellipse
    Ellipse circResult;

    int bestInlierCirc = ransacEllipse(circResult);

    if (bestInlierCirc > 0) {
      {
        
        double c[2];
        circResult.getCenter(c);

        double a[2];
        circResult.axesLength(a);

        PEN("009900", 50);

        //CIRCLE(c[0], c[1], 30);
        OVAL_ROTATED(c[0], c[1], a[0], a[1], circResult.rotationAngle());

        PEN("0000AA", 20);
        for(size_t i=0; i<circResult.x_toFit.size(); i++) {
          CIRCLE(circResult.x_toFit[i], circResult.y_toFit[i], 20);
        }
      }

      {
        Vector2d c = circResult.getCenter();
        Vector2d a = circResult.axesLength();
        
        Vector2d ax(a.x,0.0); 
        ax.rotate(circResult.rotationAngle());

        Vector2d ay(0.0,a.y); 
        ay.rotate(circResult.rotationAngle());

        PEN("FF0000", 20);
        CIRCLE(c.x, c.y, 50);
        LINE(c.x, c.y, c.x+ax.x, c.y+ax.y);
        LINE(c.x, c.y, c.x+ay.x, c.y+ay.y);


        Vector2d point(500,500);
        Vector2d p = circResult.test_project(point);
        PEN("FF0000", 20);
        CIRCLE(point.x, point.y, 50);
        PEN("0000FF", 20);
        CIRCLE(p.x, p.y, 50);
        PEN("000000", 10);
        LINE(point.x, point.y, p.x, p.y);
    }
    }
  );


}<commit_msg>cleanup<commit_after>#include "RansacLineDetector.h"

RansacLineDetector::RansacLineDetector()
{
  // initialize some stuff here
  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:draw_edgels_field", "", false);
  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:draw_lines_field", "", false);
  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:fit_and_draw_ellipse_field", "", false);

  DEBUG_REQUEST_REGISTER("Vision:RansacLineDetector:draw_circle_field", "", false);

  getDebugParameterList().add(&params);
}

RansacLineDetector::~RansacLineDetector()
{
  getDebugParameterList().remove(&params);
}

void RansacLineDetector::execute()
{
  getLinePercept().reset();
  outliers.resize(getLineGraphPercept().edgelsOnField.size());
  getLinePercept().edgelLineIDs.resize(getLineGraphPercept().edgelsOnField.size());
  // copy the edgels
  for(size_t i = 0; i < getLineGraphPercept().edgelsOnField.size(); ++i) {
    outliers[i] = i;
    getLinePercept().edgelLineIDs[i] = -1;
  }

  std::vector<size_t> inliers;

  for(int i = 0; i < params.maxLines; ++i)
  {
    Math::LineSegment result;
    if(ransac(result, inliers) > 0)
    {
      LinePercept::FieldLineSegment fieldLine;
      fieldLine.lineOnField = result;
      getLinePercept().lines.push_back(fieldLine);
    }
    else {
      break;
    }
    inliers.clear();
  }

  // circle
  Vector2d circResult;
  inliers.clear();
  int bestInlierCirc = ransacCircle(circResult, inliers);

  if (bestInlierCirc) {
    getLinePercept().middleCircleCenter = circResult;
  }

  DEBUG_REQUEST("Vision:RansacLineDetector:draw_circle_field",
    FIELD_DRAWING_CONTEXT;
    if (bestInlierCirc) {
      PEN("000099", 5);

      for(size_t i: inliers)
      {
        const Edgel& e = getLineGraphPercept().edgelsOnField[i];
        CIRCLE(e.point.x, e.point.y, 30);
      }

      PEN("009900", 10);

      CIRCLE(circResult.x, circResult.y, 750);
    }
  );

  DEBUG_REQUEST("Vision:RansacLineDetector:draw_edgels_field",
    FIELD_DRAWING_CONTEXT;

    for(size_t i=0; i<getLineGraphPercept().edgelsOnField.size(); i++)
    {
      const Edgel& e = getLineGraphPercept().edgelsOnField[i];

      if(getLinePercept().edgelLineIDs[i] > -1) {
        std::string color;
        switch(getLinePercept().edgelLineIDs[i]%3) {
          case 0: color = "00FF00"; break;
          case 1: color = "0000FF"; break;
          default: color = "00FFFF"; break;
        }
        PEN(color,2);
      } else {
        PEN("FF0000",2);
      }

      if(e.point.x > 0 || e.point.y > 0) {
        CIRCLE(e.point.x, e.point.y, 25);
      }
      PEN("000000",0.1);
      LINE(e.point.x, e.point.y, e.point.x + e.direction.x*100.0, e.point.y + e.direction.y*100.0);
    }

  );

  DEBUG_REQUEST("Vision:RansacLineDetector:draw_lines_field",
    FIELD_DRAWING_CONTEXT;
    for(size_t i=0; i<getLinePercept().lines.size(); i++)
    {
      std::string color;
      switch(i%3) {
        case 0: color = "00FF00"; break;
        case 1: color = "0000FF"; break;
        default: color = "00FFFF"; break;
      }
      PEN(color, 50);
      LINE(
        getLinePercept().lines[i].lineOnField.begin().x, getLinePercept().lines[i].lineOnField.begin().y,
        getLinePercept().lines[i].lineOnField.end().x, getLinePercept().lines[i].lineOnField.end().y);
    }

  );

  DEBUG_REQUEST("Vision:RansacLineDetector:fit_and_draw_ellipse_field",
    FIELD_DRAWING_CONTEXT;
    // fit ellipse
    Ellipse ellipseResult;

    int bestInlierCirc = ransacEllipse(ellipseResult);

    if (bestInlierCirc > 0) {
      {
        
        double c[2];
        ellipseResult.getCenter(c);

        double a[2];
        ellipseResult.axesLength(a);

        PEN("009900", 50);

        //CIRCLE(c[0], c[1], 30);
        OVAL_ROTATED(c[0], c[1], a[0], a[1], ellipseResult.rotationAngle());

        PEN("0000AA", 20);
        for(size_t i=0; i<ellipseResult.x_toFit.size(); i++) {
          CIRCLE(ellipseResult.x_toFit[i], ellipseResult.y_toFit[i], 20);
        }
      }

      {
        Vector2d c = ellipseResult.getCenter();
        Vector2d a = ellipseResult.axesLength();
        
        Vector2d ax(a.x,0.0); 
        ax.rotate(ellipseResult.rotationAngle());

        Vector2d ay(0.0,a.y); 
        ay.rotate(ellipseResult.rotationAngle());

        PEN("FF0000", 20);
        CIRCLE(c.x, c.y, 50);
        LINE(c.x, c.y, c.x+ax.x, c.y+ax.y);
        LINE(c.x, c.y, c.x+ay.x, c.y+ay.y);


        Vector2d point(500,500);
        Vector2d p = ellipseResult.test_project(point);
        PEN("FF0000", 20);
        CIRCLE(point.x, point.y, 50);
        PEN("0000FF", 20);
        CIRCLE(p.x, p.y, 50);
        PEN("000000", 10);
        LINE(point.x, point.y, p.x, p.y);
    }
    }
  );


}
{'fraction_non_alphanumeric': 0.15687292358803986, 'fraction_numerical': 0.03280730897009967, 'mean_word_length': 2.857829395274329, 'pattern_counts': {'":': 0, '<': 15, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1468081', 'n_tokens_mistral': 3929, 'n_tokens_neox': 3573, 'n_words': 512}