0. 简介

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

1. octomap

octomap是一种基于八叉树的三维地图创建工具, 可以显示包含无障碍区域及未映射区域的完整3D图形, 而且基于占有率栅格的传感器数据可以在多次测量中实现融合和更新; 地图可提供多种分辨率, 数据可压缩, 存储紧凑. 事实上, octomap的代码主要包含两个模块: 三维地图创建工具octomap和可视化工具octovis。
相比点云，能够省下大把的空间。octomap建立的地图大概是这样子的：（从左到右是不同的分辨率）。当然体素图越精细所耗费的资源是越多的。

2. 环境配置

这一章依据了激光雷达常用的PCL函数库,以及octomap函数库.这里给出在ubuntu20下的octomap的安装步骤:

• 使用git从github下载OctoMap库。

  git clone https：//github.com/OctoMap/octomap
  

• OctoMap的编译依赖于以下几个库

  sudo apt-get install build-essential cmake doxygen libqt4-dev \
  libqt4-opengl-dev libqglviewer-qt4-dev
  

• 编译octomap

  cd octomap
  mkdir build
  cd build
  cmake ..
  make
  

• 尝试一下OctoMap的图形显示功能，输入：

  bin/octovis octomap/sample.bt
  

同时个人的开发已经基本全面转向了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)
find_package(octomap REQUIRED)
find_package(octomap_msgs REQUIRED)


set(PCL_DEFINITIONS "-DFLANN_STATIC-DDISABLE_ENSENSO-DDISABLE_DAVIDSDK-DDISABLE_DSSDK-DDISABLE_PCAP-DDISABLE_PNG-DDISABLE_LIBUSB_1_0-Dqh_QHpointer-D-ffloat-store")

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


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

link_directories(${OCTOMAP_LIBRARY_DIRS})

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::EachOctomap()
  {
    // PassThroughFilter(false);
    SetOctoMapTopicMsg(echo_transform.transform.translation,cloud_after_PassThrough_,octomap_);
  }

  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::SetOctoMapTopicMsg(geometry_msgs::msg::Vector3 & sensor_tf, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, octomap_msgs::msg::Octomap& msg)
  {
    m_octree_ = new octomap::OcTree(0.05f);
    m_octree_->setProbHit(0.7);
    m_octree_->setProbMiss(0.4);
    m_octree_->setClampingThresMin(0.12);
    m_octree_->setClampingThresMax(0.97);
    sensor_tf.x =1.0;
    sensor_tf.y =1.0;
    sensor_tf.z =1.0;

    octomap::point3d octomap_point = octomap::pointTfToOctomap(sensor_tf);


    if(!m_octree_->coordToKeyChecked(octomap_point,m_updateBBXMin) || !m_octree_->coordToKeyChecked(octomap_point,m_updateBBXMax))//将三维坐标转为3D octreekey,并进行边界检查
    {
      RCLCPP_WARN(this->get_logger(),
                        "Could not generate Key for origin");
    }
    octomap::KeySet free_set, occupied_cells;
    octomap::KeyRay octkey_ray;
    unsigned char* colors = new unsigned char[3];

    for(auto it = cloud->begin(); it!=cloud->end(); it++)
    {
      octomap::point3d point(it->x,it->y,it->z);
      if(m_octree_->computeRayKeys(octomap_point,point,octkey_ray))//computeRayKeys函数的参数origin（光束起点）和参数end（传感器末端击中点）
      {
        free_set.insert(octkey_ray.begin(),octkey_ray.end());
      }
      octomap::OcTreeKey key;
      if (m_octree_->coordToKeyChecked(point, key)) {
          occupied_cells.insert(key);

          updateMinKey(key, m_updateBBXMin);
          updateMaxKey(key, m_updateBBXMax);
      }
    }
    for(auto it = free_set.begin(), end=free_set.end();
        it!= end; ++it){
        if (occupied_cells.find(*it) == occupied_cells.end()){
            // m_octree_->updateNode(*it, false);
        }
        else
        {
          m_octree_->updateNode(*it, true);
        }
    }

    m_octree_->updateInnerOccupancy();
    m_octree_->writeBinary("sample.bt");
    std::cout<<"done"<<std::endl;

  }


  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();
    EachOctomap();
    
    rclcpp::WallRate  loop_rate(5000);
    while (rclcpp::ok())
    {
    gridmap_pub_->publish(map_topic_msg_);
      if (octomap_msgs::fullMapToMsg(*m_octree_, octomap_)) {
          fullMapPub_->publish(octomap_);
          RCLCPP_INFO(this->get_logger(),
                        "Start OctoMap");
      } else {            
          RCLCPP_ERROR(this->get_logger(),
                        "Error serializing OctoMap");
      }
    loop_rate.sleep();
    }


    return 0;

  }
}

4. 实验结果

…详情请参照古月居