目录

前言

1、理解点云库

1.1、不同的点云类型

1.2、PCL中的算法

1.3、ROS的PCL接口

2、创建第一个PCL程序

2.1、创建点云

2.2、加载点云文件

2.3、创建点云文件

2.4、点云可视化

2.5、点云滤波和下采样

2.5.1、点云滤波

 2.5.2、点云下采样

2.6、点云配准与匹配

---

前言

        点云是一种能够直观地表示和操作3D传感器所提供数据的方式，这类传感器包括深度摄像头和激光雷达。该类传感器在三维坐标参考系下对空间进行有限点集采样构成点云。通俗的来说，点云就是分布在三维空间的有限个点，这些点具体化的表示了三维传感器所采集到的数据，我们可以很容易的通过点云来查看空间中物体的位置。

下面是一张固态激光雷达扫描的树林中的一栋房屋点云图：

        点云库（Point Cloud Library）提供了大量的数据类型和数据结构，不仅能够方便的表示采样空间中的点，而且可以表示采样空间的不同属性，比如颜色、法向量等。PCL同样提供了许多最先进的算法对数据样本进行处理，比如滤波、模型估计、表面重建等。ROS提供了一种基于消息的接口（PCL点云可以通过该接口进行有效的通信），还有一组将本地的PCL类型转换到ROS消息的转换函数，这和处理OpenCV图像一样。上面的图片就是调用PCL来处理过的点云图像。

        本文主要介绍PCL库的背景、相关数据类型以及ROS接口消息，然后演示如何使用PCL处理数据以及如何通过ROS发送和接收数据的技术。

1、理解点云库

点云库和ROS的PCL接口的基本概念：

点云库：处理三维数据提供了一组数据结构和算法；

ROS的PCL接口：提供一组消息以及消息与PCL数据结构之间的转换函数。

从C++的角度来说，PCL包含了一个非常重要的数据结构，那就是点云。这个数据结构被设计成一个 模板类，它把点的类型当做模板类的参数，点云类实际上是一个容器，这个容器里包含了所有点云需要的公共信息，而不管点是什么类型。下面是点云中最重要的公共字段：

header：这个字段是pcl:PCLHeader类型。指定了点云的获取时间。

points：这个字段是std::vector<PointT…>类型，它是储存所有点的容器。vector定义中的PointT对 应于类的模板参数，即点的类型。

width：这个字段指定了点云组织成一种图像时的宽度，否则它包含的是云中点的数量。

height：这个字段指定了点云组织成一种图像时的高度，否则它总是1。

is_dense：这个字段指定了点云中是否有无效值(无穷大或NaN值)。

sensor origin：这个字段是Eigen::Vector4f类型，并且定义了传感器根据相对于原点的平移所得到的位姿。

sensororientation：这个字段是Eigen::Ouaternionf类型，并且定义了传感器旋转所得到的位姿。

PCL算法利用这些字段来处理数据，并且用户可以利用它们来创造自己的算法。一但明白了点云的结构，下一步就是理解一个点云可以包含不同的点类型、PCL如何工作以及ROS中的PCL 接口。

1.1、不同的点云类型

        正如前面描述的一样，pcl::PointCloud包含了一个字段，它作为一个容器为点提供服务。这个字段就是PointT类型，它是pcl:PointCloud类的模板参数，并且定义了云所要储存的点类型。PCL定义了许多不同类型的点，下面是一些最常用到的类型:

pcl::PointXYZ: 这是最简单也可能是最常用到的点类型;它只储存了3D xyz的信息。

pcl::PointXYZI: 这种类型非常类似于上面的那种，但它还包含了一个描述点亮度(intensity)的字段。当想要获取传感器返回的亮度高于一定级别的点时非常有用。还有与此相似的其他两种标准的点数据类型:一是pcl:InterestPoint，它有一个字段储存强度(streneth)；二是pcl::PointWithRange:它有一个字段储存距离(视点到采样点)。

