C++ GridMap::getSize方法代码示例

void PotentialField::init() {
  ROS_INFO("Created map");
  map_.setFrameId("/potential_field_link");
  map_.setGeometry(Length(map_x_size_, map_y_size_), map_resolution_);
  for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
    Position position;
    map_.getPosition(*it, position);
    map_.at("obstacle_field", *it) = 0.0;
    map_.at("target_waypoint_field", *it) = 0.0;
    map_.at("vscan_points_field", *it) = 0.0;
    map_.at("potential_field", *it) = 0.0;
  }
  ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
           map_.getLength().x(), map_.getLength().y(), map_.getSize()(0),
           map_.getSize()(1));
}

TEST(GridMapCvProcessing, changeResolution)
{
  // Create grid map.
  GridMap mapIn;
  mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
  mapIn.add("layer", 1.0);
  for (grid_map::CircleIterator iterator(mapIn, mapIn.getPosition(), 0.2); !iterator.isPastEnd(); ++iterator) {
    mapIn.at("layer", *iterator) = 2.0;
  }

  // Change resolution.
  GridMap mapOut;
  EXPECT_TRUE(GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1));

  // Check data.
  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
  EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition());
  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all());
  EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner.
  EXPECT_EQ(mapIn.atPosition("layer", mapIn.getPosition()), mapOut.atPosition("layer", mapOut.getPosition())); // Center.
}

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);
  }
}

 void populateMap(GridMap &map, string layer,string file_path, float scale,float res)
 {
   cv_bridge::CvImage cv_image;
   Mat img = imread(file_path,CV_LOAD_IMAGE_GRAYSCALE);
   cv::Size img_size= img.size();
   cv::resize(img,img,cv::Size(),scale,scale, CV_INTER_CUBIC);
   map.setGeometry(Length(img.rows*res,img.cols*res),res);
   for (GridMapIterator it(map); !it.isPastEnd(); ++it) 
   {
     Position position;
     map.getPosition(*it, position);
     int x = (img.rows/2) + position.x()/res+res/2.0; 
     int y = (img.cols/2) + position.y()/res+res/2.0;
     map.at(layer, *it) = img.at<uchar>(x,y)<200?1:0;    
   }
   map.setTimestamp(ros::Time::now().toNSec());
   ROS_INFO("Created map with size %f x %f m (%i x %i cells).",map.getLength().x(), map.getLength().y(),map.getSize()(0), map.getSize()(1));
 }