本文旨在帮助读者将激光点云地图转为2D栅格地图，以便完成路径规划与导航。本方法将pcd转为pgm的原理是将接收到的点云信息以"/map"话题的形式发布，用map_server来接收"/map"话题，保存2D栅格地图！

废话不多说，直接开始！

一、安装pcd2pgm

#创建工作空间
mkdir -p ~/pcd2pgm_ws/src
cd ~/pcd2pgm_ws/src
catkin_init_workspace#克隆代码
git clone https://github.com/hujiax380/pcd2pgm.git

转换可执行文件test.cpp如下，需要修改pcd文件路径及名称：

#include <ros/ros.h>#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>  //直通滤波器头文件
#include <pcl/filters/voxel_grid.h>  //体素滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h>   //统计滤波器头文件
#include <pcl/filters/conditional_removal.h>    //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>   //半径滤波器头文件std::string file_directory;
std::string file_name;
std::string pcd_file;std::string map_topic_name;const std::string pcd_format = ".pcd";nav_msgs::OccupancyGrid map_topic_msg;double thre_z_min = 0.3;
double thre_z_max = 2.0;
int flag_pass_through = 0;double grid_x = 0.1;
double grid_y = 0.1;
double grid_z = 0.1;double map_resolution = 0.05;double thre_radius = 0.1;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_Radius(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud(new pcl::PointCloud<pcl::PointXYZ>);void PassThroughFilter(const double& thre_low, const double& thre_high, const bool& flag_in);void RadiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcd_cloud, const double &radius, const int &thre_count);void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::OccupancyGrid& msg);int main(int argc, char** argv)
{ros::init(argc, argv, "pcl_filters");ros::NodeHandle nh;ros::NodeHandle private_nh("~");ros::Rate loop_rate(1.0);private_nh.param("file_directory", file_directory, std::string("/home/ubuntu/"));//此处需要修改为自己pcd文件的路径ROS_INFO("*** file_directory = %s ***\n", file_directory.c_str());private_nh.param("file_name", file_name, std::string("good2"));//此时"good2"需要修改为自己的pcd文件名，无需.pcdROS_INFO("*** file_name = %s ***\n", file_name.c_str());pcd_file = file_directory + file_name + pcd_format;ROS_INFO("*** pcd_file = %s ***\n", pcd_file.c_str());private_nh.param("thre_z_min", thre_z_min, 0.5);private_nh.param("thre_z_max", thre_z_max, 2.0);private_nh.param("flag_pass_through", flag_pass_through, 0);private_nh.param("grid_x", grid_x, 0.1);private_nh.param("grid_y", grid_y, 0.1);private_nh.param("grid_z", grid_z, 0.1);private_nh.param("thre_radius", thre_radius, 0.5);private_nh.param("map_resolution", map_resolution, 0.05);private_nh.param("map_topic_name", map_topic_name, std::string("map"));ros::Publisher map_topic_pub = nh.advertise<nav_msgs::OccupancyGrid>(map_topic_name, 1);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;PassThroughFilter(thre_z_min, thre_z_max, bool(flag_pass_through));//   RadiusOutlierFilter(cloud_after_PassThrough, 0.1, 10);
//   SetMapTopicMsg(cloud_after_Radius, map_topic_msg);SetMapTopicMsg(cloud_after_PassThrough, map_topic_msg);while(ros::ok()){map_topic_pub.publish(map_topic_msg);loop_rate.sleep();ros::spinOnce();}return 0;
}void PassThroughFilter(const double &thre_low, const double &thre_high, const bool &flag_in)
{/*方法一：直通滤波器对点云进行处理。*/pcl::PassThrough<pcl::PointXYZ> passthrough;passthrough.setInputCloud(pcd_cloud);//输入点云passthrough.setFilterFieldName("z");//对z轴进行操作passthrough.setFilterLimits(thre_low, thre_high);//设置直通滤波器操作范围passthrough.setFilterLimitsNegative(flag_in);//true表示保留范围外，false表示保留范围内passthrough.filter(*cloud_after_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThroughstd::cout << "直通滤波后点云数据点数：" << cloud_after_PassThrough->points.size() << std::endl;
}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);     //设置radius为100的范围内找临近点radiusoutlier.setMinNeighborsInRadius(thre_count); //设置查询点的邻域点集数小于2的删除radiusoutlier.filter(*cloud_after_Radius);std::cout << "半径滤波后点云数据点数：" << cloud_after_Radius->points.size() << std::endl;
}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;}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);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;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;}
}

