MapDataLayer.cpp

Go to the documentation of this file.

#include "lanedetection-groundtruth/MapDataLayer.hpp"
 
#include "lanedetection-groundtruth/MapUtils.hpp"
 
#include <cassert>
 
#include <utils/logger.hpp>
 
#include <cmath>
 
 
using namespace galaxis::utils;
 
using namespace galaxis::map::data;
 
MapDataLayer::MapDataLayer(int amountOfThreads = 4) :
    m_threadPool(amountOfThreads), m_amountOfThreads(amountOfThreads) {
  assert(m_amountOfThreads > 0);
  init();
}
 
MapDataLayer::MapDataLayer() :
    m_threadPool(4) {
  m_amountOfThreads = 4;
  init();
}
 
MapDataLayer::~MapDataLayer() {
  m_threadPool.join();
}
 
void MapDataLayer::init() {
  log("MapDataLayer:\n - Threads: ", m_amountOfThreads, "\n - Map Size: ", MAP_SIZE, "x", MAP_SIZE);
 
  m_mapSize                   = Vector2f(MAP_SIZE, MAP_SIZE);
  m_absoluteCarPositionOffset = m_mapSize * 0.5;
 
  m_linesObjectSPtr = std::make_shared<LinesObject<int16_t>>();
 
  clear();
}
 
void MapDataLayer::clear() {
  for(ColumnMapArray& columnArray: m_data) {
    columnArray.fill(MapCell());
  }
}
 
void MapDataLayer::updateMap(visualization_msgs::msg::Marker::SharedPtr laneDetectionSPtr, char laneType) {
  // single-threaded
  m_mapMutex.lock();
 
  bool usable = false;
  geometry_msgs::msg::Point* lastPoint;
  
  if (laneType == 'l') {
    usable = m_lastPointLeftLaneUsable;
    lastPoint = &m_lastPointLeftLane;
  }
  else if (laneType == 'c') {
    usable = m_lastPointCenterLaneUsable;
    lastPoint = &m_lastPointCenterLane;
  }
  else if (laneType == 'r') {
    usable = m_lastPointRightLaneUsable;
    lastPoint = &m_lastPointRightLane;
  }
 
  for(geometry_msgs::msg::Point& point : laneDetectionSPtr->points) {
    if (usable == false) {
      lastPoint = &point;
      usable = true;
      continue;
    }
 
    // approximate between points in 1cm steps
    double distanceCM = sqrt((point.x - lastPoint->x)*(point.x - lastPoint->x) + (point.y - lastPoint->y)*(point.y - lastPoint->y)) * 100;
    if (distanceCM <= 1) {
      distanceCM = 1;
      // log_warning("Fix distanceCM.");
    }
    
    double distanceX = lastPoint->x - point.x; // in meter
    double distanceY = lastPoint->y - point.y; // in meter
    for (double p = 0; p < 1; p += 1/distanceCM) {
      double pointX = point.x + p * distanceX;
      double pointY = point.y + p * distanceY;
      pointX *= 100; // to CM
      pointY *= 100; // to CM
 
      Vector2i absoluteCellPositionOnGrid = Vector2i(std::round(pointX), std::round(pointY));
      absoluteCellPositionOnGrid[0] += static_cast<int>(m_absoluteCarPositionOffset[0]);
      absoluteCellPositionOnGrid[1] += static_cast<int>(m_absoluteCarPositionOffset[1]);
 
      updateCell(absoluteCellPositionOnGrid, laneType);
    }
 
    lastPoint = &point;
  }
 
  if (laneType == 'l') {
    m_lastPointLeftLane = *lastPoint;
    m_lastPointLeftLaneUsable = true;
  }
  else if (laneType == 'c') {
    m_lastPointCenterLane = *lastPoint;
    m_lastPointCenterLaneUsable = true;
  }
  else if (laneType == 'r') {
    m_lastPointRightLane = *lastPoint;
    m_lastPointRightLaneUsable = true;
  }
  m_mapMutex.unlock();
}
 
void MapDataLayer::updateCell(Vector2i position, char laneType) {
  if(position[0] < 0 || position[0] >= m_mapSize[0] || position[1] < 0 || position[1] >= m_mapSize[1]) {
    log_warning("MapDataLayer::updateCell: cannot update Cell (out of bounds) at Position (", position[0], ",", position[1], ")");
    return;
  }
 
  MapCell* cellPtr = &m_data[position[0]][position[1]];
  if (laneType == 'l')
    cellPtr->leftLine = true;
  else if (laneType == 'c')
    cellPtr->centerLine = true;
  else if (laneType == 'r')
    cellPtr->rightLine = true;
  else 
    log_warning("Failure to updateMap. Wrong lanetype parameter value (l,c,r)");
}
 