pcl::PointXYZRGBA: 这种点类型储存3D信息，也储存颜色(RGB=Red，Green， Blue)和透明度(A=Alpha)。

pcl::PointXYZRGB: 这种点类型与前面的点类型相似，但是它没有透明度字段。

pcl:Normal: 这是最常用的点类型，表示曲面上给定点处的法线以及测量的曲率。

pcl::PointNormal: 这种点类型跟前一个点类型一样；它包含了给定点所在曲面法线以及曲率信息，但是它也包含了点的3Dxyz坐标。这种点类型的变异类型是 PointXYZRGBNormal和PointXYZINormal，正如名字所提示，它们包含了颜色(前者)和亮度(后者)。

        除了这些常用的点类型外，还有许多标准的PCL类型，比如PointWithViewpointMomentInvariants（具有视点力矩不变量的点）、BoundaryPrincipalCurvatures（边界主曲率）、Histogram(直方图)等。更重要的是，PCL算法都是模板化的，所以这样不仅可以使用已经可用的类型，理论上还可以使用用户定义的语法正确的类型。

1.2、PCL中的算法

        整个PCL函数库都使用了非常具体的设计模式，该设计模式定义了点云处理算法。通常来说，这些类型算法的问题是它们高度可配置，为了完全发挥它们的潜能，这个库必须为用户提供一个可以指定所有要求的参数的机制以及常用的默认值。

        为了解决这个问题，PCL的开发者决定把每个算法做成一个类，这个类属于一个有着特定共性的继承类。这个方法允许PCL开发者通过获取已经存在的算法并加上新算法所需要的参数，以重用这些算法，并且它允许用户通过存取器轻松地为算法提供它所需要的参数值，而其余的参数都取默认值。下面的代码展示了通常是如何使用PCL算法的:                

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
pcl::Algorithm<pcl::PointXYZ> algorithm; 
algorithm.setInputCloud(cloud); 
algorithmsetParameter(1.0);
algorithm.setAnotherParameter(033); 
algorithmprocess(*result)

1.3、ROS的PCL接口

通过ROS自带的基于消息的通信系统，ROS的PCL接口提供了与PCL数据结构进行通信所需要的方法。为此，这里定义了不同的消息类型去处理点云和其他PCL算法中产生的数据。结合这些消息类型，也提供了一组将本地PCL数据类型转换为消息的函数。其中一些最重要的消息类型如下所示：

std_msgs::Header: 这不是真的消息类型。但它通常是每一个ROS消息的一部分。它包含消息发送时间、序列号和坐标系名称等信息。这个PCL类型等价于pcl::Header type。

sensor_msgs::PointCloud2: 这也许是最重要的消息类型。这个消息用来转换pcl::PointCloud类型。然而必须考虑的是，在未来支持pcl::PCLPointCloud2的PCL版本中这个消息类型将会弃用。

pcl_msgs::PointIndices: 这个消息类型储存了一个点云中点的索引，等价的PCL类型是pcl::PointIndices。（PointIndices:点索引）

pcl_msgs::PolygonMesh: 这个消息类型保存了描绘网格、顶点和多边形的信息。等价的PCL类型是pcl::PolygonMesh。（PolygonMesh：多边形网格）

pcl_msgs::Vertices: 这个消息类型将一组顶点的索引保存在一个数组中，例如描述一个多边形。等价的 PCL类型是pcl:Vertices。（Vertices：顶点）

pcl_msgs::ModelCoefficients: 这个消息类型储存了一个模型的不同系数，例如描述一个平面需要的四个参数。等价的PCL类型是pcl::ModelCoefficients。（ModelCoefficients：模型系数）

通过ROS的PCL功能包提供的转换函数可以将前面的消息转换为PCL类型或者从PCL类型转换为消息。所有这些函数都有一个相似签名(signature)，这意味着一旦我们知道如何转换一个类型，就知道如何转换所有的类型了。下面的函数量由pclconversions命名空间提供的:

