MapDataLayer.cpp

Go to the documentation of this file.

#include "Map/MapDataLayer.hpp"
 
#include "Map/MapUtils.hpp"
 
#include <cassert>
 
#include <utils/logger.hpp>
 
using namespace galaxis::utils;
using namespace galaxis::autopilot;
 
MapDataLayer::MapDataLayer(int amountOfThreads = 4) :
    m_threadPool(amountOfThreads), m_amountOfThreads(amountOfThreads), m_obstacleKeepDuration(1, 0) {
  assert(m_amountOfThreads > 0);
  init();
}
 
MapDataLayer::MapDataLayer() :
    m_threadPool(4), m_obstacleKeepDuration(1, 0) {
  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>>();
 
  m_staticObstacleVecSPtr = std::make_shared<std::vector<StaticObstacle>>();
 
  clear();
}
 
void MapDataLayer::clear() {
  for(ColumnMapArray& columnArray: m_data) {
    columnArray.fill(MapCell());
  }
}
 
// ############## Settings ##############
void MapDataLayer::enableSimulationMode() {
  m_simulationMode = true;
}
void MapDataLayer::setLookupWindowSizeCMWidth(int value) {
  m_lookupWindowSizeCMWidth = value;
}
void MapDataLayer::setLookupWindowSizeCMHeight(int value) {
 m_lookupWindowSizeCMHeight = value;
}
// void MapDataLayer::setLookupLookBehindCarCM(int value) {
//   m_lookupLookBehindCarCM = value;
// }
void MapDataLayer::setCellThresholdRGBValue(uint8_t value) {
  m_cellThresholdRGBValue = value;
}
void MapDataLayer::setCellThresholdExtract(uint8_t value) {
  m_cellThresholdExtract = value;
}
void MapDataLayer::setCellLineDecreasePerView(uint8_t value) {
  m_cellLineDecreasePerView = value;
}
void MapDataLayer::setCellLineIncreasePerView(uint8_t value) {
  m_cellLineIncreasePerView = value;
}
void MapDataLayer::setLaneImageConversionFactor(float factor) {
  m_laneImagePixelToPositionConversionFactor = factor;
}
void MapDataLayer::setObstaclePositionDiffForRerecognition(float diff) {
  m_obstaclePositionDiffForRerecognitionCM = diff;
}
void MapDataLayer::setObstacleTrackTimeDuration(int sec, int nsec) {
  m_obstacleKeepDuration = rclcpp::Duration(sec, nsec);
}
 
 
void MapDataLayer::updateMap(const sensor_msgs::msg::Image::ConstSharedPtr& laneDetectionSPtr, Vector2f currentAbsoluteCarPositionCM, double currentAbsoluteOrientation) {
  // TODO: check update rate of vision (MapContentsType); maybe check at runtime
  m_mapMutex.lock();
  // Increment Update Step
  m_updateStep++;
 
  int pixelLengthInBytes = laneDetectionSPtr->step / laneDetectionSPtr->width;
 
  int arrayStartIndex          = 0;
  int arrayWindowSizePerThread = (laneDetectionSPtr->height * laneDetectionSPtr->step) / m_amountOfThreads;
  arrayWindowSizePerThread -= arrayWindowSizePerThread % pixelLengthInBytes;
  int arrayEndIndex = arrayStartIndex + arrayWindowSizePerThread;
  // split message and run preprocessPoints function on multiple threads in pool
  for(int i = 0; i < m_amountOfThreads; ++i) {
    if(i == m_amountOfThreads - 1) {
      arrayEndIndex = (laneDetectionSPtr->height * laneDetectionSPtr->step) - 1;
    } else {
      arrayEndIndex = arrayStartIndex + arrayWindowSizePerThread;
    }
 
    m_sharedThreadMutex.lock_shared();
    boost::asio::post(m_threadPool, std::bind(&MapDataLayer::preprocessPoints, this, laneDetectionSPtr, currentAbsoluteCarPositionCM, arrayStartIndex, arrayEndIndex, currentAbsoluteOrientation));
    arrayStartIndex = arrayEndIndex;
  }
 
  // wait for threads to complete
  m_sharedThreadMutex.lock();
  m_sharedThreadMutex.unlock();
 
  m_mapMutex.unlock();
}
 
