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