static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
const int encoding, cv::Mat& image)
{
const float minValue = gridMap.get(layer).minCoeffOfFinites();
const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
return toImage<Type_, NChannels_>(gridMap, layer, encoding, minValue, maxValue, image);
};
void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
grid_map_msgs::GridMap& message)
{
message.info.header.stamp.fromNSec(gridMap.getTimestamp());
message.info.header.frame_id = gridMap.getFrameId();
message.info.resolution = gridMap.getResolution();
message.info.length_x = gridMap.getLength().x();
message.info.length_y = gridMap.getLength().y();
message.info.pose.position.x = gridMap.getPosition().x();
message.info.pose.position.y = gridMap.getPosition().y();
message.info.pose.position.z = 0.0;
message.info.pose.orientation.x = 0.0;
message.info.pose.orientation.y = 0.0;
message.info.pose.orientation.z = 0.0;
message.info.pose.orientation.w = 1.0;
message.layers = layers;
message.basic_layers = gridMap.getBasicLayers();
message.data.clear();
for (const auto& layer : layers) {
std_msgs::Float32MultiArray dataArray;
matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray);
message.data.push_back(dataArray);
}
message.outer_start_index = gridMap.getStartIndex()(0);
message.inner_start_index = gridMap.getStartIndex()(1);
}
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;
}
}
