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

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

方式一:

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

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话题的消息格式:
ROS点云话题sensor_msgs::PointCloud2转pcl::PointCloud的两种方式_第1张图片
再查看几个重要的参数:

  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;

ROS点云话题sensor_msgs::PointCloud2转pcl::PointCloud的两种方式_第2张图片
以我这个为例,我是从深度图像转换成的点云,所有是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;

ROS点云话题sensor_msgs::PointCloud2转pcl::PointCloud的两种方式_第3张图片
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格式的速度。

你可能感兴趣的:(计算机视觉,人工智能,c++)