Esempio n. 1
0
void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer,
                                                       const double heightClearance)
{
  data_.clear();
  resolution_ = gridMap.getResolution();
  position_ = gridMap.getPosition();
  size_ = gridMap.getSize();
  Matrix map = gridMap.get(layer); // Copy!

  float minHeight = map.minCoeffOfFinites();
  if (!std::isfinite(minHeight)) minHeight = lowestHeight_;
  float maxHeight = map.maxCoeffOfFinites();
  if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_;

  const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option).
  for (size_t i = 0; i < map.size(); ++i) {
    if (std::isnan(map(i))) map(i) = valueForEmptyCells;
  }

  // Height range of the signed distance field is higher than the max height.
  maxHeight += heightClearance;

  Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_;
  Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols());
  std::vector<Matrix> sdf;
  zIndexStartHeight_ = minHeight;

  // Calculate signed distance field from bottom.
  for (float h = minHeight; h < maxHeight; h += resolution_) {
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h;
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1;
    Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField);
    Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField);
    Matrix sdf2d;
    // If 2d sdfObstacleFree calculation failed, neglect this SDF
    // to avoid extreme small distances (-INF).
    if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle;
    else sdf2d = sdfObstacle - sdfObstacleFree;
    sdf2d *= resolution_;
    for (size_t i = 0; i < sdfElevationAbove.size(); ++i) {
      if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i);
      else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_;
      if (sdf2d(i) == 0) sdfLayer(i) = h - map(i);
      else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h));
      else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));
    }
    data_.push_back(sdfLayer);
  }
}
Esempio n. 2
0
EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
    : center_(center)
{
  semiAxisSquare_ = (0.5 * length).square();
  double sinRotation = sin(rotation);
  double cosRotation = cos(rotation);
  transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
  mapLength_ = gridMap.getLength();
  mapPosition_ = gridMap.getPosition();
  resolution_ = gridMap.getResolution();
  bufferSize_ = gridMap.getSize();
  bufferStartIndex_ = gridMap.getStartIndex();
  Index submapStartIndex;
  Index submapBufferSize;
  findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
  if(!isInside()) ++(*this);
}