bool MapDataLayer::isInImageIgnoreField(Vector2f p_position, unsigned int p_maxImageX) {
  // check if in left ignore triangle
  // y = m*x + n
  float mLeftTriangle = static_cast<float>((imageIgnoreFieldArray[2] - imageIgnoreFieldArray[0])) / static_cast<float>(imageIgnoreFieldArray[1]);
  float nLeftTriangle = imageIgnoreFieldArray[0];
 
  if(p_position[1] > mLeftTriangle * p_position[0] + nLeftTriangle) {
    return true;
  }
 
  // check if in ignore box
  if(p_position[1] > imageIgnoreFieldArray[2] || p_position[1] > imageIgnoreFieldArray[4]) {
    return true;
  }
 
  // check if in right ignore triangle
  // y = m*x + n
  float mRightTriangle = static_cast<float>((imageIgnoreFieldArray[5] - imageIgnoreFieldArray[4])) / static_cast<float>((p_maxImageX - imageIgnoreFieldArray[3]));
  float nRightTriangle = mRightTriangle * imageIgnoreFieldArray[3] - imageIgnoreFieldArray[4];
 
  if(p_position[1] > mRightTriangle * p_position[0] + nRightTriangle) {
    return true;
  }
 
  return false;
}
 
void MapDataLayer::preprocessPoints(const sensor_msgs::msg::Image::ConstSharedPtr& laneDetectionSPtr, Vector2f const& currentPositionCM, int arrayOffsetStartIndex, int arrayOffsetEndIndex, double currentAbsoluteOrientation) {
  // TODO: position and orientation of car at recorded MapContents Message (history)
 
  Eigen::Rotation2Df rotationMatrix(-currentAbsoluteOrientation);
 
  // num of rows == height == Y, num of columns == width == X
 
  int pixelLengthInBytes = laneDetectionSPtr->step / laneDetectionSPtr->width;
  for(int index = arrayOffsetStartIndex; index < arrayOffsetEndIndex; index += pixelLengthInBytes) {
    int posXOriginal = (index % laneDetectionSPtr->step) / pixelLengthInBytes;
    int posYOriginal = static_cast<int>(index / laneDetectionSPtr->step);
    
    // image position conversion to map positions
    int posX = posXOriginal - laneDetectionSPtr->width / 2;
    int posY = laneDetectionSPtr->height - posYOriginal;
    Vector2f absoluteCellPosition = Vector2f(posX, posY) * m_laneImagePixelToPositionConversionFactor;
    absoluteCellPosition = rotationMatrix * absoluteCellPosition;
    absoluteCellPosition += currentPositionCM + m_absoluteCarPositionOffset;
 
    MapCell cellUpdate;
    cellUpdate.leftLine   = laneDetectionSPtr->data[index + galaxis::autopilot::imageLLinePixelByteIndex];
    cellUpdate.centerLine = laneDetectionSPtr->data[index + galaxis::autopilot::imageCLinePixelByteIndex];
    cellUpdate.rightLine  = laneDetectionSPtr->data[index + galaxis::autopilot::imageRLinePixelByteIndex];
 
    Vector2i absoluteCellPositionOnGrid = Vector2i(std::round(absoluteCellPosition[0]), std::round(absoluteCellPosition[1]));
 
 
    // skip Image Ignore Fields
    if (!m_simulationMode && isInImageIgnoreField(Vector2f(posXOriginal, posYOriginal), laneDetectionSPtr->width)) {
      m_data[absoluteCellPositionOnGrid[0]][absoluteCellPositionOnGrid[1]].imageIgnoreField = true;
    }
    else {
      updateCell(cellUpdate, absoluteCellPositionOnGrid);
    }
  }
 
  m_sharedThreadMutex.unlock_shared();
}
 
