<commit_before>/* 
 * File:   LineGraphProvider.cpp
 * Author: <NAME>
 */

#include "LineGraphProvider.h"

#include "Tools/CameraGeometry.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Math/Geometry.h"

LineGraphProvider::LineGraphProvider()
:
  cameraID(CameraInfo::Top)
{
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:edgel_pairs", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_neighbors", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_neighbors_field", "mark the edgels on the image", false);

  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:edgel_cluster", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_lines", "draw the esimated lines in the image", false);

  //DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_compas", "draw compas direcion based on the edgel directions", false);

  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_line_graph", "", false);

  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_extended_line_graph", "", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_extended_line_graph_top", "", false);

  getDebugParameterList().add(&parameters);
}


LineGraphProvider::~LineGraphProvider()
{
  getDebugParameterList().remove(&parameters);
}


void LineGraphProvider::execute(CameraInfo::CameraID id)
{
  cameraID = id;

  calculatePairsAndNeigbors(getScanLineEdgelPercept().pairs, edgelPairs, edgelNeighbors, parameters.edgelSimThreshold);


  // calculate the projection for all edgels
  edgelProjectionsBegin.resize(getScanLineEdgelPercept().pairs.size());
  edgelProjectionsEnd.resize(getScanLineEdgelPercept().pairs.size());
  for(size_t i = 0; i < getScanLineEdgelPercept().pairs.size(); i++) 
  {  
    const ScanLineEdgelPercept::EdgelPair& pair = getScanLineEdgelPercept().pairs[i];

    const Edgel& end = getScanLineEdgelPercept().edgels[pair.end];
    const Edgel& begin = getScanLineEdgelPercept().edgels[pair.begin];
    
    // NOTE: edgels are assumed to be always below horizon and so the projection should be allways valid
    CameraGeometry::imagePixelToFieldCoord(
      getCameraMatrix(), getCameraInfo(),
      end.point.x,
      end.point.y,
      0.0,
      edgelProjectionsEnd[i]);

    CameraGeometry::imagePixelToFieldCoord(
      getCameraMatrix(), getCameraInfo(),
      begin.point.x,
      begin.point.y,
      0.0,
      edgelProjectionsBegin[i]);

    //pair.projectedWidth = Vector2d(beginPointOnField - endPointOnField).abs();
  }

  // extend line graph
  lineGraphsIds.clear();
  extendLineGraph(edgelNeighbors);

  std::vector<std::vector<EdgelD>>& graph = (cameraID == CameraInfo::Top)?
        getLineGraphPercept().lineGraphsTop:
        getLineGraphPercept().lineGraphs;

  // add the graphs to the representation
  for(const std::vector<int>& subgraph : lineGraphsIds)
  {
    graph.emplace_back();
    for(size_t left=0, right=1; right < subgraph.size(); ++left, ++right) {

      const Vector2d& edgelLeft = (edgelProjectionsBegin[subgraph[left]] + edgelProjectionsEnd[subgraph[left]])*0.5;
      const Vector2d& edgelRight = (edgelProjectionsBegin[subgraph[right]] + edgelProjectionsEnd[subgraph[right]])*0.5;

      EdgelD edgel;
      edgel.point = Vector2d(edgelLeft + edgelRight)*0.5;
      edgel.direction = (edgelRight - edgelLeft).normalize();

      graph.back().push_back(edgel);
    }
  }

  // calculate the LineGraphPercept
  for(size_t j = 0; j < edgelPairs.size(); j++)
  {
    const EdgelPair& edgelPair = edgelPairs[j];
    const Vector2d& edgelLeft = (edgelProjectionsBegin[edgelPair.left] + edgelProjectionsEnd[edgelPair.left])*0.5;
    const Vector2d& edgelRight = (edgelProjectionsBegin[edgelPair.right] + edgelProjectionsEnd[edgelPair.right])*0.5;

    const double projectedWidthLeft = (edgelProjectionsBegin[edgelPair.left] - edgelProjectionsEnd[edgelPair.left]).abs();
    const double projectedWidthRight = (edgelProjectionsBegin[edgelPair.right] - edgelProjectionsEnd[edgelPair.right]).abs();

	  //TODO: should this be double?
    Edgel edgel;
    edgel.point = Vector2d(edgelLeft + edgelRight)*0.5;
    edgel.direction = (edgelRight - edgelLeft).normalize(); // is it correct?

    //const ScanLineEdgelPercept::EdgelPair& el = getScanLineEdgelPercept().pairs[edgelPair.left];
    //const ScanLineEdgelPercept::EdgelPair& er = getScanLineEdgelPercept().pairs[edgelPair.right];

    if(projectedWidthLeft > parameters.maximalProjectedLineWidth && projectedWidthRight > parameters.maximalProjectedLineWidth) {
      getLineGraphPercept().edgels.push_back(edgel);
        
      DEBUG_REQUEST("Vision:LineGraphProvider:draw_line_graph",
        FIELD_DRAWING_CONTEXT;
        PEN("FF0000",2);
        CIRCLE( edgel.point.x, edgel.point.y, 25);
      );
    }
  }

  

  // neighbors line graph in image
  for(size_t i = 0; i < edgelNeighbors.size(); i++)
  {
    bool add_to_percept = false;

    if(edgelNeighbors[i].left != -1) {
      add_to_percept = true;
    }

    if(edgelNeighbors[i].right != -1) {
      add_to_percept = true;
    }

    if(add_to_percept) {<commit_msg>visualize the projected line width<commit_after>/* 
 * File:   LineGraphProvider.cpp
 * Author: <NAME>
 */

#include "LineGraphProvider.h"

#include "Tools/CameraGeometry.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Math/Geometry.h"

LineGraphProvider::LineGraphProvider()
:
  cameraID(CameraInfo::Top)
{
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:edgel_pairs", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_neighbors", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_neighbors_field", "mark the edgels on the image", false);

  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:edgel_cluster", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_lines", "draw the esimated lines in the image", false);

  //DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_compas", "draw compas direcion based on the edgel directions", false);

  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_line_graph", "", false);

  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_extended_line_graph", "", false);
  DEBUG_REQUEST_REGISTER("Vision:LineGraphProvider:draw_extended_line_graph_top", "", false);

  getDebugParameterList().add(&parameters);
}


LineGraphProvider::~LineGraphProvider()
{
  getDebugParameterList().remove(&parameters);
}


void LineGraphProvider::execute(CameraInfo::CameraID id)
{
  cameraID = id;

  calculatePairsAndNeigbors(getScanLineEdgelPercept().pairs, edgelPairs, edgelNeighbors, parameters.edgelSimThreshold);


  // calculate the projection for all edgels
  edgelProjectionsBegin.resize(getScanLineEdgelPercept().pairs.size());
  edgelProjectionsEnd.resize(getScanLineEdgelPercept().pairs.size());
  for(size_t i = 0; i < getScanLineEdgelPercept().pairs.size(); i++) 
  {  
    const ScanLineEdgelPercept::EdgelPair& pair = getScanLineEdgelPercept().pairs[i];

    const Edgel& end = getScanLineEdgelPercept().edgels[pair.end];
    const Edgel& begin = getScanLineEdgelPercept().edgels[pair.begin];
    
    // NOTE: edgels are assumed to be always below horizon and so the projection should be allways valid
    CameraGeometry::imagePixelToFieldCoord(
      getCameraMatrix(), getCameraInfo(),
      end.point.x,
      end.point.y,
      0.0,
      edgelProjectionsEnd[i]);

    CameraGeometry::imagePixelToFieldCoord(
      getCameraMatrix(), getCameraInfo(),
      begin.point.x,
      begin.point.y,
      0.0,
      edgelProjectionsBegin[i]);

    //pair.projectedWidth = Vector2d(beginPointOnField - endPointOnField).abs();
  }

  // extend line graph
  lineGraphsIds.clear();
  extendLineGraph(edgelNeighbors);

  std::vector<std::vector<EdgelD>>& graph = (cameraID == CameraInfo::Top)?
        getLineGraphPercept().lineGraphsTop:
        getLineGraphPercept().lineGraphs;

  // add the graphs to the representation
  for(const std::vector<int>& subgraph : lineGraphsIds)
  {
    graph.emplace_back();
    for(size_t left=0, right=1; right < subgraph.size(); ++left, ++right) {

      const Vector2d& edgelLeft = (edgelProjectionsBegin[subgraph[left]] + edgelProjectionsEnd[subgraph[left]])*0.5;
      const Vector2d& edgelRight = (edgelProjectionsBegin[subgraph[right]] + edgelProjectionsEnd[subgraph[right]])*0.5;

      EdgelD edgel;
      edgel.point = Vector2d(edgelLeft + edgelRight)*0.5;
      edgel.direction = (edgelRight - edgelLeft).normalize();

      graph.back().push_back(edgel);
    }
  }

  // calculate the LineGraphPercept
  for(size_t j = 0; j < edgelPairs.size(); j++)
  {
    const EdgelPair& edgelPair = edgelPairs[j];
    const Vector2d& edgelLeft = (edgelProjectionsBegin[edgelPair.left] + edgelProjectionsEnd[edgelPair.left])*0.5;
    const Vector2d& edgelRight = (edgelProjectionsBegin[edgelPair.right] + edgelProjectionsEnd[edgelPair.right])*0.5;

    const double projectedWidthLeft = (edgelProjectionsBegin[edgelPair.left] - edgelProjectionsEnd[edgelPair.left]).abs();
    const double projectedWidthRight = (edgelProjectionsBegin[edgelPair.right] - edgelProjectionsEnd[edgelPair.right]).abs();

	  //TODO: should this be double?
    Edgel edgel;
    edgel.point = Vector2d(edgelLeft + edgelRight)*0.5;
    edgel.direction = (edgelRight - edgelLeft).normalize(); // is it correct?

    //const ScanLineEdgelPercept::EdgelPair& el = getScanLineEdgelPercept().pairs[edgelPair.left];
    //const ScanLineEdgelPercept::EdgelPair& er = getScanLineEdgelPercept().pairs[edgelPair.right];

    if(projectedWidthLeft > parameters.maximalProjectedLineWidth && projectedWidthRight > parameters.maximalProjectedLineWidth) {
      getLineGraphPercept().edgels.push_back(edgel);
        
      DEBUG_REQUEST("Vision:LineGraphProvider:draw_line_graph",
        FIELD_DRAWING_CONTEXT;
        PEN("FF0000",2);
        CIRCLE( edgel.point.x, edgel.point.y, 25);

        PEN("0000FF",2);
        CIRCLE( edgelProjectionsBegin[edgelPair.left].x, edgelProjectionsBegin[edgelPair.left].y, 10);
        PEN("FF0000",2);
        CIRCLE( edgelProjectionsEnd[edgelPair.left].x, edgelProjectionsEnd[edgelPair.left].y, 10);
        PEN("000000",2);
        LINE( edgelProjectionsBegin[edgelPair.left].x, edgelProjectionsBegin[edgelPair.left].y, edgelProjectionsEnd[edgelPair.left].x, edgelProjectionsEnd[edgelPair.left].y);

        PEN("0000FF",2);
        CIRCLE( edgelProjectionsBegin[edgelPair.right].x, edgelProjectionsBegin[edgelPair.right].y, 10);
        PEN("FF0000",2);
        CIRCLE( edgelProjectionsEnd[edgelPair.right].x, edgelProjectionsEnd[edgelPair.right].y, 10);
        PEN("000000",2);
        LINE( edgelProjectionsBegin[edgelPair.right].x, edgelProjectionsBegin[edgelPair.right].y, edgelProjectionsEnd[edgelPair.right].x, edgelProjectionsEnd[edgelPair.right].y);
      );
    }
  }

  

  // neighbors line graph in image
  for(size_t i = 0; i < edgelNeighbors.size(); i++)
  {
    bool add_to_percept = false;

    if(edgelNeighbors[i].left != -1) {
      add_to_percept = true;
    }

    if(edgelNeighbors[i].right != -1) {
      add_to_percept = true;
    }

    if(add_to_percept) {
{'fraction_non_alphanumeric': 0.12598564720474884, 'fraction_numerical': 0.010277310179852929, 'mean_word_length': 4.773913043478261, 'pattern_counts': {'":': 0, '<': 19, '<?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': '5835477', 'n_tokens_mistral': 3873, 'n_tokens_neox': 3717, 'n_words': 604}