occupancyGrid 栅格地图转cv::mat图片

将occupancyGrid栅格地图转换为cv::Mat图片的步骤如下：

1. 获取栅格地图信息，包括地图的宽度、高度和分辨率等信息。

2. 创建一个空的cv::Mat图片，大小为栅格地图的宽度和高度。

3. 遍历栅格地图中的每个栅格，将其转换为像素点，并在cv::Mat图片中填充相应的像素值。这里可以使用不同的颜色表示不同的栅格状态，比如黑色表示障碍物，白色表示自由空间。

4. 最后将cv::Mat图片保存为图片文件或显示在屏幕上。

下面是一个示例代码，可以将ROS中的occupancyGrid栅格地图转换为cv::Mat图片：

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

void occupancyGridToMat(const nav_msgs::OccupancyGrid& occupancy_grid, cv::Mat& image)
{
    int width = occupancy_grid.info.width;
    int height = occupancy_grid.info.height;
    double resolution = occupancy_grid.info.resolution;
    double origin_x = occupancy_grid.info.origin.position.x;
    double origin_y = occupancy_grid.info.origin.position.y;

    image.create(height, width, CV_8UC1);
    for (int y = 0; y < height; y++)
    {
        for (int x = 0; x < width; x++)
        {
            int index = x + y * width;
            int value = occupancy_grid.data[index];
            if (value == 100)
            {
                image.at<uchar>(y, x) = 0; // black represents obstacles
            }
            else if (value == 0)
            {
                image.at<uchar>(y, x) = 255; // white represents free space
            }
            else
            {
                image.at<uchar>(y, x) = 127; // gray represents unknown space
            }
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "occupancy_grid_to_mat");
    ros::NodeHandle nh;

    // subscribe to the occupancy grid topic
    nav_msgs::OccupancyGrid::ConstPtr occupancy_grid_ptr;
    occupancy_grid_ptr = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("map", nh);

    // convert occupancy grid to cv::Mat image
    cv::Mat image;
    occupancyGridToMat(*occupancy_grid_ptr, image);

    // display the image
    cv::imshow("Occupancy Grid", image);
    cv::waitKey(0);

    return 0;
}

在这个示例中，我们订阅ROS中的地图话题，并将其转换为cv::Mat图片。然后，我们将图片显示在屏幕上。你可以将代码中的显示部分替换为保存图片到文件的代码。