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(DEPSPCLCeresgeometry_msgsmessage_filterspcl_conversionsrclcpptf2tf2_rossensor_msgsstd_msgsnav_msgsBoost
)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::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;}
}

…详情请参照古月居

基于ROS2开发的点云栅格化相关推荐

基于ROS2开发的点云体素化
0. 简介最近在收到了很多读者的消息后,我觉得有必要开这个坑,来给大家阐述下如何对激光雷达点云以及图像点云去做栅格化以及体素化的操作.这部分需要各位读者拥有PCL,octomap,ROS2,C++的 ...
【云原生】基于Kubernetes开发的阿里云ACK之可观测监控
目录 Kubernetes监控类型 ACK监控体系 1.资源监控-云监控 2.日志监控-SLS 3.业务监控-ARMS 4.架构监控-AHAS ACK监控解决方案 Kubernetes监控类型 ACK ...
校园社团微信小程序，基于腾讯小程序云开发，后端完整代码包括社团通知，社团简介，社团福利，社团章程，社团招新，社团活动报名预约等
功能介绍校园社团小程序,前后端完整代码包括社团通知,社团简介,社团福利,社团章程,社团招新,社团活动报名预约等功能,采用腾讯提供的小程序云开发解决方案,无须服务器和域名预约管理:开始/截止时间/人 ...
【软件应用开发】基于SSM框架的共享云盘系统设计与实现
本文编写目的:描述共享云盘系统的实现功能.运行环境以及系统的使用说明. 共享云盘系统完整代码:https://download.csdn.net/download/weixin_47936614/86 ...
【安卓开发系列 -- 系统开发】搭建云手机容器环境 (基于 openvmi)
[安卓开发系列 -- 系统开发]搭建云手机容器环境 (基于 openvmi) [1]编译安装 openvmi [1.1]安装相关依赖 apt install -y build-essential cm ...
使用创思通信4G Cat1 DTU基于TencentOS-tiny对接腾讯云物联网开发平台
一.简介本文档主要讲述如何使用创思通信4G Cat1 DTU开发板,基于TencentOS-tiny对接腾讯云物联网开发平台IoT Explorer,演示温度数据上报平台.平台下发控制指令控制继电器 ...
基于深度学习的点云分割网络及点云分割数据集
作者丨泡椒味的泡泡糖来源丨深蓝AI 引言点云分割是根据空间.几何和纹理等特征对点云进行划分,使得同一划分内的点云拥有相似的特征.点云的有效分割是许多应用的前提,例如在三维重建领域,需要对场景内的物 ...
前沿丨基于深度学习的点云分割网络及点云分割数据集
众所周知,点云的有效分割是许多应用的前提,例如在三维重建领域,需要对场景内的物体首先进行分类处理,然后才能进行后期的识别和重建.传统的点云分割主要依赖聚类算法和基于随机采样一致性的分割算法,在很多技术 ...
CVPR 2022 | PTTR: 基于Transformer的三维点云目标跟踪
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨罗志鹏来源丨商汤学术导读在CVPR 2022上,商汤研究院团队提出了基于Transform ...

基于ROS2开发的点云栅格化