void MapDataLayer::updateCell(MapCell const& cellUpdate, Vector2i position) {
  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]];
 
  // decrease
  // decreaseStepCell(cellPtr);
 
  // increase
  if(cellUpdate.leftLine > m_cellThresholdRGBValue) {
    // int value = cellPtr->leftLine;
    // value += m_cellLineIncreasePerView;
    // cellPtr->leftLine = value > MAPCELL_MAXVALUE ? MAPCELL_MAXVALUE : value;
    cellPtr->leftLine = 255;
  }
  else {
    cellPtr->leftLine = 0;
  }
  if(cellUpdate.centerLine > m_cellThresholdRGBValue) {
    // int value = cellPtr->centerLine;
    // value += m_cellLineIncreasePerView;
    // cellPtr->centerLine = value > MAPCELL_MAXVALUE ? MAPCELL_MAXVALUE : value;
    cellPtr->centerLine = 255;
  }
  else {
    cellPtr->centerLine = 0;
  }
  if(cellUpdate.rightLine > m_cellThresholdRGBValue) {
    // int value = cellPtr->rightLine;
    // value += m_cellLineIncreasePerView;
    // cellPtr->rightLine = value > MAPCELL_MAXVALUE ? MAPCELL_MAXVALUE : value;
    cellPtr->rightLine = 255;
  }
  else {
    cellPtr->rightLine = 0;
  }
 
  cellPtr->imageIgnoreField = false;
 
  cellPtr->lastUpdateStep = m_updateStep;
}
 
inline void MapDataLayer::decreaseStepCell(MapCell* cellPtr) {
  // return; // TODO: remove
  uint32_t updateDiff = m_updateStep - cellPtr->lastUpdateStep;
  int decreaseValue   = updateDiff * m_cellLineDecreasePerView;
 
  // decrease
  if(decreaseValue < cellPtr->leftLine)
    cellPtr->leftLine = cellPtr->leftLine - decreaseValue;
  else 
    cellPtr->leftLine = 0;
 
  if(decreaseValue < cellPtr->centerLine)
    cellPtr->centerLine = cellPtr->centerLine - decreaseValue;
  else 
    cellPtr->centerLine = 0;
  
  if(decreaseValue < cellPtr->rightLine)
    cellPtr->rightLine = cellPtr->rightLine - decreaseValue;
  else 
    cellPtr->rightLine = 0;
      
  cellPtr->lastUpdateStep = m_updateStep;
}
 
std::shared_ptr<LinesObject<int16_t>> MapDataLayer::getLineObject(Vector2f absolutePositionCM, float lookBehindPosOffsetCM, double absoluteOrientation, Vector2i windowSize) {
  m_mapMutex.lock();
 
  Eigen::Rotation2Df rotationMatrix(-absoluteOrientation);
  Eigen::Rotation2Df reverseRotationMatrix(absoluteOrientation);
 
  Vector2f lookBehindVector = rotationMatrix * Vector2f(0.f, galaxis::autopilot::lookupLookBehindCarCM);
 
  m_linesObjectSPtr->clear();
  m_linesObjectSPtr->rotationMatrix        = rotationMatrix;
  m_linesObjectSPtr->reverseRotationMatrix = reverseRotationMatrix;
  m_linesObjectSPtr->rotateOrientation = -absoluteOrientation;
 
  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 - lookBehindVector - marginX; // left lower 
  windowEdges[1] = absolutePositionCM - lookBehindVector + marginX; // right lower
  windowEdges[2] = windowEdges[0] + marginY; // left upper
  windowEdges[3] = windowEdges[1] + marginY; // right upper
 
  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;
 
  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;
    }
    
    m_sharedThreadMutex.lock_shared();
    boost::asio::post(m_threadPool, std::bind(&MapDataLayer::iterateThroughMap, this, arrayStartX, arrayEndX, arrayStartY, arrayEndY, absolutePositionInteger, windowEdges));
    arrayStartX = arrayEndX;
  }
 
  // wait for threads to complete
  m_sharedThreadMutex.lock();
  m_sharedThreadMutex.unlock();
 
  m_linesObjectSPtr->windowSize = Vector2i(largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0], largerWindowTopRightCorner[1] - largerWindowBottomLeftCorner[1]);
 
  m_mapMutex.unlock();
  return m_linesObjectSPtr;
}
 
