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;
}
bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
cv::Mat& cvImage, const float dataMin, const float dataMax)
{
if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
// Initialize blank image.
cvImage = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), CV_8UC4);
} else {
ROS_ERROR("Invalid grid map?");
return false;
}
// Clamp outliers.
grid_map::GridMap map = gridMap;
map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(dataMin, dataMax));
// Find upper and lower values.
float lowerValue = map.get(layer).minCoeffOfFinites();
float upperValue = map.get(layer).maxCoeffOfFinites();
uchar imageMax = std::numeric_limits<unsigned char>::max();
for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
if (map.isValid(*iterator, layer)) {
float value = map.at(layer, *iterator);
uchar imageValue = (uchar)(((value - lowerValue) / (upperValue - lowerValue)) * (float)imageMax);
grid_map::Index imageIndex(iterator.getUnwrappedIndex());
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[0] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[1] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[2] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[3] = imageMax;
}
}
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;
}
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;
};
static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
const int encoding, const float lowerValue, const float upperValue,
cv::Mat& image)
{
// Initialize image.
if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding);
} else {
std::cerr << "Invalid grid map?" << std::endl;
return false;
}
// Get max image value.
unsigned int imageMax = (unsigned int)std::numeric_limits<Type_>::max();
// Clamp outliers.
grid_map::GridMap map = gridMap;
map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(lowerValue, upperValue));
const grid_map::Matrix& data = gridMap[layer];
// Convert to image.
bool isColor = false;
if (image.channels() >= 3) isColor = true;
bool hasAlpha = false;
if (image.channels() >= 4) hasAlpha = true;
for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
const Index index(*iterator);
if (std::isfinite(data(index(0), index(1)))) {
const float& value = data(index(0), index(1));
const Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax);
const Index imageIndex(iterator.getUnwrappedIndex());
unsigned int channel = 0;
image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;
if (isColor) {
image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
}
if (hasAlpha) {
image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageMax;
}
}
}
return true;
};
bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end)
{
start_ = start;
end_ = end;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
initializeIterationParameters();
return true;
}
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;
}
}
PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
: polygon_(polygon)
{
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
Index submapStartIndex;
Size submapBufferSize;
findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
if(!isInside()) ++(*this);
}
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;
}
}
SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center,
const double radius)
: center_(center),
radius_(radius),
distance_(0)
{
radiusSquare_ = radius_ * radius_;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
gridMap.getIndex(center_, indexCenter_);
nRings_ = std::ceil(radius_ / resolution_);
if (checkIfIndexWithinRange(indexCenter_, bufferSize_)) pointsRing_.push_back(indexCenter_);
else generateRing();
}
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::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;
}
}
}