std::shared_ptr<LinesObject<int16_t>> MapDataLayer::getLineObject(Vector2f absolutePositionCM, float absoluteOrientation, Vector2i windowSize, Vector2i& effectiveWindowSize) {
  m_mapMutex.lock();
 
  // Eigen rotates COUNTER-clock wise!
  Eigen::Rotation2Df rotationMatrix(-absoluteOrientation);
  Eigen::Rotation2Df reverseRotationMatrix(absoluteOrientation);
 
  m_linesObjectSPtr->clear();
  m_linesObjectSPtr->rotationMatrix        = rotationMatrix;
  m_linesObjectSPtr->reverseRotationMatrix = reverseRotationMatrix;
 
  absolutePositionCM += m_absoluteCarPositionOffset;
  Vector2i absolutePositionInteger = Vector2i(std::round(absolutePositionCM[0]), std::round(absolutePositionCM[1]));
 
  // put larger rectangle above window for map iteration
  Vector2f marginX = Vector2f(windowSize[0] / 2, 0);
  Vector2f marginY = Vector2f(0, windowSize[1]);
  marginX          = rotationMatrix * marginX;
  marginY          = rotationMatrix * marginY;
 
  std::array<Vector2f, 4> windowEdges;
  windowEdges[0] = absolutePositionCM - marginX;
  windowEdges[1] = absolutePositionCM + marginX;
  windowEdges[2] = windowEdges[0] + marginY;
  windowEdges[3] = windowEdges[1] + marginY;
 
  Vector2i largerWindowBottomLeftCorner = Vector2i(std::round(map::utils::getMin(windowEdges, 0)), std::round(map::utils::getMin(windowEdges, 1)));
  Vector2i largerWindowTopRightCorner   = Vector2i(std::round(map::utils::getMax(windowEdges, 0)), std::round(map::utils::getMax(windowEdges, 1)));
 
  // check if lookup (axis aligned) window (larger window) inside of map
  if(largerWindowBottomLeftCorner[0] < 0) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (X smaller than 0)");
    largerWindowBottomLeftCorner[1] = 0;
  }
  if(largerWindowTopRightCorner[0] >= m_mapSize[0]) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (X larger than map)");
    largerWindowTopRightCorner[0] = m_mapSize[0] - 1;
  }
  if(largerWindowBottomLeftCorner[1] < 0) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (Y smaller than 0)");
    largerWindowBottomLeftCorner[1] = 0;
  }
  if(largerWindowTopRightCorner[1] >= m_mapSize[1]) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (Y larger than map)");
    largerWindowTopRightCorner[1] = m_mapSize[1] - 1;
  }
 
  // int largerWindowStepsPerThread = (largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0]) / m_amountOfThreads;
 
  // assert(largerWindowStepsPerThread > 0);
 
  // int arrayStartX = largerWindowBottomLeftCorner[0];
  // int arrayEndX   = arrayStartX + largerWindowStepsPerThread;
  // int arrayStartY = largerWindowBottomLeftCorner[1];
  // int arrayEndY   = largerWindowTopRightCorner[1];
 
  // for(int i = 0; i < m_amountOfThreads; ++i) {
  //   if(i == m_amountOfThreads - 1) {
  //     arrayEndX = largerWindowTopRightCorner[0] + 1;
  //   } else {
  //     arrayEndX = arrayStartX + largerWindowStepsPerThread;
  //   }
 
  //   boost::asio::post(m_threadPool, std::bind(&MapDataLayer::iterateThroughMap, this, arrayStartX, arrayEndX, arrayStartY, arrayEndY, absolutePositionInteger));
  //   arrayStartX = arrayEndX;
  // }
 
  iterateThroughMap(largerWindowBottomLeftCorner[0], largerWindowTopRightCorner[0] + 1, largerWindowBottomLeftCorner[1], largerWindowTopRightCorner[1], absolutePositionInteger);
 
  // m_threadPool.wait();
  m_mapMutex.unlock();
 
  effectiveWindowSize[0] = largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0];
  assert(effectiveWindowSize[0] >= 0);
  effectiveWindowSize[1] = largerWindowTopRightCorner[1] - largerWindowBottomLeftCorner[1];
  assert(effectiveWindowSize[1] >= 0);
 
  return m_linesObjectSPtr;
}
 
void MapDataLayer::iterateThroughMap(int arrayStartX, int arrayEndX, int arrayStartY, int arrayEndY, Vector2i const& absoluteCarPosition) {
  MapCell const* cellPtr = nullptr;
  for(int x = arrayStartX; x < arrayEndX; ++x) {
    for(int y = arrayStartY; y <= arrayEndY; ++y) {
      // append cell information to linesObject
      cellPtr = &m_data[x][y]; 
 
      if(cellPtr->leftLine) {
        m_linesObjectSPtr->leftLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->centerLine) {
        m_linesObjectSPtr->centerLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->rightLine) {
        m_linesObjectSPtr->rightLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
    }
  }
}