Beispiel #1
0
  static bool addColorLayerFromImage(const cv::Mat& image, const std::string& layer,
                                     grid_map::GridMap& gridMap)
  {
    if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
      std::cerr << "Image size does not correspond to grid map size!" << std::endl;
      return false;
    }

    bool hasAlpha = false;
    if (image.channels() >= 4) hasAlpha = true;

    cv::Mat imageRGB;
    if (hasAlpha) {
      cv::cvtColor(image, imageRGB, CV_BGRA2RGB);
    } else {
      imageRGB = image;
    }

    gridMap.add(layer);

    for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
      const auto& cvColor = imageRGB.at<cv::Vec<Type_, 3>>((*iterator)(0), (*iterator)(1));
      Eigen::Vector3i colorVector;
      colorVector(0) = cvColor[0];
      colorVector(1) = cvColor[1];
      colorVector(2) = cvColor[2];
      colorVectorToValue(colorVector, gridMap.at(layer, *iterator));
    }

    return true;
  }
bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image,
                                                 const std::string& layer,
                                                 grid_map::GridMap& gridMap)
{
  cv_bridge::CvImagePtr cvPtr;
  try {
    cvPtr = cv_bridge::toCvCopy(image, image.encoding);
//    cvPtr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8); // FixMe
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return false;
  }

  gridMap.add(layer);

  if (gridMap.getSize()(0) != image.height || gridMap.getSize()(1) != image.width) {
    ROS_ERROR("Image size does not correspond to grid map size!");
    return false;
  }

  for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
    const auto& cvColor = cvPtr->image.at<cv::Vec3b>((*iterator)(0), (*iterator)(1));
    Eigen::Vector3i colorVector;
    // TODO Add cases for different image encodings.
    colorVector(2) = cvColor[0];  // From BGR to RGB.
    colorVector(1) = cvColor[1];
    colorVector(0) = cvColor[2];
    colorVectorToValue(colorVector, gridMap.at(layer, *iterator));
  }

  return true;
}
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;
  }
}
	void FillPolygonAreas(grid_map::GridMap &out_grid_map, const std::vector<std::vector<geometry_msgs::Point>> &in_area_points,
		                      const std::string &in_grid_layer_name, const int in_layer_background_value,
		                      const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value,
		                      const std::string &in_tf_target_frame, const std::string &in_tf_source_frame,
		                      const tf::TransformListener &in_tf_listener)
	{
		if(!out_grid_map.exists(in_grid_layer_name))
		{
			out_grid_map.add(in_grid_layer_name);
		}
		out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value);

		cv::Mat original_image;
		grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map,
		                                                        in_grid_layer_name,
		                                                        CV_8UC1,
		                                                        in_layer_min_value,
		                                                        in_layer_max_value,
		                                                        original_image);

		cv::Mat filled_image = original_image.clone();

		tf::StampedTransform tf = FindTransform(in_tf_target_frame, in_tf_source_frame, in_tf_listener);

		// calculate out_grid_map position
		grid_map::Position map_pos = out_grid_map.getPosition();
		double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x();
		double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y();

		for (const auto &points : in_area_points)
		{
			std::vector<cv::Point> cv_points;

			for (const auto &p : points)
			{
				// transform to GridMap coordinate
				geometry_msgs::Point tf_point = TransformPoint(p, tf);

				// coordinate conversion for cv image
				double cv_x = (out_grid_map.getLength().y() - origin_y_offset - tf_point.y) / out_grid_map.getResolution();
				double cv_y = (out_grid_map.getLength().x() - origin_x_offset - tf_point.x) / out_grid_map.getResolution();
				cv_points.emplace_back(cv::Point(cv_x, cv_y));
			}

			cv::fillConvexPoly(filled_image, cv_points.data(), cv_points.size(), cv::Scalar(in_fill_color));
		}

		// convert to ROS msg
		grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image,
		                                                                  in_grid_layer_name,
		                                                                  out_grid_map,
		                                                                  in_layer_min_value,
		                                                                  in_layer_max_value);
	}
Beispiel #5
0
  static bool addLayerFromImage(const cv::Mat& image, const std::string& layer,
                                grid_map::GridMap& gridMap, const float lowerValue = 0.0,
                                const float upperValue = 1.0, const double alphaThreshold = 0.5)
  {
    if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
      std::cerr << "Image size does not correspond to grid map size!" << std::endl;
      return false;
    }

    bool isColor = false;
    if (image.channels() >= 3) isColor = true;
    bool hasAlpha = false;
    if (image.channels() >= 4) hasAlpha = true;

    cv::Mat imageMono;
    if (isColor && !hasAlpha) {
      cv::cvtColor(image, imageMono, CV_BGR2GRAY);
    } else if (isColor && hasAlpha) {
      cv::cvtColor(image, imageMono, CV_BGRA2GRAY);
    } else if (!isColor && !hasAlpha){
      imageMono = image;
    } else {
      std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl;
      return false;
    }

    const float mapValueDifference = upperValue - lowerValue;
    const float maxImageValue = (float)std::numeric_limits<Type_>::max();
    const Type_ alphaTreshold = (Type_)(alphaThreshold * std::numeric_limits<Type_>::max());

    gridMap.add(layer);
    grid_map::Matrix& data = gridMap[layer];

    for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
      const Index index(*iterator);

      // Check for alpha layer.
      if (hasAlpha) {
        const Type_ alpha = image.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
        if (alpha < alphaTreshold) continue;
      }

      // Compute value.
      const Type_ imageValue = imageMono.at<Type_>(index(0), index(1));
      const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
      data(index(0), index(1)) = mapValue;
    }

    return true;
  };
bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid,
                                            const std::string& layer, grid_map::GridMap& gridMap)
{
  const Size size(occupancyGrid.info.width, occupancyGrid.info.height);
  const double resolution = occupancyGrid.info.resolution;
  const Length length = resolution * size.cast<double>();
  const string& frameId = occupancyGrid.header.frame_id;
  Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y);
  // Different conventions of center of map.
  position += 0.5 * length.matrix();

  const auto& orientation = occupancyGrid.info.origin.orientation;
  if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0
      && orientation.z == 0 && orientation.w == 0)) {
    ROS_WARN_STREAM("Conversion of occupancy grid: Grid maps do not support orientation.");
    ROS_INFO_STREAM("Orientation of occupancy grid: " << endl << occupancyGrid.info.origin.orientation);
    return false;
  }

  if (size.prod() != occupancyGrid.data.size()) {
    ROS_WARN_STREAM("Conversion of occupancy grid: Size of data does not correspond to width * height.");
    return false;
  }

  if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution
      || (gridMap.getLength() != length).any() || gridMap.getPosition() != position
      || gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) {
    gridMap.setTimestamp(occupancyGrid.header.stamp.toNSec());
    gridMap.setFrameId(frameId);
    gridMap.setGeometry(length, resolution, position);
  }

  // Reverse iteration is required because of different conventions
  // between occupancy grid and grid map.
  grid_map::Matrix data(size(0), size(1));
  for (std::vector<int8_t>::const_reverse_iterator iterator = occupancyGrid.data.rbegin();
      iterator != occupancyGrid.data.rend(); ++iterator) {
    size_t i = std::distance(occupancyGrid.data.rbegin(), iterator);
    data(i) = *iterator != -1 ? *iterator : NAN;
  }

  gridMap.add(layer, data);
  return true;
}
bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap)
{
  gridMap.setTimestamp(message.info.header.stamp.toNSec());
  gridMap.setFrameId(message.info.header.frame_id);
  gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution,
                      Position(message.info.pose.position.x, message.info.pose.position.y));

  if (message.layers.size() != message.data.size()) {
    ROS_ERROR("Different number of layers and data in grid map message.");
    return false;
  }

  for (unsigned int i = 0; i < message.layers.size(); i++) {
    Matrix data;
    multiArrayMessageCopyToMatrixEigen(message.data[i], data); // TODO Could we use the data mapping (instead of copying) method here?
    // TODO Check if size is good.   size_ << getRows(message.data[0]), getCols(message.data[0]);
    gridMap.add(message.layers[i], data);
  }

  gridMap.setBasicLayers(message.basic_layers);
  gridMap.setStartIndex(Index(message.outer_start_index, message.inner_start_index));
  return true;
}
bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image,
                                            const std::string& layer, grid_map::GridMap& gridMap,
                                            const double lowerValue, const double upperValue)
{
  cv_bridge::CvImagePtr cvPtrAlpha, cvPtrMono;

  // If alpha channel exist, read it.
  if (image.encoding == sensor_msgs::image_encodings::BGRA8
      || image.encoding == sensor_msgs::image_encodings::BGRA16) {
    try {
      cvPtrAlpha = cv_bridge::toCvCopy(image, image.encoding);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return false;
    }
  }

  unsigned int depth;
  // Convert color image to grayscale.
  try {
    if (image.encoding == sensor_msgs::image_encodings::BGRA8
        || image.encoding == sensor_msgs::image_encodings::BGR8
        || image.encoding == sensor_msgs::image_encodings::MONO8) {
      cvPtrMono = cv_bridge::toCvCopy(image,
                                      sensor_msgs::image_encodings::MONO8);
      depth = std::pow(2, 8);
      ROS_DEBUG("Color image converted to mono8");
    } else if (image.encoding == sensor_msgs::image_encodings::BGRA16
        || image.encoding == sensor_msgs::image_encodings::BGR16
        || image.encoding == sensor_msgs::image_encodings::MONO16) {
      cvPtrMono = cv_bridge::toCvCopy(image,
                                      sensor_msgs::image_encodings::MONO16);
      depth = std::pow(2, 16);
      ROS_DEBUG("Color image converted to mono16");
    } else {
      ROS_ERROR("Expected BGR, BGRA, or MONO image encoding.");
      return false;
    }
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return false;
  }

  gridMap.add(layer);

  if (gridMap.getSize()(0) != image.height
      || gridMap.getSize()(1) != image.width) {
    ROS_ERROR("Image size does not correspond to grid map size!");
    return false;
  }

  for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
    // Set transparent values.
    if (image.encoding == sensor_msgs::image_encodings::BGRA8) {
      const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec4b>((*iterator)(0),
                                                            (*iterator)(1));
      unsigned int alpha = cvAlpha[3];
      if (cvAlpha[3] < depth / 2)
        continue;
    }
    if (image.encoding == sensor_msgs::image_encodings::BGRA16) {
      const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec<uchar, 8>>(
          (*iterator)(0), (*iterator)(1));
      int alpha = (cvAlpha[6] << 8) + cvAlpha[7];
      if (alpha < depth / 2)
        continue;
    }

    // Compute height.
    unsigned int grayValue;
    if (depth == std::pow(2, 8)) {
      uchar cvGrayscale = cvPtrMono->image.at<uchar>((*iterator)(0),
                                                     (*iterator)(1));
      grayValue = cvGrayscale;
    }
    if (depth == std::pow(2, 16)) {
      const auto& cvGrayscale = cvPtrMono->image.at<cv::Vec2b>((*iterator)(0),
                                                               (*iterator)(1));
      grayValue = (cvGrayscale[0] << 8) + cvGrayscale[1];
    }

    double height = lowerValue
        + (upperValue - lowerValue) * ((double) grayValue / (double) depth);
    gridMap.at(layer, *iterator) = height;
  }

  return true;
}