<commit_before>/*
 * @file VirtualVisionProcessor.cpp
 *
 * @author <a href="mailto:<EMAIL>"><NAME></a>
 * @breief calculate perceptions from virtual vision
 *
 */

#include "VirtualVisionProcessor.h"

#include <Tools/Math/Matrix_mxn.h>

#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/Stopwatch.h"
#include "Tools/DataConversion.h"

using namespace std;

VirtualVisionProcessor::VirtualVisionProcessor()
{
  goalPostNames.insert("G1L");
  goalPostNames.insert("G2L");
  goalPostNames.insert("G1R");
  goalPostNames.insert("G2R");

  goalPostTypes["G1L"] = GoalPercept::GoalPost::rightPost;
  goalPostTypes["G2L"] = GoalPercept::GoalPost::leftPost;
  goalPostTypes["G1R"] = GoalPercept::GoalPost::leftPost;
  goalPostTypes["G2R"] = GoalPercept::GoalPost::rightPost;

  // NOTE: this is the configuration of SPL, blue team defence blue goal
  // it is not match the visualization of SimSpark
  goalPostColors["G1L"] = ColorClasses::skyblue;
  goalPostColors["G2L"] = ColorClasses::skyblue;
  goalPostColors["G1R"] = ColorClasses::yellow;
  goalPostColors["G2R"] = ColorClasses::yellow;

  lineNames.push_back(LineName("F1L","F1R"));
  lineNames.push_back(LineName("F2L","F2R"));
  lineNames.push_back(LineName("F1L","F2L"));
  lineNames.push_back(LineName("F1R","F2R"));

  cornerNames.insert("F1L");
  cornerNames.insert("F1R");
  cornerNames.insert("F2L");
  cornerNames.insert("F2R");

  const FieldInfo& theFieldInfo = getFieldInfo();
  flagPosOnField[GameData::blue]["F1L"] = theFieldInfo.crossings[FieldInfo::ownCornerLeft].position;
  flagPosOnField[GameData::blue]["F2L"] = theFieldInfo.crossings[FieldInfo::ownCornerRight].position;
  flagPosOnField[GameData::blue]["F1R"] = theFieldInfo.crossings[FieldInfo::opponentCornerLeft].position;
  flagPosOnField[GameData::blue]["F2R"] = theFieldInfo.crossings[FieldInfo::opponentCornerRight].position;

  flagPosOnField[GameData::red]["F1L"] = theFieldInfo.crossings[FieldInfo::opponentCornerRight].position;
  flagPosOnField[GameData::red]["F2L"] = theFieldInfo.crossings[FieldInfo::opponentCornerLeft].position;
  flagPosOnField[GameData::red]["F1R"] = theFieldInfo.crossings[FieldInfo::ownCornerRight].position;
  flagPosOnField[GameData::red]["F2R"] = theFieldInfo.crossings[FieldInfo::ownCornerLeft].position;

  DEBUG_REQUEST_REGISTER("VirtualVisionProcessor:corner_flags", "draw the corner flags", false);

  DEBUG_REQUEST_REGISTER("VirtualVisionProcessor:LineDetector:mark_circle", "", false);
}

VirtualVisionProcessor::~VirtualVisionProcessor()
{
  
}

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

  updateBall();
  updateGoal();
  updateLine();
  updateCorners();
  updatePlayers();

  findIntersections();
  LinePercept& theLinePercept = getLinePercept();
  classifyIntersections(theLinePercept.lines);
}//end execute

Vector3d VirtualVisionProcessor::calculatePosition(const Vector3d& pol)
{
  return getCameraMatrix() * pol2xyz(pol);
}//end calculatePosition