float MapDataLayer::checkPositionRelativeToEdge(int x1, int y1, int x2, int y2, int xp, int yp) {
  // Edge: (x1,y1) and (x2,y2) [counterclockwise]
  // https://stackoverflow.com/questions/2752725/finding-whether-a-point-lies-inside-a-rectangle-or-not
  return (x2 - x1) * (yp - y1) - (xp - x1) * (y2 - y1);
}
 
void MapDataLayer::iterateThroughMap(int arrayStartX, int arrayEndX, int arrayStartY, int arrayEndY, Vector2i const& absoluteCarPosition, std::array<Vector2f, 4> const& windowEdges) {
  MapCell* cellPtr = nullptr;
  for(int x = arrayStartX; x < arrayEndX; ++x) {
    for(int y = arrayStartY; y <= arrayEndY; ++y) {
      // check if point lies in smaller/real window NOT only in larger Window
      if ( checkPositionRelativeToEdge(windowEdges[0][0], windowEdges[0][1], windowEdges[2][0], windowEdges[2][1], x, y) > 0 // left edge
        || checkPositionRelativeToEdge(windowEdges[2][0], windowEdges[2][1], windowEdges[3][0], windowEdges[3][1], x, y) > 0 // upper edge
        || checkPositionRelativeToEdge(windowEdges[3][0], windowEdges[3][1], windowEdges[1][0], windowEdges[1][1], x, y) > 0 // right edge
        || checkPositionRelativeToEdge(windowEdges[1][0], windowEdges[1][1], windowEdges[0][0], windowEdges[0][1], x, y) > 0 // lower edge
         ){
          continue;
      }
 
      // append cell information to linesObject
      cellPtr = &m_data[x][y];
 
      // clean 
      // decreaseStepCell(cellPtr);
      if(m_updateStep - cellPtr->lastUpdateStep > 20) { //&& (!cellPtr->imageIgnoreField || m_updateStep - cellPtr->lastUpdateStep > 5)) {
        cellPtr->leftLine = 0;
        cellPtr->centerLine = 0;
        cellPtr->rightLine = 0;
        continue;
      }
 
      if(cellPtr->leftLine > m_cellThresholdExtract) {
        m_linesObjectSPtr->leftLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->centerLine > m_cellThresholdExtract) {
        m_linesObjectSPtr->centerLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->rightLine > m_cellThresholdExtract) {
        m_linesObjectSPtr->rightLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
    }
  }
 
  m_sharedThreadMutex.unlock_shared();
}
 
// obstacle functions
std::shared_ptr<std::vector<StaticObstacle>> MapDataLayer::getObstacles(rclcpp::Time const& timestamp, bool removeOld) {
  if(removeOld) {
    // rm old obstacles
    for(std::vector<StaticObstacle>::iterator it = m_staticObstacleVecSPtr->begin(); it != m_staticObstacleVecSPtr->end(); /* noop */) {
      if (timestamp - it->get_last_update() > m_obstacleKeepDuration) {
        it = m_staticObstacleVecSPtr->erase(it);
      }
      else {
        ++it;
      }
    }
  }
 
  return m_staticObstacleVecSPtr;
}
 
void MapDataLayer::addOrUpdateStaticObstacle(ObstacleType const& p_obstacle, Vector2f absolutePositionCM, double currentOrientation, rclcpp::Time const& timestamp) {
  Vector2f obsPosition = Vector2f(p_obstacle.pos_x * 100, p_obstacle.pos_y * 100);
  Eigen::Rotation2Df rotationMat(-currentOrientation);
  obsPosition = rotationMat * obsPosition;
  obsPosition += absolutePositionCM;
 
  bool obstacleUpdated = false;
  for(StaticObstacle& obstacleStatic : *m_staticObstacleVecSPtr) {
    Vector2f distanceDiff = obsPosition - obstacleStatic.get_position_now();
    // TODO: also check dimensions
    if(distanceDiff.norm() < m_obstaclePositionDiffForRerecognitionCM) { // obstacle already inserted -> update
      obstacleStatic.update_position(timestamp, obsPosition);
      obstacleUpdated = true;
      break;
    }
  }
 
  if(obstacleUpdated == false) { // add new static obstacle
    StaticObstacle newObstacle(timestamp, obsPosition, p_obstacle.width * 100);
    m_staticObstacleVecSPtr->push_back(newObstacle);
  }
}