void fromPCL(const<PCL Type>&ROS Message types&); 
void MoveFromPCL(<PCL Type>&ROS Mesage type>&); 
void toPCL(const R0S Message types &, <PCL Type> &); 
void moveToPCL(<ROS Message type> &, <PCL Type>&);

这里，PCL Type必须用一个预先指定的PCL类型替代，ROS Message type必须用消息替代。sensormsgs::PointCloud2指定了一组函数执行这些转换:

void toROSMsg(const pcl:PointCloud<T>&, sensor_msgs::Pointcloud2 &);
void fromROSMsg(const sensor_msgs::PointCloud2 &, pcl:PointCloud<T>&);
void moveFromROSMsg(sensor_msgs::PointCloud2 &, pcl:PointCloud<T>&);

你也许会好奇每个函数和它的move版本之间的区别。答案很简单，标准版本执行对数据的深复制。而move 版本执行浅复制并注销源数据容器。这称为“移动语义”(movesemantic)。

2、创建第一个PCL程序

        本节将学习如何集成PCL和ROS。非常有必要知道并理解ROS软件如何布局以及编译，尽管这些步骤是简单的重复。在第一个PCL程序中用到的示例除了能够成功编译一个有效的ROS节点之外没有任何其他作用。

在工作空间中创建一个ROS软件包。这个软件包依赖于pcl_conversions、pcl_ros、pcl_msgs和sensors_msgs包：

mkdir -p pclstudy_ws/src
cd pclstudy_ws/src
catkin_create_pkg chapter6_tutorials pcl_ros roscpp rospy sensor_msgs std_msgs 
cd ..
catkin_make 

在/pclstudy_ws/src/chapter6_tutorials内的CMakeList.txt添加如下内容：

catkin_package()
find_package(PCL REQUIRED) 
include_directories(include${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS}) 
add_executable(pcl_create src/pcl_create.cpp)
target_link_libraries(pcl_create ${catkin_LIBRARIES} ${PCL_LIBRARIES})

2.1、创建点云

pcl_create.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pcl_create");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output",1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    cloud.width = 100;
    cloud.height  = 1;
    cloud.points.resize(cloud.width*cloud.height);

    for(size_t i =0;i<cloud.points.size();i++){
        cloud.points[i].x = 1024*rand() / (RAND_MAX+1.0f);
        cloud.points[i].y = 1024*rand() / (RAND_MAX+1.0f);
        cloud.points[i].z = 1024*rand() / (RAND_MAX+1.0f);
    }

    pcl::toROSMsg(cloud,output);
    output.header.frame_id = "odm";

    ros::Rate loop_rate(1);
    while(ros::ok()){
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

编译：

cd ~/pclstudy_ws | catkin_make 

刷新环境：

source devel/setup.bash

运行和在RVIZ中显示：

#分三个终端输入
roscore
rosrun chapter6_tutorials pcl_create
rosrun rviz rviz

在rviz中增加PointCloud2、topic选择pcl_output、fixed Frame输入odom，结果如图：

2.2、加载点云文件

PCL提供了一些标准的文件格式加载和存储点云到磁盘，研究者常用这种方法将有趣的数据集分享给其他人去试验。这种格式叫做PCD，经过设计它可以支持PCL指定的扩展。

PCD文件格式：

# .PCD v.5 - Point Cloud Data file format
FIELDS x y z intensity distance sid
SIZE 4 4 4 4 4 4
TYPE F F F F F F
COUNT 1 1 1 1 1 1
WIDTH 460400
HEIGHT 1
POINTS 460400
DATA ascii
-0.93387 -0.6825 -1.1865 12 1.485 7
-0.93173 -0.68323 -1.1878 10 1.485 8
-0.92185 -0.68054 -1.1831 10 1.475 10

加载程序：

将下面的代码复制到新建的pcl_read.cpp文件中，pcl_read.cpp放在chapter6_tutorials/src文件夹下，目前该目录下有两个cpp文件，另一个是pcl_create.cpp。

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

int main(int argc, char **argv)
{
	ros::init(argc, argv, "pcl_read");

	ros::NodeHandle nh;
	ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output", 1);

	pcl::PointCloud<pcl::PointXYZ> cloud;
	sensor_msgs::PointCloud2 output;

	pcl::io::loadPCDFile("test_pcd.pcd",cloud);

	pcl::toROSMsg(cloud,output);// pcl格式转为ROS格式
	output.header.frame_id = "odom";
	ros::Rate loop_rate(1);

	while(ros::ok()){
		pcl_pub.publish(output);
		ros::spinOnce();
		loop_rate.sleep();
	}

	return 0;
}

我们在chapter6_tutorials目录下新建一个data文件夹，用来存放pcd文件，我找了一个之前跑算法保存好的pcd文件放在其中。

和上面创建点云一样，我们在/pclstudy_ws/src/chapter6_tutorials内的CMakeList.txt添加如下内容：

add_executable(pcl_read src/pcl_read.cpp)
target_link_libraries(pcl_read ${catkin_LIBRARIES} ${PCL_LIBRARIES})

编译：

cd ~/pclstudy_ws | catkin_make 

刷新环境：

source devel/setup.bash

运行和在RVIZ中显示：

#分三个终端输入，第二行和第三行在一个终端里
roscore
roscd chapter6_tutorials/data
rosrun chapter6_tutorials pcl_read
rosrun rviz rviz

在rviz中增加PointCloud2、topic选择pcl_output、fixed Frame输入odom，结果如图：

2.3、创建点云文件

 通过下面的程序，我们可以将接收到的点云储存为文件，它订阅了一个sensor_msgs/PointCloud2主题。此文件命名为pcl_write.cpp，放在和pcl_read.cpp相同的目录下。

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input){
	pcl::PointCloud<pcl::PointXYZ> cloud;
	pcl::fromROSMsg(input,cloud);
	pcl::io::savePCDFileASCII("write_pcd_test.pcd",cloud);
}

