參考Adam大神的文章 激光雷達的地面-非地面分割和pcl_ros實踐
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/src
cd pcl_ws
catkin_make
source devel/setup.bash cd src catkin_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 文件：
1 cmake_minimum_required(VERSION 2.8.3)
2 project(pcl_test) 3 4 add_compile_options(-std=c++11) 5 6 7 find_package(catkin REQUIRED COMPONENTS 8 pcl_ros 9 roscpp 10 sensor_msgs 11 ) 12 13 14 catkin_package( 15 INCLUDE_DIRS include 16 CATKIN_DEPENDS roscpp sensor_msgs pcl_ros 17 ) 18 19 include_directories( 20 include 21 ${catkin_INCLUDE_DIRS} 22 ) 23 link_directories(${PCL_LIBRARY_DIRS}) 24 25 26 add_executable(${PROJECT_NAME}_node src/pcl_test_node.cpp src/pcl_test_core.cpp) 27 28 29 target_link_libraries(${PROJECT_NAME}_node 30 ${catkin_LIBRARIES} 31 ${PCL_LIBRARIES} 32 )
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函數，是節點的入口，編寫頭文件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） 來表示這個立方體的所有點，保留這些點作為降采樣的輸出。
驗證效果
pcl_test文件夾下創建launch文件夾
mkdir launch
cd launch
我們寫一個launch文件pcl_test.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）
rosbag play --clock test.bag
打開第三個終端，啟動Rviz：
rviz
配置Rviz的Fixed Frame為velodyne，
background color 改為 黑色
點擊 "Add" -->> By topic 選擇velodyne_points點雲
style 選擇 points
加載原始點雲和過濾以后的點雲的display
原始點雲：(topic 選擇 velodyne_points)
降采樣之后的點雲（即我們的節點的輸出）：(topic 選擇 /filtered_points)
