ROS点云话题sensor_msgs::PointCloud2转pcl::PointCloud的两种方式

在ROS中订阅点云话题的时候，需要先将数据类型转换成PCL格式之后再做操作。

方式一：

直接调用pcl自带的函数
pcl::fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
需要添加头文件#include <pcl_conversions/pcl_conversions.h>
使用举例：

void callback(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr)
{
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr colorcloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*in_cloud_ptr, *colorcloud);
｝

这个方式使用比较简单，一般都使用方式一。

方式二：

目前我使用realsense相机遇到一个问题，订阅得到的初始点云话题数据量过大，有307200个点云，使用pcl::fromROSMsg转成pcl点云，再进行降采样，这两步会用去大量的时间，在对程序运算速度有要求的时候并不好用。

因此采用mencpy的方式，直接从地址提取部分点云，可以有效节省时间。
memcpy函数用法：
void *memcpy(void *dest, const void *src, size_t n);

第一个参数是接收数据的参数，第二个参数是数据的地址，第三个参数是数据的大小。关键就在于第二和第三个参数，先看ros话题的消息格式：

再查看几个重要的参数：

  sensor_msgs::PointCloud2 output_pc;
  pcl::toROSMsg(cloud,output_pc); //我是从pcd文件读取的点云转成ros话题，如果是订阅的点云话题，则不需要这一步，直接cout即可
  std::cout<<"width: "<<(int)output_pc.width<<std::endl;
  std::cout<<"height: "<<(int)output_pc.height<<std::endl;
  std::cout<<"point_step: "<<(int)output_pc.point_step<<std::endl;
  std::cout<<"row_step: "<<(int)output_pc.row_step<<std::endl;
  std::cout<<"fields_size: "<<(int)output_pc.fields.size()<<std::endl;

以我这个为例，我是从深度图像转换成的点云，所有是640*480，point_step代表每个点存储了32个字节，
row_step就是width×point_step的大小，
fields_size表示每个点中有几个数据。
再查看fields里面的具体内容：

  std::cout<<"fields0: "<<(int)output_pc.fields[0].datatype<<" "<<output_pc.fields[0].name<<std::endl;
  std::cout<<"fields1: "<<(int)output_pc.fields[1].datatype<<" "<<output_pc.fields[1].name<<std::endl;
  std::cout<<"fields2: "<<(int)output_pc.fields[2].datatype<<" "<<output_pc.fields[2].name<<std::endl;
  std::cout<<"fields3: "<<(int)output_pc.fields[3].datatype<<" "<<output_pc.fields[3].name<<std::endl;

datatype是7，对应的是float32。也就是说每个点都有x,y,z,rgba四个数据，数据类型都是float32，占四个字节。这四个数据的存储方式如下图所示：

上面提到point_step是32，也就是4字节✖8，x,y,z占用前面12个字节，再空出4个字节后，存储rgba,再空出12个字节。
所有点云的数据是保存在 uint8[] data中的，我们只要每隔32个字节，把其中对应的数据取出，就完成了ROS话题转换pcl话题：

void callback(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr)
{
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclcloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
	for(int i=0;i<in_cloud_ptr->width*in_cloud_ptr->height;i++)
    {
        pcl::PointXYZRGBA p;
        std::memcpy(&p.x,&in_cloud_ptr->data[32*i],4);
        std::memcpy(&p.y,&in_cloud_ptr->data[32*i+4],4);
        std::memcpy(&p.z,&in_cloud_ptr->data[32*i+8],4);
        std::memcpy(&p.rgba,&in_cloud_ptr->data[32*i+16],4);
        pclcloud->points.push_back(p);
    }
}

这样就实现了从地址中读取ros点云数据。
为了省去之后的降采样的工作，可以只读取部分点云的数据：

void callback(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr)
{
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclcloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
	for(int i=0;i<in_cloud_ptr->width*in_cloud_ptr->height;i+=4)//每一行中，每隔四个点读一个点的数据
    {
    	if((i/in_cloud_ptr->width)%4!=0)  //每隔四行读一行数据，参数可以自己调节
        {
            continue;
        }
        pcl::PointXYZRGBA p;
        std::memcpy(&p.x,&in_cloud_ptr->data[32*i],4);
        std::memcpy(&p.y,&in_cloud_ptr->data[32*i+4],4);
        std::memcpy(&p.z,&in_cloud_ptr->data[32*i+8],4);
        std::memcpy(&p.rgba,&in_cloud_ptr->data[32*i+16],4);
        pclcloud->points.push_back(p);
    }
}

这样就减少了原始的数据量，加快了ROS话题转换成PCL格式的速度。