int main(int argc, char **argv)
{
	ros::init(argc,argv,"pcl_write");
	ros::NodeHandle nh;
	ros::Subscriber bat_sub = nh.subscribe("pcl_output",10,cloudCB);

	ros::spin();

	return 0;
}

和上面创建点云一样，我们在/pclstudy_ws/src/chapter6_tutorials内的CMakeList.txt添加如下内容：

add_executable(pcl_write src/pcl_write.cpp)
target_link_libraries(pcl_write ${catkin_LIBRARIES} ${PCL_LIBRARIES})

 编译：

cd ~/pclstudy_ws | catkin_make 

刷新环境：

source devel/setup.bash

运行和在RVIZ中显示：

#分三个终端输入,第二行和第三行在一个终端输入，
第四行在哪个目录下，pcd文件就会保存在哪个目录下
roscore
roscd chapter6_tutorials/data
rosrun chapter6_tutorials pcl_read
rosrun chapter6_tutorials pcl_write

这样我们可以看到在上述第四行命令所在文件夹下多了一个write_pcd_test.pcd文件。

2.4、点云可视化

PCL提供了几种方法将点云可视化，第一种也是最简单的一种是通过点云查看器，例如Cloudcompare和pcl_viewer，可以直接打开pcd文件。第二种是写一个像上面所写的pcl_read.cpp程序，通过运行这个程序使pcd文件在rviz中显示。第三种是创建一个节点订阅sensor_msgs/PointCloud2，这个节点会使用PCL中的cloud_reviewer显示主题中的点云数据。

在src目录下创建pcl_visualize.cpp：

#include <ros/ros.h>
#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
        pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this);
        viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }
    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(input, cloud);
        viewer.showCloud(cloud.makeShared());
    }
    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }
protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_visualize");
    cloudHandler handler;
    ros::spin();
    return 0;
}

跟之前的一样在CMakeLists.txt文件中加入：

add_executable(pcl_visualize src/pcl_visualize.cpp)
target_link_libraries(pcl_visualize ${catkin_LIBRARIES}  ${PCL_LIBRARIES})

编译、source完成后按以下步骤运行：

