Esempio n. 1
0
 static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
                     const int encoding, cv::Mat& image)
 {
   const float minValue = gridMap.get(layer).minCoeffOfFinites();
   const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
   return toImage<Type_, NChannels_>(gridMap, layer, encoding, minValue, maxValue, image);
 };
Esempio n. 2
0
void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
                      grid_map_msgs::GridMap& message)
{
  message.info.header.stamp.fromNSec(gridMap.getTimestamp());
  message.info.header.frame_id = gridMap.getFrameId();
  message.info.resolution = gridMap.getResolution();
  message.info.length_x = gridMap.getLength().x();
  message.info.length_y = gridMap.getLength().y();
  message.info.pose.position.x = gridMap.getPosition().x();
  message.info.pose.position.y = gridMap.getPosition().y();
  message.info.pose.position.z = 0.0;
  message.info.pose.orientation.x = 0.0;
  message.info.pose.orientation.y = 0.0;
  message.info.pose.orientation.z = 0.0;
  message.info.pose.orientation.w = 1.0;

  message.layers = layers;
  message.basic_layers = gridMap.getBasicLayers();

  message.data.clear();
  for (const auto& layer : layers) {
    std_msgs::Float32MultiArray dataArray;
    matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
    message.data.push_back(dataArray);
  }

  message.outer_start_index = gridMap.getStartIndex()(0);
  message.inner_start_index = gridMap.getStartIndex()(1);
}
void ElevationChangeDetection::computeElevationChange(grid_map::GridMap& elevationMap)
{
  elevationMap.add("elevation_change", elevationMap.get(layer_));
  elevationMap.clear("elevation_change");

  for (GridMapIterator iterator(elevationMap);
      !iterator.isPastEnd(); ++iterator) {
    // Check if elevation map has valid value
    if (!elevationMap.isValid(*iterator, layer_)) continue;
    double height = elevationMap.at(layer_, *iterator);

    // Get the ground truth height
    Vector2d position, groundTruthPosition;
    Array2i groundTruthIndex;
    elevationMap.getPosition(*iterator, position);
    groundTruthMap_.getIndex(position, groundTruthIndex);
    if (!groundTruthMap_.isValid(groundTruthIndex, layer_)) continue;
    double groundTruthHeight = groundTruthMap_.at(layer_, groundTruthIndex);

    // Add to elevation change map
    double diffElevation = std::abs(height - groundTruthHeight);
    if (diffElevation <= threshold_) continue;
    elevationMap.at("elevation_change", *iterator) = diffElevation;
  }
}