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