void VirtualVisionProcessor::updateBall()
{
  const VirtualVision& theVirtualVision = getVirtualVision();
  BallPercept& theBallPercept = getBallPercept();
  const FrameInfo& theFrameInfo = getFrameInfo();
  const FieldInfo& theFieldInfo = getFieldInfo();
  const CameraInfo& theCameraInfo = getCameraInfo();
  const CameraMatrix& theCameraMatrix = getCameraMatrix();
  VirtualVision::PointMap::const_iterator iter = theVirtualVision.data.find("B");
  if ( iter != theVirtualVision.data.end() )
  {
    theBallPercept.ballWasSeen = true;
    theBallPercept.frameInfoWhenBallWasSeen = theFrameInfo;
    Vector3d p = calculatePosition(iter->second);
    theBallPercept.bearingBasedOffsetOnField.x = p.x;
    theBallPercept.bearingBasedOffsetOnField.y = p.y;

    theBallPercept.radiusInImage = theFieldInfo.ballRadius / iter->second.x * theCameraInfo.getFocalLength();
    theBallPercept.centerInImage = CameraGeometry::relativePointToImage(theCameraMatrix, theCameraInfo, p);
  }
  else
  {
    theBallPercept.ballWasSeen = false;
  }
}//end updateBall

void VirtualVisionProcessor::updateGoal()
{
  GoalPercept& theGoalPercept = getGoalPercept();
  const VirtualVision& theVirtualVision = getVirtualVision();
  const CameraInfo& theCameraInfo = getCameraInfo();
  const CameraMatrix& theCameraMatrix = getCameraMatrix();
  theGoalPercept.reset();

  GoalPercept::GoalPost yellowPostOne;
  GoalPercept::GoalPost yellowPostTwo;

  GoalPercept::GoalPost bluePostOne;
  GoalPercept::GoalPost bluePostTwo;

  for(set<string>::const_iterator i=goalPostNames.begin(); i!=goalPostNames.end(); ++i)
  {
    map<string, Vector3d>::const_iterator j = theVirtualVision.data.find(*i);
    if ( j != theVirtualVision.data.end() )
    {
      Vector3d p = calculatePosition(j->second);
      Vector2d position(p.x, p.y);
<commit_msg>bugfix: this information is needed by the MCSL<commit_after>/*
 * @file VirtualVisionProcessor.cpp
 *
 * @author <a href="mailto:<EMAIL>"><NAME></a>
 * @breief calculate perceptions from virtual vision
 *
 */

#include "VirtualVisionProcessor.h"

#include <Tools/Math/Matrix_mxn.h>

#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/Stopwatch.h"
#include "Tools/DataConversion.h"

using namespace std;

VirtualVisionProcessor::VirtualVisionProcessor()
{
  goalPostNames.insert("G1L");
  goalPostNames.insert("G2L");
  goalPostNames.insert("G1R");
  goalPostNames.insert("G2R");

  goalPostTypes["G1L"] = GoalPercept::GoalPost::rightPost;
  goalPostTypes["G2L"] = GoalPercept::GoalPost::leftPost;
  goalPostTypes["G1R"] = GoalPercept::GoalPost::leftPost;
  goalPostTypes["G2R"] = GoalPercept::GoalPost::rightPost;

  // NOTE: this is the configuration of SPL, blue team defence blue goal
  // it is not match the visualization of SimSpark
  goalPostColors["G1L"] = ColorClasses::skyblue;
  goalPostColors["G2L"] = ColorClasses::skyblue;
  goalPostColors["G1R"] = ColorClasses::yellow;
  goalPostColors["G2R"] = ColorClasses::yellow;

  lineNames.push_back(LineName("F1L","F1R"));
  lineNames.push_back(LineName("F2L","F2R"));
  lineNames.push_back(LineName("F1L","F2L"));
  lineNames.push_back(LineName("F1R","F2R"));

  cornerNames.insert("F1L");
  cornerNames.insert("F1R");
  cornerNames.insert("F2L");
  cornerNames.insert("F2R");

  const FieldInfo& theFieldInfo = getFieldInfo();
  flagPosOnField[GameData::blue]["F1L"] = theFieldInfo.crossings[FieldInfo::ownCornerLeft].position;
  flagPosOnField[GameData::blue]["F2L"] = theFieldInfo.crossings[FieldInfo::ownCornerRight].position;
  flagPosOnField[GameData::blue]["F1R"] = theFieldInfo.crossings[FieldInfo::opponentCornerLeft].position;
  flagPosOnField[GameData::blue]["F2R"] = theFieldInfo.crossings[FieldInfo::opponentCornerRight].position;

  flagPosOnField[GameData::red]["F1L"] = theFieldInfo.crossings[FieldInfo::opponentCornerRight].position;
  flagPosOnField[GameData::red]["F2L"] = theFieldInfo.crossings[FieldInfo::opponentCornerLeft].position;
  flagPosOnField[GameData::red]["F1R"] = theFieldInfo.crossings[FieldInfo::ownCornerRight].position;
  flagPosOnField[GameData::red]["F2R"] = theFieldInfo.crossings[FieldInfo::ownCornerLeft].position;

  DEBUG_REQUEST_REGISTER("VirtualVisionProcessor:corner_flags", "draw the corner flags", false);

  DEBUG_REQUEST_REGISTER("VirtualVisionProcessor:LineDetector:mark_circle", "", false);
}

VirtualVisionProcessor::~VirtualVisionProcessor()
{
  
}

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

  updateBall();
  updateGoal();
  updateLine();
  updateCorners();
  updatePlayers();

  findIntersections();
  LinePercept& theLinePercept = getLinePercept();
  classifyIntersections(theLinePercept.lines);
}//end execute

Vector3d VirtualVisionProcessor::calculatePosition(const Vector3d& pol)
{
  return getCameraMatrix() * pol2xyz(pol);
}//end calculatePosition

void VirtualVisionProcessor::updateBall()
{
  const VirtualVision& theVirtualVision = getVirtualVision();
  BallPercept& theBallPercept = getBallPercept();
  const FrameInfo& theFrameInfo = getFrameInfo();
  const FieldInfo& theFieldInfo = getFieldInfo();
  const CameraInfo& theCameraInfo = getCameraInfo();
  const CameraMatrix& theCameraMatrix = getCameraMatrix();
  VirtualVision::PointMap::const_iterator iter = theVirtualVision.data.find("B");
  if ( iter != theVirtualVision.data.end() )
  {
    theBallPercept.ballWasSeen = true;
    theBallPercept.frameInfoWhenBallWasSeen = theFrameInfo;
    Vector3d p = calculatePosition(iter->second);
    theBallPercept.bearingBasedOffsetOnField.x = p.x;
    theBallPercept.bearingBasedOffsetOnField.y = p.y;

    theBallPercept.radiusInImage = theFieldInfo.ballRadius / iter->second.x * theCameraInfo.getFocalLength();
    theBallPercept.centerInImage = CameraGeometry::relativePointToImage(theCameraMatrix, theCameraInfo, p);
  }
  else
  {
    theBallPercept.ballWasSeen = false;
  }
}//end updateBall

void VirtualVisionProcessor::updateGoal()
{
  GoalPercept& theGoalPercept = getGoalPercept();
  const VirtualVision& theVirtualVision = getVirtualVision();
  const CameraInfo& theCameraInfo = getCameraInfo();
  const CameraMatrix& theCameraMatrix = getCameraMatrix();
  theGoalPercept.reset();
  theGoalPercept.horizonScan = true;

  GoalPercept::GoalPost yellowPostOne;
  GoalPercept::GoalPost yellowPostTwo;

  GoalPercept::GoalPost bluePostOne;
  GoalPercept::GoalPost bluePostTwo;

  for(set<string>::const_iterator i=goalPostNames.begin(); i!=goalPostNames.end(); ++i)
  {
    map<string, Vector3d>::const_iterator j = theVirtualVision.data.find(*i);
    if ( j != theVirtualVision.data.end() )
    {
      Vector3d p = calculatePosition(j->second);
      Vector2d position(p.x, p.y);

{'fraction_non_alphanumeric': 0.1348779478692594, 'fraction_numerical': 0.008067852709971039, 'mean_word_length': 6.848214285714286, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 23, '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': '3878526', 'n_tokens_mistral': 3278, 'n_tokens_neox': 3209, 'n_words': 465}