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