ROS下自定义PointCloud类型

如果您只想了解如何自定义PointCloud类型，请跳过前半部分分析内容。

Velodyne VLP16线激光雷达的输出数据其实很丰富，但是由于PCL预先定义的PointCloud数据类型的成员变量有限，因此部分数据并未得到使用，而是被直接遗弃。

如果想要使用这部分数据，就应当自行定义PonitCloudT。

典型的Velodyne输出数据如下，可见其中ring的数据并未被使用。本文以此为例，说明如何使用自定义PointCloudT。

header: 
  seq: 4301
  stamp: 
    secs: 1468570227
    nsecs: 890882000
  frame_id: velodyne
height: 1
width: 20264
fields: 
  - 
    name: x
    offset: 0
    datatype: 7
    count: 1
  - 
    name: y
    offset: 4
    datatype: 7
    count: 1
  - 
    name: z
    offset: 8
    datatype: 7
    count: 1
  - 
    name: intensity
    offset: 16
    datatype: 7
    count: 1
  - 
    name: ring
    offset: 20
    datatype: 4
    count: 1
is_bigendian: False
point_step: 32
row_step: 648448
data: [119, 77, 185, ...]

点云数据流如下

sensor_msgs/PointCloud2中定义如下，可见x,y,z等数据并非在此被提取，而是整个fields被复制。

Header header

uint32 height
uint32 width

PointField[] fields

bool    is_bigendian 
uint32  point_step   
uint32  row_step     
uint8[] data         

bool is_dense       

查看pcl::fromROSMsg函数可以发现，此函数将fields中的数据逐一填入pcl::PointCloud的成员变量中。因此，想要得到ring数据，需要使用成员变量含有ring的pcl::PointCloud < T >，因此需要通过自定义PointCloud数据类型实现。

  template<typename T>
  void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
  {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
  }

至此，思路已经清晰了，以下为如何自定义PointCloud类型。

定义自定义点云的示例代码如下：

/*******point_xyzo.h********/
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
struct MyPointType                 //定义点类型结构
{
    PCL_ADD_POINT4D;               //该点类型有4个元素
    float test;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW//确保new操作符对齐操作
}EIGEN_ALIGN16;                    //强制SSE对齐
POINT_CLOUD_REGISTER_POINT_STRUCT(MyPointType,//注册点类型宏
    (float,x,x)
    (float,y,y)
    (float,z,z)
    (float,test,test)
)
int main(int argc,char** argv)
{
    pcl::PointCloud<MyPointType> cloud;
    cloud.points.resize(2);
    cloud.width=2;
    cloud.height=1;
    cloud.points[0].test=1;
    cloud.points[1].test=2;
             cloud.points[0].x=cloud.points[0].y=cloud.points[0].z=0;
         cloud.points[1].x=cloud.points[1].y=cloud.points[1].z=3;
    pcl::io::savePCDFile("test.pcd",cloud);
}

其他注意事项：

• 要用到哪个类中的函数，不仅需要包含该类的头文件，还需要包含该类impl文件夹下对应的hpp文件，例如

// 用到passthrough函数
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>

• 使用时包含自定义头文件

#include <pcl_ros/point_cloud.h>
#include "point_xyzo.h"