roscore
roscd chapter6_tutorials/data
rosrun chapter6_tutorials pcl_read
rosrun chapter6_tutorials pcl_visualize

 

2.5、点云滤波和下采样

2.5.1、点云滤波

当处理点云时，可能会遇到两个问题，过大的噪声和过大的密度。此时我们需要进行滤波，过滤掉无用的噪声和降低点云密度。

在src目录下创建pcl_filtered.cpp文件：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/statistical_outlier_removal.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_filtered", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2& input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);

        pcl::toROSMsg(cloud_filtered, output);
        pcl_pub.publish(output);
    }

protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};

main(int argc, char** argv)
{
    ros::init(argc, argv, "pcl_filter");

    cloudHandler handler;

    ros::spin();

    return 0;
}

跟之前的一样在CMakeLists.txt文件中加入：

add_executable(pcl_filtered src/pcl_filtered.cpp)
target_link_libraries(pcl_filtered ${catkin_LIBRARIES}  ${PCL_LIBRARIES})

编译、source完成后按以下步骤运行：

roscore
roscd chapter6_tutorials/data
rosrun chapter6_tutorials pcl_read
rosrun chapter6_tutorials pcl_filtered
rosrun rviz rviz

 2.5.2、点云下采样

在src目录下创建pcl_downsampled.cpp文件：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_filtered", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_downsampled", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud);

        pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
        voxelSampler.setInputCloud(cloud.makeShared());
        voxelSampler.setLeafSize(0.0001f, 0.0001f, 0.0001f);
        voxelSampler.filter(cloud_downsampled);

        pcl::toROSMsg(cloud_downsampled, output);
        pcl_pub.publish(output);

    }
protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};

main(int argc, char **argv)
{
    ros::init(argc, argv, "pcl_downsampling");

    cloudHandler handler;

    ros::spin();

    return 0;
}

跟之前的一样在CMakeLists.txt文件中加入：

add_executable(pcl_downsampled src/pcl_downsampled.cpp)
target_link_libraries(pcl_downsampled ${catkin_LIBRARIES}  ${PCL_LIBRARIES})

编译、source完成后按以下步骤运行：

roscore
roscd chapter6_tutorials/data
rosrun chapter6_tutorials pcl_read
rosrun chapter6_tutorials pcl_filtered
rosrun chapter6_tutorials pcl_downsampled
rosrun rviz rviz

2.6、点云配准与匹配

配准和匹配时一种在很多场景中经常使用的技术，这个应用场景中包括在两个数据集中寻找共同的结构或特征，然后利用他们将数据集拼接在一起。当一个高速移动的源中获取一个点云时这个技术很有用，并且我们会得到对源运动的一个估计。有了这个算法，我们可以将这些云集拼接在一起并降低估计传感器时的不确定性。PCL提供了一个成为迭代最近点（Iterative Closest Point）算法执行配准和匹配。

在src目录下创建pcl_matching.cpp文件：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_downsampled", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_matched", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud_in;
        pcl::PointCloud<pcl::PointXYZ> cloud_out;
        pcl::PointCloud<pcl::PointXYZ> cloud_aligned;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud_in);

        cloud_out = cloud_in;

        for (size_t i = 0; i < cloud_in.points.size (); ++i)
        {
            cloud_out.points[i].x = cloud_in.points[i].x + 0.7f;
        }

        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setInputSource(cloud_in.makeShared());
        icp.setInputTarget(cloud_out.makeShared());

        icp.setMaxCorrespondenceDistance(5);
        icp.setMaximumIterations(100);
        icp.setTransformationEpsilon (1e-12);
        icp.setEuclideanFitnessEpsilon(0.1);

        icp.align(cloud_aligned);

        pcl::toROSMsg(cloud_aligned, output);
        pcl_pub.publish(output);
    }

protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};

main(int argc, char **argv)
{
    ros::init(argc, argv, "pcl_matching");

    cloudHandler handler;

    ros::spin();

    return 0;
}