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(DEPSPCLCeresgeometry_msgsmessage_filterspcl_conversionsoctomapoctomap_msgsrclcpptf2tf2_rossensor_msgsstd_msgsnav_msgsBoost
)link_directories(${OCTOMAP_LIBRARY_DIRS})ament_export_include_directories(include)add_executable(${PROJECT_NAME}_nodesrc/conversions.cppsrc/transforms.cpp  src/octomap_projection.cppsrc/main.cpp
)target_link_libraries(${PROJECT_NAME}_node${PCL_LIBRARIES}${OCTOMAP_LIBRARIES}
)ament_target_dependencies(${PROJECT_NAME}_node${DEPS}
)install(TARGETS ${PROJECT_NAME}_nodeARCHIVE DESTINATION libLIBRARY DESTINATION libRUNTIME DESTINATION bin)install(TARGETS ${PROJECT_NAME}_nodeRUNTIME 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. 实验结果

…详情请参照古月居

基于ROS2开发的点云体素化相关推荐

基于ROS2开发的点云栅格化
0. 简介最近在收到了很多读者的消息后,我觉得有必要开这个坑,来给大家阐述下如何对激光雷达点云以及图像点云去做栅格化以及体素化的操作.这部分需要各位读者拥有PCL,octomap,ROS2,C++的 ...
【云原生】基于Kubernetes开发的阿里云ACK之可观测监控
目录 Kubernetes监控类型 ACK监控体系 1.资源监控-云监控 2.日志监控-SLS 3.业务监控-ARMS 4.架构监控-AHAS ACK监控解决方案 Kubernetes监控类型 ACK ...
matlab体素化,一种三维激光点云数据快速体素化处理方法与流程
本发明涉及一种三维激光点云数据快速体素化处理方法. 背景技术: 目前,三维激光扫描系统快速发展,由于研究的需要,往往需要把不具有空间长度信息的点数据转为具有三维空间信息的立方体,如何使用软件进行快速. ...
史上最全 | 基于深度学习的3D分割综述（RGB-D/点云/体素/多目）
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[分割]术交流群后台回复[分割综述]获取语义分割.实例分割.全景分割.弱监督分割等超全 ...
小程序·云开发实战：SCRM社交化客户管理小程序
点击观看大咖分享随着微信小程序不断发展壮大,传统的 CRM 厂商也在不断向微信上迁移,毕竟微信的背后是巨大的用户和流量,还有极其方便的移动支付渠道.用微信小程序来做CRM,和以前的做法存在一些比较明 ...
云边有个小书屋——基于墨刀的读书APP原型化系统
云边有个小书屋--基于墨刀的读书APP原型化系统开发背景:近年来,环保问题越来越引起全球人民的关注,人们开始通过各种方式来保护我们共同的地球家园.而纸张浪费一直是众多问题中的比较严重的一个,所以国家 ...
三维点云体素滤波python_一种基于点云的Voxel（三维体素）特征的深度学习方法...
兰州大学在读硕士研究生,主要研究方向无人驾驶,深度学习:兰大未来计算研究院无人车团队负责人,自动驾驶全栈工程师. 之前我们提到使用SqueezeSeg进行了三维点云的分割,由于采用的是SqueezeN ...
利用PCL点云下采样实现数据体素化
利用PCL点云下采样实现数据体素化 PCL PCL(Point Cloud Library) 库集成了针对大体量级别的空间点数据处理所需要的算法和操作,降低了处理相关需求的复杂度,对快速建立点云数据文 ...
校园社团微信小程序，基于腾讯小程序云开发，后端完整代码包括社团通知，社团简介，社团福利，社团章程，社团招新，社团活动报名预约等
功能介绍校园社团小程序,前后端完整代码包括社团通知,社团简介,社团福利,社团章程,社团招新,社团活动报名预约等功能,采用腾讯提供的小程序云开发解决方案,无须服务器和域名预约管理:开始/截止时间/人 ...

基于ROS2开发的点云体素化