0. 简介

最近在收到了很多读者的消息后,我觉得有必要开这个坑,来给大家阐述下如何对激光雷达点云以及图像点云去做栅格化以及体素化的操作.这部分需要各位读者拥有PCL,octomap,ROS2,C++的一些基础.好了废话不多说,我们第一章主要介绍点云的栅格化.

1. 栅格化

点云地图存储的是传感器对环境的原始扫描点云,优点是保留信息完整,缺点是计算量大、但是不能直接用于导航避障;特征地图存储的是环境中的特殊几何特征,如电线杆、路标、障碍物边缘等,其计算量小但保留信息过少需进行过滤后才能进行使用。
 　　所以点云栅格化是非常有必要的,其核心思想是将激光雷达或者视觉所扫描到的区域用网格进行处理，每个栅格点云代表空间的一小块区域，内含一部分点云，点云栅格化处理分为二维栅格化和三维栅格化，二维其实就是将三维点云进行一个投影。不考虑ｚ值的变化。
这里我们先讲一下二维栅格化的处理,这部分在机器人中经常使用,具有较多的文档,比如Gmapping, Cartographer等二维激光雷达算法就是使用的是二维栅格地图的方法：
 　 该方法假设地面相对平坦，即地面扫描点的 z 轴方向的波动较小，通过将扫描区域进行栅格划分，将扫描点云投影到 xy 的栅格平面，通过统计栅格中 z 轴方面的最高点和最低点的差值(即，极差)，判断栅格中的点是否为地面点或障碍物点。

2. 环境配置

这一章依据了激光雷达常用的PCL函数库.详细的技术细节已经在文章:SLAM本质剖析-PCL中已经详细的提及.这里就不过多展开了.二维的栅格映射部分的操作只需要通过PCL库即可完成.

同时个人的开发已经基本全面转向了ROS2的开发.所以该整体代码为ROS2版本,ROS1版本的同学需要根据需求自行移植.

3. 相关代码

这里将核心代码进行展示,同时这部分代码将会在Github上实现开源,这里只是一个简单的pcd文件的读取并实现栅格化的过程.同时留好了ros2接口,可以轻松的根据自身的需求进行改变以及移植.
CMakeLists.txt

cmake_minimum_required(VERSION 3.3)
project(octomap_server)

add_compile_options(-std=c++14)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

set(CMAKE_BUILD_TYPE Release)
add_definitions(${PCL_DEFINITIONS})

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)


