三维pcd地图转二维栅格地图

1.概述

在使用导航时，通常会根据二维栅格地图做路径规划，需要将三维点云地图转化成栅格地图。
本文采用滤波及投影的方法，
主要步骤包括

• 对输入点云进行直通滤波，获取限定高度范围的数据
• 在进行半径滤波，去除部分孤立点
• 转换为栅格地图

2.方法说明

完整程序代码:github

运行方法：
下载编译后，

mkdir -p test_ws/src&& cd test_ws/src
git clone -b develop  https://github.com/Hinson-A/pcd2pgm_package
cd ../
catkin_make

编译完成后，查看 src/pcd2pgm_package/pcd2pgm/launch/中的run.launch文件，修改相应的文件路径及名称

launch文件如下

<!-- -->
<launch>
<node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
<!-- 存放pcd文件的路径-->
<param name="file_directory" value= "/home/robot/map/" />
<!-- pcd文件名称-->
<param name="file_name" value= "map" />
<!-- 选取的范围　最小的高度-->
<param name="thre_z_min" value= "0.1" />
<!-- 选取的范围　最大的高度-->
<param name="thre_z_max" value= "1.5" />
<!--0 选取高度范围内的，１选取高度范围外的-->
<param name="flag_pass_through" value= "0" />
<!-- 半径滤波的半径-->
<param name="thre_radius" value= "0.5" />
<!-- 半径滤波的要求点数个数-->
<param name="thres_point_count" value= "10" />
<!-- 存储的栅格map的分辨率-->
<param name="map_resolution" value= "0.05" />
<!-- 转换后发布的二维地图的topic，默认使用map即可，可使用map_server保存-->
<param name="map_topic_name" value= "map" />
</node>

</launch>

修改完成后，运行程序

roslaun run.launch

此时，通过map_server 保存该栅格地图

rosrun map_server map_saver

生成对应的map文件

同时在原pcd文件夹下，也保存了直通滤波和半径滤波后的pcd文件。

原图：

直通滤波：

半径滤波：

二维栅格地图

3.关键程序解析

3.1 获取pcd文件

  // 下载pcd文件
  if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file, *pcd_cloud) == -1) {
    PCL_ERROR("Couldn't read file: %s \n", pcd_file.c_str());
    return (-1);
  }

3.2 进行直通滤波

指定字段，指定坐标范围进行裁剪。可以选择保留范围内的点或者范围外的点。
这里滤出设置范围之外的点云数据

//直通滤波器对点云进行过滤，获取设定高度范围内的数据
void PassThroughFilter(const double &thre_low, const double &thre_high,
                       const bool &flag_in) {
  // 创建滤波器对象
  pcl::PassThrough<pcl::PointXYZ> passthrough;
  //输入点云
  passthrough.setInputCloud(pcd_cloud);
  //设置对z轴进行操作
  passthrough.setFilterFieldName("z");
  //设置滤波范围
  passthrough.setFilterLimits(thre_low, thre_high);
  // true表示保留滤波范围外，false表示保留范围内
  passthrough.setFilterLimitsNegative(flag_in);
  //执行滤波并存储
  passthrough.filter(*cloud_after_PassThrough);
  // test 保存滤波后的点云到文件
  pcl::io::savePCDFile<pcl::PointXYZ>(file_directory + "map_filter.pcd",
                                      *cloud_after_PassThrough);
  std::cout << "直通滤波后点云数据点数："
            << cloud_after_PassThrough->points.size() << std::endl;
}

3.３ 进行半径滤波

半径滤波说明：
点云半径滤波也叫基于连通分析的点云滤波，该方法的基本思想是:假定原始点云中每个激光点在指定的半径邻域中至少包含一定数量的近邻点。原始点云中符合假设条件的激光点被视为正常点进行保留，反之，则视为噪声点并进行去除。
该方法对原始激光点云中存在的一些悬空的孤立点或无效点具有很好的去除效果。

//半径滤波
void RadiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcd_cloud0,
                         const double &radius, const int &thre_count) {
  //创建滤波器
  pcl::RadiusOutlierRemoval<pcl::PointXYZ> radiusoutlier;
  //设置输入点云
  radiusoutlier.setInputCloud(pcd_cloud0);
  //设置半径,在该范围内找临近点
  radiusoutlier.setRadiusSearch(radius);
  //设置查询点的邻域点集数，小于该阈值的删除
  radiusoutlier.setMinNeighborsInRadius(thre_count);
  radiusoutlier.filter(*cloud_after_Radius);
  // test 保存滤波后的点云到文件
  pcl::io::savePCDFile<pcl::PointXYZ>(file_directory + "map_radius_filter.pcd",
                                      *cloud_after_Radius);
  std::cout << "半径滤波后点云数据点数：" << cloud_after_Radius->points.size()
            << std::endl;
}

3.４ 发布map

转换为栅格地图数据并发布

//转换为栅格地图数据并发布
void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                    nav_msgs::OccupancyGrid &msg) {
  msg.header.seq = 0;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "map";

  msg.info.map_load_time = ros::Time::now();
  msg.info.resolution = map_resolution;

  double x_min, x_max, y_min, y_max;
  double z_max_grey_rate = 0.05;
  double z_min_grey_rate = 0.95;
  //? ? ??
  double k_line =
      (z_max_grey_rate - z_min_grey_rate) / (thre_z_max - thre_z_min);
  double b_line =
      (thre_z_max * z_min_grey_rate - thre_z_min * z_max_grey_rate) /
      (thre_z_max - thre_z_min);

  if (cloud->points.empty()) {
    ROS_WARN("pcd is empty!\n");
    return;
  }

  for (int i = 0; i < cloud->points.size() - 1; i++) {
    if (i == 0) {
      x_min = x_max = cloud->points[i].x;
      y_min = y_max = cloud->points[i].y;
    }

    double x = cloud->points[i].x;
    double y = cloud->points[i].y;

    if (x < x_min)
      x_min = x;
    if (x > x_max)
      x_max = x;

    if (y < y_min)
      y_min = y;
    if (y > y_max)
      y_max = y;
  }
  // origin的确定
  msg.info.origin.position.x = x_min;
  msg.info.origin.position.y = y_min;
  msg.info.origin.position.z = 0.0;
  msg.info.origin.orientation.x = 0.0;
  msg.info.origin.orientation.y = 0.0;
  msg.info.origin.orientation.z = 0.0;
  msg.info.origin.orientation.w = 1.0;
  //设置栅格地图大小
  msg.info.width = int((x_max - x_min) / map_resolution);
  msg.info.height = int((y_max - y_min) / map_resolution);
  //实际地图中某点坐标为(x,y)，对应栅格地图中坐标为[x*map.info.width+y]
  msg.data.resize(msg.info.width * msg.info.height);
  msg.data.assign(msg.info.width * msg.info.height, 0);

  ROS_INFO("data size = %d\n", msg.data.size());

  for (int iter = 0; iter < cloud->points.size(); iter++) {
    int i = int((cloud->points[iter].x - x_min) / map_resolution);
    if (i < 0 || i >= msg.info.width)
      continue;

    int j = int((cloud->points[iter].y - y_min) / map_resolution);
    if (j < 0 || j >= msg.info.height - 1)
      continue;
    // 栅格地图的占有概率[0,100]，这里设置为占据
    msg.data[i + j * msg.info.width] = 100;
    //    msg.data[i + j * msg.info.width] = int(255 * (cloud->points[iter].z *
    //    k_line + b_line)) % 255;
  }
}

reference pcd转pgm/3d点云转2d灰度图