修改之后完成编译：

cd ~/pcd2pgm_ws
catkin_make

二、转换地图

编译成功后：

roscore#另起终端
rosrun pcd2pgm pcd2topic

终端显示pcd文件信息：

另起终端启动map_server

#另起终端，保存地图
rosrun map_server map_saver

如此就生成可用作2D导航的yaml与pgm文件！

本文主要参考：pcd转pgm/3d点云转2d灰度图

3D激光SLAM点云地图pcd转导航可用的2D栅格地图相关推荐

3D激光SLAM：LeGO-LOAM论文解读---激光雷达里程计与建图
3D激光SLAM:LeGO-LOAM论文解读---激光雷达里程计与建图激光雷达里程计针对LOAM的改进激光雷达建图原文激光雷达里程计激光雷达里程计模块的功能就是:估计相邻帧之间的位姿变换. ...
3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析前言利用地面点优化利用角点优化代码部分 gazebo测试前言 LeGO-LOAM的全称是 Lightweight an ...
SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中. 由于参与分享汇报的同事有许多是做其他方向的机器人工程师(包括硬件.控制等各方面并 ...
A-LOAM/LOAM/Lego-LOAM/SC_Lego_LOAM实时构建3d点云地图与2d栅格地图（octomap）
本篇文章旨在快速让读者实现运行LOAM系列3D激光SLAM算法并实时构建3d点云地图与2d栅格地图.废话不多说,直接开始~ 首先说明我们要用到的3D点云地图转2D栅格地图的工具是octomap,附上高 ...
3D激光SLAM:LOAM 论文--算法详细解读
3D激光SLAM:LOAM 论文--算法详细解读 LOAM简介论文里面的符号表示算法部分激光雷达里程计 A 特征点提取 B 找特征点的匹配对 C 运动估计 lidar 建图测试结果 LOAM是 ...
激光SLAM算法学习（三）——3D激光SLAM
3D激光SLAM 1.3D激光SLAM的介绍 3D激光SLAM的输入: IMU数据 3D激光雷达数据里程计数据 3D激光SLAM的输出: 3D点云地图机器人的轨迹 or PoseGraph 2.3 ...
彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总！
地图定位算法是自动驾驶模块的核心,而激光SLAM则是地图定位算法的关键技术,其重要性不言而喻,在许多AI产品中应用非常多(包括但不限于自动驾驶.移动机器人.扫地机等).相比于传统的视觉传感器,激光传感 ...
国庆特惠！超全技术栈来袭！视觉/激光SLAM+点云处理+三维重建+多传感器融合...
随着AI技术的大规模落地,国内外资本对AI视觉兴趣只增不减,自动驾驶.工业视觉.AR/VR.测量测绘.移动机器人等领域涌现了大量独角兽公司,相关产品更是受到重点关注. 自动驾驶是一个技术输出密集程度非 ...
32线镭神雷达跑LeGO-LOAM：3D 激光SLAM
32线镭神雷达跑LeGO-LOAM:3D 激光SLAM 安装LeGO-LOAM 镭神雷达的相关修改 LeGO-LOAM的修改修改utility.h 修改imageproject.cpp Enjoy ...

3D激光SLAM点云地图pcd转导航可用的2D栅格地图

一、安装pcd2pgm

二、转换地图

3D激光SLAM点云地图pcd转导航可用的2D栅格地图相关推荐

最新文章

热门文章