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 ElevationVisualization::visualize( const grid_map::GridMap& map, const std::string& typeNameElevation, const std::string& typeNameColor, const float lowerValueBound, const float upperValueBound) { // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); marker_.scale.x = map.getResolution(); marker_.scale.y = map.getResolution(); // Clear points. marker_.points.clear(); marker_.colors.clear(); float markerHeightOffset = static_cast<float>(markerHeight_/2.0); const Eigen::Array2i buffSize = map.getBufferSize(); const Eigen::Array2i buffStartIndex = map.getBufferStartIndex(); const bool haveColor = map.isType("color"); for (unsigned int i = 0; i < buffSize(0); ++i) { for (unsigned int j = 0; j < buffSize(1); ++j) { // Getting map data. const Eigen::Array2i cellIndex(i, j); const Eigen::Array2i buffIndex = grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex); const float& elevation = map.at(typeNameElevation, buffIndex); if(std::isnan(elevation)) continue; const float color = haveColor ? map.at(typeNameColor, buffIndex) : lowerValueBound; // Add marker point. Eigen::Vector2d position; map.getPosition(buffIndex, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); point.z = elevation - markerHeightOffset; marker_.points.push_back(point); // Add marker color. if(haveColor) { std_msgs::ColorRGBA markerColor = grid_map_visualization::color_utils::interpolateBetweenColors( color, lowerValueBound, upperValueBound, lowerColor_, upperColor_); marker_.colors.push_back(markerColor); } } } markerPublisher_.publish(marker_); }
void ObjectsToCostmap::setCostInPolygon(const grid_map::Polygon& polygon, const std::string& gridmap_layer_name, const float score, grid_map::GridMap& objects_costmap) { grid_map::PolygonIterator iterators(objects_costmap, polygon); for (grid_map::PolygonIterator iterator(objects_costmap, polygon); !iterator.isPastEnd(); ++iterator) { const float current_score = objects_costmap.at(gridmap_layer_name, *iterator); if (score > current_score) { objects_costmap.at(gridmap_layer_name, *iterator) = score; } } }
bool VectorVisualization::visualize(const grid_map::GridMap& map) { if (!isActive()) return true; for (const auto& type : types_) { if (!map.exists(type)) { ROS_WARN_STREAM( "VectorVisualization::visualize: No grid map layer with name '" << type << "' found."); return false; } } // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); // Clear points. marker_.points.clear(); marker_.colors.clear(); for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue; geometry_msgs::Vector3 vector; vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set vector.y = map.at(types_[1], *iterator); vector.z = map.at(types_[2], *iterator); Eigen::Vector3d position; map.getPosition3(positionLayer_, *iterator, position); geometry_msgs::Point startPoint; startPoint.x = position.x(); startPoint.y = position.y(); startPoint.z = position.z(); marker_.points.push_back(startPoint); geometry_msgs::Point endPoint; endPoint.x = startPoint.x + scale_ * vector.x; endPoint.y = startPoint.y + scale_ * vector.y; endPoint.z = startPoint.z + scale_ * vector.z; marker_.points.push_back(endPoint); marker_.colors.push_back(color_); // Each vertex needs a color. marker_.colors.push_back(color_); } publisher_.publish(marker_); 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; }
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 VectorVisualization::visualize(const grid_map::GridMap& map) { if (markerPublisher_.getNumSubscribers () < 1) return true; // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); // Clear points. marker_.points.clear(); marker_.colors.clear(); for (grid_map_lib::GridMapIterator iterator(map); !iterator.isPassedEnd(); ++iterator) { if (!map.isValid(*iterator) || !map.isValid(*iterator, types_)) continue; geometry_msgs::Vector3 vector; vector.x = map.at(types_[0], *iterator); vector.y = map.at(types_[1], *iterator); vector.z = map.at(types_[2], *iterator); Eigen::Vector3d position; map.getPosition3d(positionType_, *iterator, position); geometry_msgs::Point startPoint; startPoint.x = position.x(); startPoint.y = position.y(); startPoint.z = position.z(); marker_.points.push_back(startPoint); geometry_msgs::Point endPoint; endPoint.x = startPoint.x + scale_ * vector.x; endPoint.y = startPoint.y + scale_ * vector.y; endPoint.z = startPoint.z + scale_ * vector.z; marker_.points.push_back(endPoint); marker_.colors.push_back(color_); // Each vertex needs a color. marker_.colors.push_back(color_); } markerPublisher_.publish(marker_); return true; }
void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer, float lowerThreshold, float upperThreshold, nav_msgs::GridCells& gridCells) { gridCells.header.frame_id = gridMap.getFrameId(); gridCells.header.stamp.fromNSec(gridMap.getTimestamp()); gridCells.cell_width = gridMap.getResolution(); gridCells.cell_height = gridMap.getResolution(); for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { if (!gridMap.isValid(*iterator, layer)) continue; if (gridMap.at(layer, *iterator) >= lowerThreshold && gridMap.at(layer, *iterator) <= upperThreshold) { Position position; gridMap.getPosition(*iterator, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); gridCells.cells.push_back(point); } } }
void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer, float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid) { occupancyGrid.header.frame_id = gridMap.getFrameId(); occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp()); occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; // Same as header stamp as we do not load the map. occupancyGrid.info.resolution = gridMap.getResolution(); occupancyGrid.info.width = gridMap.getSize()(0); occupancyGrid.info.height = gridMap.getSize()(1); Position positionOfOrigin; getPositionOfDataStructureOrigin(gridMap.getPosition(), gridMap.getLength(), positionOfOrigin); occupancyGrid.info.origin.position.x = positionOfOrigin.x(); occupancyGrid.info.origin.position.y = positionOfOrigin.y(); occupancyGrid.info.origin.position.z = 0.0; occupancyGrid.info.origin.orientation.x = 0.0; occupancyGrid.info.origin.orientation.y = 0.0; occupancyGrid.info.origin.orientation.z = 1.0; // yes, this is correct. occupancyGrid.info.origin.orientation.w = 0.0; occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height); // Occupancy probabilities are in the range [0,100]. Unknown is -1. const float cellMin = 0; const float cellMax = 100; const float cellRange = cellMax - cellMin; for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin); if (isnan(value)) value = -1; else value = cellMin + min(max(0.0f, value), 1.0f) * cellRange; // Occupancy grid claims to be row-major order, but it does not seem that way. // http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html. unsigned int index = get1dIndexFrom2dIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false); occupancyGrid.data[index] = value; } }
void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer, float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid) { occupancyGrid.header.frame_id = gridMap.getFrameId(); occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp()); occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; // Same as header stamp as we do not load the map. occupancyGrid.info.resolution = gridMap.getResolution(); occupancyGrid.info.width = gridMap.getSize()(0); occupancyGrid.info.height = gridMap.getSize()(1); Position position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix(); occupancyGrid.info.origin.position.x = position.x(); occupancyGrid.info.origin.position.y = position.y(); occupancyGrid.info.origin.position.z = 0.0; occupancyGrid.info.origin.orientation.x = 0.0; occupancyGrid.info.origin.orientation.y = 0.0; occupancyGrid.info.origin.orientation.z = 0.0; occupancyGrid.info.origin.orientation.w = 1.0; size_t nCells = gridMap.getSize().prod(); occupancyGrid.data.resize(nCells); // Occupancy probabilities are in the range [0,100]. Unknown is -1. const float cellMin = 0; const float cellMax = 100; const float cellRange = cellMax - cellMin; for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin); if (isnan(value)) value = -1; else value = cellMin + min(max(0.0f, value), 1.0f) * cellRange; size_t index = getLinearIndexFromIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false); // Reverse cell order because of different conventions between occupancy grid and grid map. occupancyGrid.data[nCells - index - 1] = value; } }
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; }
void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers, const std::string& pointLayer, sensor_msgs::PointCloud2& pointCloud) { // Header. pointCloud.header.frame_id = gridMap.getFrameId(); pointCloud.header.stamp.fromNSec(gridMap.getTimestamp()); pointCloud.is_dense = false; // Fields. std::vector <std::string> fieldNames; for (const auto& layer : layers) { if (layer == pointLayer) { fieldNames.push_back("x"); fieldNames.push_back("y"); fieldNames.push_back("z"); } else if (layer == "color") { fieldNames.push_back("rgb"); } else { fieldNames.push_back(layer); } } pointCloud.fields.clear(); pointCloud.fields.reserve(fieldNames.size()); int offset = 0; for (auto& name : fieldNames) { sensor_msgs::PointField point_field; point_field.name = name; point_field.count = 1; point_field.datatype = sensor_msgs::PointField::FLOAT32; point_field.offset = offset; pointCloud.fields.push_back(point_field); offset = offset + point_field.count * 4; // 4 for sensor_msgs::PointField::FLOAT32 } // Resize. size_t nPoints = gridMap.getSize().prod(); pointCloud.height = 1; pointCloud.width = nPoints; pointCloud.point_step = offset; pointCloud.row_step = pointCloud.width * pointCloud.point_step; pointCloud.data.resize(pointCloud.height * pointCloud.row_step); // Points. std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators; for (auto& name : fieldNames) { fieldIterators.insert( std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>( name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name))); } GridMapIterator mapIterator(gridMap); for (size_t i = 0; i < nPoints; ++i) { Position3 position; position.setConstant(NAN); gridMap.getPosition3(pointLayer, *mapIterator, position); for (auto& iterator : fieldIterators) { if (iterator.first == "x") { *iterator.second = (float) position.x(); } else if (iterator.first == "y") { *iterator.second = (float) position.y(); } else if (iterator.first == "z") { *iterator.second = (float) position.z(); } else if (iterator.first == "rgb") { *iterator.second = gridMap.at("color", *mapIterator); } else { *iterator.second = gridMap.at(iterator.first, *mapIterator); } } ++mapIterator; for (auto& iterator : fieldIterators) { ++iterator.second; } } }
void SurfaceVisualization::visualize( const grid_map::GridMap& map, const std::string& typeNameSurface, const std::string& typeNameColor, const float lowerValueBound, const float upperValueBound) { // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); marker_.scale.x = map.getResolution(); marker_.scale.y = map.getResolution(); // Clear points. marker_.points.clear(); marker_.colors.clear(); float markerHeightOffset = static_cast<float>(markerHeight_/2.0); const Eigen::Array2i buffSize = map.getBufferSize(); const Eigen::Array2i buffStartIndex = map.getBufferStartIndex(); const bool haveColor = map.isType("class"); for (unsigned int i = 0; i < buffSize(0); ++i) { for (unsigned int j = 0; j < buffSize(1); ++j) { // Getting map data. const Eigen::Array2i cellIndex(i, j); const Eigen::Array2i buffIndex = grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex); const float& elevation = map.at("mu", buffIndex); const float& class_pred = map.at("class", buffIndex); int class_number=0; if(!std::isnan(class_pred)) { class_number=(int)class_pred; } if(std::isnan(elevation)) { continue; } //const float color = haveColor ? 16711680/class_pred : lowerValueBound; //ROS_INFO_STREAM("class_pred: "<<class_pred <<" int: "<<(int)class_pred); std_msgs::ColorRGBA surface_color; surface_color.a = 1.0; switch (class_number) { case 1: //long_grass surface_color.r = 0.0; surface_color.g = 1.0; surface_color.b = 0.0; break; case 3: //cobblestone surface_color.r = 0.62; surface_color.g = 0.32; surface_color.b = 0.17; break; case 4: //gravel surface_color.r = 0.2; surface_color.g = 0.2; surface_color.b = 0.2; break; default: surface_color.r = 1.0; surface_color.g = 1.0; surface_color.b = 1.0; break; } // Add marker point. Eigen::Vector2d position; map.getPosition(buffIndex, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); point.z = elevation - markerHeightOffset; marker_.points.push_back(point); // Add marker color. if(haveColor) { // std_msgs::ColorRGBA markerColor = // grid_map_visualization::color_utils::interpolateBetweenColors( // color, lowerValueBound, upperValueBound, lowerColor_, upperColor_); //marker_.colors.push_back(markerColor); marker_.colors.push_back(surface_color); } } } markerPublisher_.publish(marker_); }