find_package(PCL REQUIRED)
find_package(Ceres REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(Boost REQUIRED)

include_directories(
include 
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${geometry_msgs_INCLUDE_DIRS}


set(DEPS
  PCL
  Ceres
  geometry_msgs
  message_filters
  pcl_conversions
  rclcpp
  tf2
  tf2_ros
  sensor_msgs
  std_msgs
  nav_msgs
  Boost
)

ament_export_include_directories(include)


add_executable(${PROJECT_NAME}_node
  src/conversions.cpp
  src/transforms.cpp  
  src/octomap_projection.cpp
  src/main.cpp
)

target_link_libraries(
  ${PROJECT_NAME}_node
  ${PCL_LIBRARIES}
  ${OCTOMAP_LIBRARIES}
)

ament_target_dependencies(${PROJECT_NAME}_node
  ${DEPS}
)

install(TARGETS ${PROJECT_NAME}_node
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

install(TARGETS ${PROJECT_NAME}_node
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)

install(
  DIRECTORY include/
  DESTINATION include
)

install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

ament_package()

octomap_projection.cpp

# include "octomap_projection.h"


namespace octomap_project
{
  OctomapProject::OctomapProject(): Node("octomap_projection")
  {
    InitParams();
  }
  
  OctomapProject::~OctomapProject()
  {

  }


  void OctomapProject::OctomapCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
  {
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
  }

  void OctomapProject::EachGridmap()
  {
   PassThroughFilter(false);
   SetMapTopicMsg(cloud_after_PassThrough_, map_topic_msg_);
  }
  



  void OctomapProject::PassThroughFilter(const bool& flag_in)
  {
    // 初始化,并通过tf2_ros::TransformListener完成对tf2_ros::Buffer类的初始化和构造，并订阅相应tf消息
    buffers_.reset(new tf2_ros::Buffer(this->get_clock()));
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*buffers_.get());
    
    
    /*方法一：直通滤波器对点云进行处理。*/
    cloud_after_PassThrough_.reset(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PassThrough<pcl::PointXYZ> passthrough;
    passthrough.setInputCloud(pcd_cloud_);//输入点云
    passthrough.setFilterFieldName("x");//对x轴进行操作
    passthrough.setFilterLimits(thre_x_min_, thre_x_max_);//设置直通滤波器操作范围
    passthrough.setFilterLimitsNegative(flag_in);//true表示保留范围外，false表示保留范围内
    passthrough.filter(*cloud_after_PassThrough_);//执行滤波，过滤结果保存在 cloud_after_PassThrough_

    passthrough.setFilterFieldName("y");//对y轴进行操作
    passthrough.setFilterLimits(thre_y_min_, thre_y_max_);//设置直通滤波器操作范围
    passthrough.filter(*cloud_after_PassThrough_);

    passthrough.setFilterFieldName("z");//对z轴进行操作
    passthrough.setFilterLimits(thre_z_min_, thre_z_max_);//设置直通滤波器操作范围
    passthrough.filter(*cloud_after_PassThrough_);

    std::cout << "直通滤波后点云数据点数：" << cloud_after_PassThrough_->points.size() << std::endl;
    try
    {
      echo_transform = buffers_->lookupTransform(world_frame_id_,robot_frame_id_,tf2::TimePoint());
      Eigen::Matrix4f matrix_transform = pcl_ros::transformAsMatrix(echo_transform);

      pcl::transformPointCloud(*cloud_after_PassThrough_.get(),*cloud_after_PassThrough_.get(),matrix_transform);
    }
    catch(const tf2::TransformException& ex)
    {
      RCLCPP_ERROR_STREAM(this->get_logger(), "Transform error of sensor data: " << ex.what() << ", quitting callback");
    }

  }

  void OctomapProject::SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::msg::OccupancyGrid& msg)
  {
    msg.header.stamp = builtin_interfaces::msg::Time(this->now());;
    msg.header.frame_id = "map";
    msg.info.map_load_time = builtin_interfaces::msg::Time(this->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())
    {
      RCLCPP_WARN(this->get_logger(),"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;
    }
  
    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_);//可以根据x_max和x_min来设置地图动态大小
    msg.info.height = int((y_max - y_min) / map_resolution_);
  
    msg.data.resize(msg.info.width * msg.info.height);
    msg.data.assign(msg.info.width * msg.info.height, 0);
  
    RCLCPP_INFO(this->get_logger(), "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;
      msg.data[i + j * msg.info.width] = 100;//int(255 * (cloud->points[iter].z * k_line + b_line)) % 255;
    }
  }

  bool OctomapProject::InitParams()
  {
    world_frame_id_ = "map";
    robot_frame_id_ = "robot";
    this->declare_parameter<float>("thre_x_min", -0.0);
    this->get_parameter_or<float>("thre_x_min", thre_x_min_, 0.0);
    this->declare_parameter<float>("thre_x_max", 2.0);
    this->get_parameter_or<float>("thre_x_max", thre_x_max_, 2.0);

    this->declare_parameter<float>("thre_y_min", 0.0);
    this->get_parameter_or<float>("thre_y_min", thre_y_min_, 0.0);
    this->declare_parameter<float>("thre_y_max", 2.0);
    this->get_parameter_or<float>("thre_y_max", thre_y_max_, 2.0);

    this->declare_parameter<float>("thre_z_min", 0.0);
    this->get_parameter_or<float>("thre_z_min", thre_z_min_, 0.0);
    this->declare_parameter<float>("thre_z_max", 2.0);
    this->get_parameter_or<float>("thre_z_max", thre_z_max_, 2.0);

    this->declare_parameter<float>("map_resolution", 0.05);
    this->get_parameter_or<float>("map_resolution", map_resolution_, 0.05);

    gridmap_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("grid_map", 1);

    fullMapPub_ = this->create_publisher<octomap_msgs::msg::Octomap>("octomap_full", 1);

    std::string pcd_file = "src/octomap_server/dat/pointcloudmap_2661935000.pcd";
    pcd_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());
    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;
    }


    std::cout << "初始点云数据点数：" << pcd_cloud_->points.size() << std::endl;
    EachGridmap();
    
    rclcpp::WallRate  loop_rate(5000);
    while (rclcpp::ok())
    {
	    gridmap_pub_->publish(map_topic_msg_);
	    loop_rate.sleep();
    }
    return 0;
  }
}

…详情请参照古月居