0. 前言

在深入剖析了Ceres、Eigen、Sophus、G2O后，以V-SLAM为代表的计算方式基本已经全部讲完，但是就L-SLAM而言我们还差一块，那就是PCL、GTSAM点云计算部分我们还没有详细的去写，正好就这个时间，我想把这块坑给填完，来形成一整个系列，方便自己回顾以及后面的人一起学习。

1. PCL系统认知

PCL（Point Cloud Library） 是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。支持多种操作系统平台，可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。如果说OpenCV是2D信息获取与处理的结晶，那么PCL就在3D信息获取与处理上具有同等地位，PCL是BSD授权方式，可以免费进行商业和学术应用。

如图PCL架构图所示，对于3D点云处理来说，PCL完全是一个的模块化的现代C++模板库。其基于以下第三方库：Boost、Eigen、FLANN、VTK、CUDA、OpenNI、Qhull，实现点云相关的获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。

而作为PCL的输入数据格式，一般我们默认使用.pcd的格式输出
pcd文件数据举例

VERSION .7                # 版本号
FIELDS x y z rgb        #  指定一个点可以有的每一个维度和字段的名字
SIZE 4 4 4 4                # 用字节数指定每一个维度的大小。例如：
TYPE F FFF                  # 用一个字符指定每一个维度的类型 int uint float
COUNT 1 1 1 1           # 指定每一个维度包含的元素数目
WIDTH 640                  # 像图像一样的有序结构，有640行和480列，
HEIGHT 480                # 这样该数据集中共有640*480=307200个点
VIEWPOINT 0 0 0 1 0 0 0     # 指定数据集中点云的获取视点 视点信息被指定为平移（txtytz）+四元数（qwqxqyqz）
POINTS 307200           # 指定点云中点的总数。从0.7版本开始，该字段就有点多余了
DATA ascii                    # 指定存储点云数据的数据类型。支持两种数据类型：ascii和二进制
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06

文件头格式

每一个PCD文件包含一个文件头，它确定和声明文件中存储的点云数据的某种特性。PCD文件头必须用ASCII码来编码。PCD文件中指定的每一个文件头字段以及ascii点数据都用一个新行（\n）分开了，从0.7版本开始，PCD文件头包含下面的字段：

• VERSION –指定PCD文件版本

• FIELDS –指定一个点可以有的每一个维度和字段的名字。例如：
  FIELDS x y z # XYZ data
  FIELDS x y z rgb # XYZ + colors
  FIELDS x y z normal_xnormal_y normal_z # XYZ + surface normals
  FIELDS j1 j2 j3 # moment invariants
  …

• SIZE –用字节数指定每一个维度的大小。例如：
  unsigned char/char has 1 byte
  unsigned short/short has 2 bytes
  unsignedint/int/float has 4 bytes
  double has 8 bytes

• TYPE –用一个字符指定每一个维度的类型。现在被接受的类型有：
  I – 表示有符号类型int8（char）、int16（short）和int32（int）；
  U – 表示无符号类型uint8（unsigned char）、uint16（unsigned short）和uint32（unsigned int）；
  F – 表示浮点类型。

• COUNT –指定每一个维度包含的元素数目。例如，x这个数据通常有一个元素，但是像VFH这样的特征描述子就有308个。实际上这是在给每一点引入n维直方图描述符的方法，把它们当做单个的连续存储块。默认情况下，如果没有COUNT，所有维度的数目被设置成1。

• WIDTH –用点的数量表示点云数据集的宽度。根据是有序点云还是无序点云，WIDTH有两层解释：
  1)它能确定无序数据集的点云中点的个数（和下面的POINTS一样）；
  2)它能确定有序点云数据集的宽度（一行中点的数目）。

  注意：有序点云数据集，意味着点云是类似于图像（或者矩阵）的结构，数据分为行和列。这种点云的实例包括立体摄像机和时间飞行摄像机生成的数据。有序数据集的优势在于，预先了解相邻点（和像素点类似）的关系，邻域操作更加高效，这样就加速了计算并降低了PCL中某些算法的成本。
  例如：
  WIDTH 640 # 每行有640个点

• HEIGHT –用点的数目表示点云数据集的高度。类似于WIDTH ，HEIGHT也有两层解释：
  1)它表示有序点云数据集的高度（行的总数）；
  2)对于无序数据集它被设置成1（被用来检查一个数据集是有序还是无序）。

  有序点云例子： WIDTH 640 # 像图像一样的有序结构，有640行和480列， HEIGHT 480 #
  这样该数据集中共有640*480=307200个点 无序点云例子： WIDTH 307200 HEIGHT 1 #
  有307200个点的无序点云数据集

• VIEWPOINT–指定数据集中点云的获取视点。VIEWPOINT有可能在不同坐标系之间转换的时候应用，在辅助获取其他特征时也比较有用，例如曲面法线，在判断方向一致性时，需要知道视点的方位，
  视点信息被指定为平移（txtytz）+四元数（qwqxqyqz）。默认值是：
  VIEWPOINT 0 0 0 1 0 0 0

• POINTS–指定点云中点的总数。从0.7版本开始，该字段就有点多余了，因此有可能在将来的版本中将它移除。
  例子：
  POINTS 307200 #点云中点的总数为307200

• DATA –指定存储点云数据的数据类型。从0.7版本开始，支持两种数据类型：ascii和二进制。查看下一节可以获得更多细节。
  注意：文件头最后一行（DATA）的下一个字节就被看成是点云的数据部分了，它会被解释为点云数据。

2. 消息转换

ros 消息转换为 pcl 点云

这部分是在激光SLAM中常用的输入处理方式，我们选取了Lego-LOAM中对velodyne的lidar做了区分处理的代码部分，以16线雷达为例，激光雷达通过16根雷达光束进行旋转扫描，从而得到一周360°的点云。velodyne的lidar对点云属于16线中的哪一线做了标记，这个标记被称为Ring index，其中的Ring表示环的意思。如果不是velodyne的lidar后面会通过计算得出点云属于哪个线束。因此保存点云的时候分别保存为"laserCloudIn"和"laserCloudInRing"中。

 void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
     // 1. 读取ROS点云转换为PCL点云
     cloudHeader = laserCloudMsg->header;
     // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
     pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
     // 2. 去除点云中的Nan points
     std::vector<int> indices;
     pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
     // 3. 如果点云有"ring"通过，则保存为laserCloudInRing
     if (useCloudRing == true){
         pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
         if (laserCloudInRing->is_dense == false) {
             ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
             ros::shutdown();
         }  
     }
 }

下面是核心代码sensor_msgs::PointCloud2转到 pcl::PointCloud&ltpcl::PointXYZ>，从ros到pcl类型的转换，用到fromROSMsg方法。

 //输入的ros pointcloud2 类型
 sensor_msgs::PointCloud2 t_cloud;
 //输出的pcl 类型
 pcl::PointCloud<pcl::PointXYZ> t_pclCloud;
 pcl::fromROSMsg(t_cloud,t_pclCloud);

…详情请参照古月居