将pcl二维点云转化为一张pgm图片

在处理建图之后的地图时，可能对建图效果不太满意，需要利用pcl库将地图转化为点云，通过一些方法处理完之后，再将好看的点云转化回地图，这个过程需要利用到ros中的nav_msgs/OccupancyGrid格式的topic进行转换。

首先我们看看nav_msgs/OccupancyGrid的官方资料：

Header header 

#MetaData for the map
MapMetaData info

# The map data, in row-major order, starting with (0,0).  Occupancy
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data

这是为二维栅格地图准备的消息格式，分为header（时间戳、frameID），nav_msgs/MapMetaData消息，data[ ]三部分。其中data[ ]代表着整个地图的栅格序列，数组值代表着在某个栅格中地图的“灰度值”（是否有点）。

再看看nav_msgs/MapMetaData消息：

# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad].  This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin

地图分辨率是指栅格的大小，通常为0.02m，而地图的长度、宽度是指X/Y方向上栅格的数量，我们可以利用点云的长度与宽度进行计算，得到合适的地图大小。

接下来我们利用KD树将点云存为树状结构，利用这个树，查找每个栅格的最近pcl点，若是落在栅格内把这个栅格标为100，没有点的就标0：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

//do something...//

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
pcl::PointXYZ searchPoint;

for(int i=0;i<mapWidth;i++)
{
  for(int j=0;j<mapHeight;j++)
  {
    searchPoint.x=minX+(i+0.5)*resolution;
    searchPoint.y=minY+(j+0.5)*resolution;
    searchPoint.z=0;
  
    int K=1;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    if(kdtree.nearestKSearch(searchPoint,K,pointIdxNKNSearch, pointNKNSquaredDistance)>0)
    {
      if(sqrt(pointNKNSquaredDistance[0])<0.01)
      {
        data.push_back(100);
      }
      else data.push_back(0);
    }
  }
}

这样我们便得到了一个完整的nav_msgs/OccupancyGrid消息，我们将它发送出去，再编写一个保存为地图的subscriber，这个就很简单了，参照map_server的map_saver就好~