点击上方“3D视觉工坊”,选择“星标”
干货第一时间送达
作者丨AdamShan@CSDN
来源丨https://blog.csdn.net/AdamShan/article/details/82901295
编辑丨睿小妹 睿慕课
PCL基本入门
PCL是一个开源的点云处理库,是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库,它实现了大量点云相关的通用算法和高效数据结构,包含点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等大量开源代码。支持多种操作系统平台,可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。如果说OpenCV是2D信息获取与处理的结晶,那么PCL就在3D信息获取与处理上具有同等地位。ROS kinetic完整版中本身已经包含了pcl库,同时ROS自带的pcl_ros 包可以连接ROS和PCL库。我们从一个简单的Voxel Grid Filter的ROS节点实现来了解一下PCL在ROS中的基本用法,同时了解PCL中的一些基本数据结构:
在ROS项目中引入PCL库
在此我们假定读者已经自行安装好ROS kinetic 的完整版,首先在我们的catkin workspace中新建一个package,我们将它命名为pcl_test,可以通过如下指令生成workspace和package:
cd ~mkdir -p pcl_ws/srccd pcl_wscatkin_makesource devel/setup.bashcd srccatkin_create_pkg pcl_test roscpp sensor_msgs pcl_ros
这样,我们就新建了一个workspace,用于学习PCL,同时新建了一个名为pcl_test的package,这个ROS包依赖于roscpp,sensor_msgs, pcl_ros这几个包,我们修改pcl_test包下的CMakeList文件以及package.xml配置文件,如下:
package.xml 文件:
<?xml version="1.0"?><package> <name>pcl_test</name> <version>0.0.1</version> <description>The pcl_test package</description> <maintainer email="shenzb12@lzu.edu.cn">adam</maintainer> <license>MIT</license> <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>sensor_msgs</build_depend> <build_depend>pcl_ros</build_depend> <run_depend>roscpp</run_depend> <run_depend>sensor_msgs</run_depend> <run_depend>pcl_ros</run_depend></package>
CMakeList.txt 文件:
cmake_minimum_required(VERSION 2.8.3)project(pcl_test)add_compile_options(-std=c++11)find_package(catkin REQUIRED COMPONENTSpcl_rosroscppsensor_msgs)catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS roscpp sensor_msgs pcl_ros)include_directories( include ${catkin_INCLUDE_DIRS})link_directories(${PCL_LIBRARY_DIRS})add_executable(${PROJECT_NAME}_node src/pcl_test_node.cpp src/pcl_test_core.cpp)target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${PCL_LIBRARIES})
package.xml的内容很简单,实际上就是这个包的描述文件,build_depend
和 run_depend 两个描述符分别指定了程序包编译和运行的依赖项,通常是所用到的库文件的名称。在这里我们指定了三个编译和运行时依赖项,分别是roscpp(编写C++ ROS节点),sensor_msgs(定义了激光雷达的msg),pcl_ros(连接ROS和pcl库)。
同样的,在CMakeList中,我们通过 find_package 查找这三个包的路径,然后将三个包添加到 CATKIN_DEPENDS , 在使用pcl库前,需要将PCL库的路径链接,通过 link_directories(${PCL_LIBRARY_DIRS}) 来完成,并在最后的 target_link_libraries 中添加 ${PCL_LIBRARIES} 。
编写节点进行Voxel Grid Filter
接着我们在pcl_test/src目录下新建 pcl_test_node.cpp 文件:
#include "pcl_test_core.h"
int main(int argc, char **argv){ ros::init(argc, argv, "pcl_test");
ros::NodeHandle nh;
PclTestCore core(nh); return 0;}
此文件仅包含main函数,是节点的入口,编写头文件 include/pcl_test_core.h :
#pragma once#include <ros/ros.h>#include <pcl_conversions/pcl_conversions.h>#include <pcl/point_types.h>#include <pcl/conversions.h>#include <pcl_ros/transforms.h>#include <pcl/filters/voxel_grid.h>#include <sensor_msgs/PointCloud2.h>class PclTestCore{ private: ros::Subscriber sub_point_cloud_; ros::Publisher pub_filtered_points_; void point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud); public: PclTestCore(ros::NodeHandle &nh); ~PclTestCore(); void Spin();};
以及 pcl_test_core.cpp :
#include "pcl_test_core.h"PclTestCore::PclTestCore(ros::NodeHandle &nh){ sub_point_cloud_ = nh.subscribe("/velodyne_points",10, &PclTestCore::point_cb, this); pub_filtered_points_ = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10); ros::spin();}PclTestCore::~PclTestCore(){}void PclTestCore::Spin(){}void PclTestCore::point_cb(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr){ pcl::PointCloud<pcl::PointXYZI>::Ptr current_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>); pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>); pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr); pcl::VoxelGrid<pcl::PointXYZI> vg; vg.setInputCloud(current_pc_ptr); vg.setLeafSize(0.2f, 0.2f, 0.2f); vg.filter(*filtered_pc_ptr); sensor_msgs::PointCloud2 pub_pc; pcl::toROSMsg(*filtered_pc_ptr, pub_pc); pub_pc.header = in_cloud_ptr->header; pub_filtered_points_.publish(pub_pc);}
这个节点的功能是订阅来自 /velodyne_points 话题的点云数据,使用PCL内置的Voxel Grid Filter对原始的点云进行降采样,将降采样的结果发布到 /filtered_points 话题上。我们重点看回调函数 PclTestCore::point_cb ,在该回调函数中,我们首先定义了两个点云指针,在PCL库中, pcl::PointCloud<T> 是最基本的一种数据结构,它表示一块点云数据(点的集合),我们可以指定点的数据结构,在上述实例中,采用了 pcl::PointXYZI 这种类型的点。pcl::PointXYZI 结构体使用(x, y, z, intensity)这四个数值来描述一个三维度空间点。
intensity,即反射强度,是指激光雷达的激光发射器发射激光后收到的反射的强度,通常所说的16线,32线激光雷达,其内部实际是并列纵排的多个激光发射器,通过电机自旋,产生360环视的点云数据,不同颜色的物体对激光的反射强度也是不同的,通常来说,白色物体的反射强度(intensity)最强,对应的,黑色的反射强度最弱。
通常使用sensor_msgs/PointCloud2.h 做为点云数据的消息格式,可使用pcl::fromROSMsg和pcl::toROSMsg将sensor_msgs::PointCloud2与pcl::PointCloud<T> 进行转换。
为了使用Voxel Grid Filter对原始点云进行降采样,只需定义 pcl::VocelGrid 并且指定输入点云和leaf size,在本例中,我们使用leaf size为 0.2。Voxel Grid Filter将输入点云使用0.2m*0.2m*0.2m的立方体进行分割,使用小立方体的 形心(centroid) 来表示这个立方体的所有点,保留这些点作为降采样的输出。
验证效果
我们写一个launch文件 pcl_test.launch 来启动这个节点:
<launch> <node pkg="pcl_test" type="pcl_test_node" name="pcl_test_node" output="screen"/></launch>
回到workspace 目录,使用catkin_make 编译:
catkin_make
启动这个节点:
roslaunch pcl_test pcl_test.launch
新建终端,并运行我们的测试bag。测试bag下载链接:
https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w
打开第三个终端,启动Rviz:
rosrun rviz rviz
配置Rviz的Frame为velodyne,并且加载原始点云和过滤以后的点云的display
原始点云:
降采样之后的点云(即我们的节点的输出):
点云地面过滤
过滤地面是激光雷达感知中一步基础的预处理操作,因为我们环境感知通常只对路面上的障碍物感兴趣,且地面的点对于障碍物聚类容易产生影响,所以在做Lidar Obstacle Detection之前通常将地面点和非地面点进行分离。在此文中我们介绍一种被称为Ray Ground Filter的路面过滤方法,并且在ROS中实践。
由于代码过多,文章只节选关键部分讲解。完整代码:
https://download.csdn.net/download/adamshan/10696443
对点云剪裁和过滤
要分割地面和非地面,那么过高的区域首先就可以忽略不计,我们先对点云进行高度的裁剪。我们实验用的bag在录制的时候lidar的高度约为1.78米,我们剪裁掉1.28米以上的部分,代码如下:
void PclTestCore::clip_above(double clip_height, const pcl::PointCloud<pcl::PointXYZI>::Ptr in, const pcl::PointCloud<pcl::PointXYZI>::Ptr out){ pcl::ExtractIndices<pcl::PointXYZI> cliper; cliper.setInputCloud(in); pcl::PointIndices indices;#pragma omp for for (size_t i = 0; i < in->points.size(); i++) { if (in->points[i].z > clip_height) { indices.indices.push_back(i); } } cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices)); cliper.setNegative(true); //ture to remove the indices cliper.filter(*out);}
为了消除车身自身的雷达反射的影响,我们对近距离的点云进行过滤,仍然使用 pcl::ExtractIndices 进行剪裁:
void PclTestCore::remove_close_pt(double min_distance, const pcl::PointCloud<pcl::PointXYZI>::Ptr in, const pcl::PointCloud<pcl::PointXYZI>::Ptr out){ pcl::ExtractIndices<pcl::PointXYZI> cliper; cliper.setInputCloud(in); pcl::PointIndices indices;#pragma omp for for (size_t i = 0; i < in->points.size(); i++) { double distance = sqrt(in->points[i].x*in->points[i].x + in->points[i].y*in->points[i].y); if (distance < min_distance) { indices.indices.push_back(i); } } cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices)); cliper.setNegative(true); //ture to remove the indices cliper.filter(*out);}
其中,#pragma omp for语法OpenMP的并行化语法,即希望通过OpenMP并行化执行这条语句后的for循环,从而起到加速的效果。
角度微分和地面/非地面判断
Ray Ground Filter算法的核心是以射线(Ray)的形式来组织点云。我们现在将点云的 (x, y, z)三维空间降到(x,y)平面来看,计算每一个点到车辆x正方向的平面夹角 θ , 我们对360度进行微分,分成若干等份,每一份的角度为0.18度,这个微分的等份近似的可以看作一条射线,如下图所示,图中是一个激光雷达的纵截面的示意图,雷达由下至上分布多个激光器,发出如图所示的放射状激光束,这些激光束在平地上即表现为,图中的水平线即为一条射线:
0.18度是VLP32C雷达的水平光束发散间隔。
为了方便地对点进行半径和夹角的表示,我们使用如下数据结构代替pcl::PointCloudXYZI:
struct PointXYZIRTColor { pcl::PointXYZI point; float radius; //cylindric coords on XY Plane float theta; //angle deg on XY plane size_t radial_div; //index of the radial divsion to which this point belongs to size_t concentric_div; //index of the concentric division to which this points belongs to size_t original_index; //index of this point in the source pointcloud }; typedef std::vector<PointXYZIRTColor> PointCloudXYZIRTColor;
其中,radius 表示点到lidar的水平距离(半径),即:
theta 是点相对于车头正方向(即x方向)的夹角,计算公式为:
我们用 radial_div 和 concentric_div 分别描述角度微分和距离微分。对点云进行水平角度微分之后,可得到:条射线,将这些射线中的点按照距离的远近进行排序,如下所示:
//将同一根射线上的点按照半径(距离)排序#pragma omp for for (size_t i = 0; i < radial_dividers_num_; i++) { std::sort(out_radial_ordered_clouds[i].begin(), out_radial_ordered_clouds[i].end(),[](const PointXYZIRTColor &a, const PointXYZIRTColor &b) { return a.radius < b.radius; }); }
通过判断射线中前后两点的坡度是否大于我们事先设定的坡度阈值,从而判断点是否为地面点。代码如下:
void PclTestCore::classify_pc(std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds, pcl::PointIndices &out_ground_indices, pcl::PointIndices &out_no_ground_indices){ out_ground_indices.indices.clear(); out_no_ground_indices.indices.clear();#pragma omp for for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) //sweep through each radial division 遍历每一根射线 { float prev_radius = 0.f; float prev_height = -SENSOR_HEIGHT; bool prev_ground = false; bool current_ground = false; for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) //loop through each point in the radial div { float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius; float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance; float current_height = in_radial_ordered_clouds[i][j].point.z; float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius; //for points which are very close causing the height threshold to be tiny, set a minimum value if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_) { height_threshold = min_height_threshold_; } //check current point height against the LOCAL threshold (previous point) if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold)) { //Check again using general geometry (radius from origin) if previous points wasn't ground if (!prev_ground) { if (current_height <= (-SENSOR_HEIGHT + general_height_threshold) && current_height >= (-SENSOR_HEIGHT - general_height_threshold)) { current_ground = true; } else { current_ground = false; } } else { current_ground = true; } } else { //check if previous point is too far from previous one, if so classify again if (points_distance > reclass_distance_threshold_ && (current_height <= (-SENSOR_HEIGHT + height_threshold) && current_height >= (-SENSOR_HEIGHT - height_threshold))) { current_ground = true; } else { current_ground = false; } } if (current_ground) { out_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index); prev_ground = true; } else { out_no_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index); prev_ground = false; } prev_radius = in_radial_ordered_clouds[i][j].radius; prev_height = in_radial_ordered_clouds[i][j].point.z; } }}
这里有两个重要参数,一个是 local_max_slope_ ,是我们设定的同条射线上邻近两点的坡度阈值,一个是 general_max_slope_ ,表示整个地面的坡度阈值,这两个坡度阈值的单位为度(degree),我们通过这两个坡度阈值以及当前点的半径(到lidar的水平距离)求得高度阈值,通过判断当前点的高度(即点的z值)是否在地面加减高度阈值范围内来判断当前点是为地面。
在地面判断条件中, current_height <= (-SENSOR_HEIGHT + general_height_threshold) && current_height >= (-SENSOR_HEIGHT - general_height_threshold) 中SENSOR_HEIGHT表示lidar挂载的高度,-SNESOR_HEIGHT即表示水平地面。
分割效果
我们使用上文中的bag来验证地面分割节点的工作效果。运行bag并且运行我们的节点,打开Rviz,加载两个点云display,效果如下所示:其中,红色的点为我们分割出来的地面,来自于 /filtered_points_ground 话题,白色的点为非地面,来自于 /filtered_points_no_ground 话题。分割出非地面点云之后,我们就可以让Lidar Detection的代码工作在这个点云上了,从而排除了地面对于Lidar聚类以及Detection的影响。
· end ·
本文仅做学术分享,如有侵权,请联系删文。
下载1
在「3D视觉工坊」公众号后台回复:3D视觉,即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2
在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总,即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。
下载3
在「3D视觉工坊」公众号后台回复:相机标定,即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配,即可下载独家立体匹配学习课件与视频网址。
重磅!3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群,目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球:针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近2000星球成员为创造更好的AI世界共同进步,知识星球入口:
学习3D视觉核心技术,扫描查看介绍,3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用,麻烦给个赞和在看~