ROS与PCL中点云数据之间的转换

点云PCL免费知识星球,点云论文速读。

标题:ROS与PCL中点云数据之间的转换

作者:particle

欢迎各位加入免费知识星球,获取PDF文档,欢迎转发朋友圈,分享快乐。

应小伙伴们后台留言,想要了解ROS中如何使用PCL,本篇文章就将具体介绍一下。文章中如有错误,欢迎留言指出。也期待大家能够积极分享和讨论。

PCL是随着ROS的而出现的三维点云处理的库,很多做机器人的朋友一定不陌生,这里将首先介绍在PCL库中经常使用的两种点云之间的转换,这里将根据工程中的经验,从代码层面举例分析如何实现程序中定义的各种点云数据之间转换,并且介绍PCL在应用于ROS中应该如何转换数据结构。

pcl::PCLPointCloud2::Ptr 与 pcl::PointCloudpcl::PointXYZ之间的关系

pcl::PointXYZ 是数据结构,pcl::PointCloud 是一个构造函数,比如

代码语言:javascript
复制
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));
cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ()));

构造函数pcl::PointCloud 还包含了点云的其他属性,比如点云数据的width, height ,is_dense ,sensor_origin_ ,sensor_orientation_。

而pcl::PCLPointCloud2 是一个结构体,同样包含了点云的基本属性,在PCL中的定义为

代码语言:javascript
复制
struct PCLPointCloud2 {
  PCLPointCloud2 () : header (), height (0), width (0), fields (),
 is_bigendian (false), point_step (0), row_step (0),
  data (), is_dense (false) #if defined(BOOST_BIG_ENDIAN) 
  { 
  is_bigendian = true; #elif defined(BOOST_LITTLE_ENDIAN)
  is_bigendian = false; #else  #error "unable to determine system endianness" #endif
  }::pcl::PCLHeader header;
  pcl::uint32_t height;
  pcl::uint32_t width;  std::vector< ::pcl::PCLPointField> fields;
  pcl::uint8_t is_bigendian;
  pcl::uint32_t point_step;
  pcl::uint32_t row_step;
  std::vector<pcl::uint8_t> data;
  pcl::uint8_t is_dense; 
  public:
  typedef boost::shared_ptr< ::pcl::PCLPointCloud2> Ptr;
  typedef boost::shared_ptr< ::pcl::PCLPointCloud2 const> ConstPtr;
  }; // struct PCLPointCloud2

那么在这个结构体中加上Ptr

pcl::PCLPointCloud2::Ptr,就表示智能指针,

下面在程序中实现滤波的功能,并实例说明两者之间的变换

代码语言:javascript
复制
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), 
             cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr  cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PCDReader reader;
//在这里读取一个pcl::PCLPointCloud2::Ptr 定义的cloud_blob 
  reader.read ("table_scene_lms400.pcd", *cloud_blob);  
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  
  sor.setInputCloud (cloud_blob);             
  sor.setLeafSize (0.01f, 0.01f, 0.01f); 
//经过体素滤波后输出的点云格式仍然是pcl::PCLPointCloud2::Ptr
  sor.filter (*cloud_filtered_blob);  
  //重点:这里是为了将pcl::PCLPointCloud2::Ptr  转换到pcl::PointCloud<pcl::PointXYZ>::Ptr
  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);

pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);

这一句实现pcl::PCLPointCloud数据格式的点云转化为pcl::PointCloud格式

智能指针Ptr类型点云数据和非Ptr类型相互转换

代码语言:javascript
复制
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud=*cloud_Ptr;
cloud_Ptr=cloud.makeShared;

比如下面的程序如果我们定义了非智能指针形式的点云表示 cloud,实现一个分割的代码如下,此时我们需要注意在setInputCloud 中需要注意为cloud.makeShared() 将点云表示为指针的形式,因为该函数输入要求是智能指针的点云。

代码语言:javascript
复制
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::SACSegmentation<pcl::PointXYZ>seg;
****
seg.setInputCloud(cloud.makeShared());

总结一下PCL中提供的点云数据格式之间的转换

(1)

代码语言:javascript
复制
void pcl::fromPCLPointCloud(const pcl:PCLPointCloud2 & msg
                          pcl::PointCloud<PointT>  & cloud
                          const MsgFieldMap & filed_map
                         )

(2)

代码语言:javascript
复制
void pcl::fromPCLPointCloud2(const pcl::PCLPointCloud & msg
                           pcl::PointCloud<pointT> &cloud
                           )

(3)

代码语言:javascript
复制
 void pcl::fromROSMsg(const pcl:PCLPointCloud2 & msg
                    pcl::PointCloud<PointT>  & cloud
                   const MsgFieldMap & filed_map
                      )

(4)

代码语言:javascript
复制
void pcl::fromROSMsg(const pcl:PCLPointCloud2 & msg
                     pcl::PointCloud<PointT>  & cloud
                     )

在ROS中又该如何实现ROS中定义的点云与PCL定义的点云数据转换呢?

首先我们举例在ROS中有以下的两中点云数据格式

sensor_msgs::PointCloud

sensor_msgs::PointCloud2

ROS与PCL中的pcl::PointCloud 点云数据格式转换(使用PCL库里的转换函数):

sensor_msgs::PointCloud2pcl::PointCloud之间的转换

使用pcl::fromROSMsg 和 pcl::toROSMsg

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

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

ROS与PCL中的pcl::PCLPointCloud2点云数据转换(使用ROS中的pcl_conversions函数进行转换):

sensor_msgs::PointCloud2ConstPtrpcl::PCLPointCloud2之间的转换使用

使用pcl_conversions::toPCL和pcl_conversions::fromPCL

void fromPCL(const <PCL Type> &, <ROS Message type> &);

void toPCL(const <ROS Message type> &, <PCL Type> &);

ROS中两种点云数据格式之间的转换:

sensor_msgs::PointCloudsensor_msgs::PointCloud2之间的转换

使用sensor_msgs::convertPointCloud2ToPointCloud 和 sensor_msgs::convertPointCloudToPointCloud2.

(这里为什么ROS有两种点云的数据格式呢,由于在ROS的迭代,刚开始定义的点云是sensor_msgs::PointCloud 仅仅包含了点云的XYZ以及的多个通道点云,而随着ROS的发展该形式已经不能满足需求,所以出现了 sensor_msgs::PointCloud2 不仅包含了 sensor_msgs::PointCloud2 中的多通道的点云数据,而且还增加了点云的其他属性,比如宽,高,是否稠密等)

这里举个例子比如我们要通过ROS发布一个点云数据的topic,我们该如何写程序呢?

以下功能是实现sensor_msgs::PointCloud2ConstPtr到 sensor_msgs::PointCloud2之间的转换

代码语言:javascript
复制
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h> 
#include <pcl/point_types.h> 
ros::Publisher pub;
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) {                  sensor_msgs::PointCloud2 output;     
output = *input;    
pub.publish (output); // Publish the data.  
}

int main (int argc, char** argv) {
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
ros::spin ();
}

以上案例是最为简单的为了实现采集点云通过ROS发布出去的实例,如果我们想在上面的程序中的回调函数cloud_cb中经过一个滤波处理中该如何进行数据转换呢?我们可以分析以下,需要经过以下的转换

sensor_msgs::PointCloud2ConstPtr -->pcl::PCLPointCloud2

(这里我们举例该类型为滤波函数的输入,当然也可以是其他PCL的点云形式)

-->sensor_msgs::PointCloud2

(这是最种需要发布出去的点云的数据形式,为什么要这种形式,因为这种形式在ROS中的RVIZ可视化的时候不会出现警告)

代码语言:javascript
复制
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>//滤波的头文件
#include <pcl/filters/voxel_grid.h>
ros::Publisher pub;
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(input, cloud);
// 进行一个滤波处理
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.1, 0.1, 0.1);
sor.filter (cloud_filtered);
sensor_msgs::PointCloud2 output; //声明的输出的点云的格式
pcl_conversions::fromPCL(cloud_filtered, output);
pub.publish (output);
}
int main (int argc, char
argv) {
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
ros::spin ();
}

解析:上面的函数使用ROS的转换函数,进行了两次点云数据的转换

代码语言:javascript
复制
  pcl_conversions::toPCL(*input, *cloud);
pcl_conversions::fromPCL(cloud_filtered, output);

以下是一个kinect点云数据在ROS中可视化

sensor_msgs::PointCloud2 与 pcl::PointCloud之间的转换,这里直接以一个回调函数实现平面分割为例,使用PCL提供的接口实现到ROS的转换:

代码语言:javascript
复制
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) {
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (*input, cloud);
pcl::ModelCoefficients coefficients;
pcl::PointIndices inliers;
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE); //平面模型
seg.setMethodType (pcl::SAC_RANSAC); //分割平面模型所使用的分割方法
seg.setDistanceThreshold (0.01); //设置最小的阀值距离
seg.setInputCloud (cloud.makeShared ()); //设置输入的点云
seg.segment (inliers, coefficients); //cloud.makeShared() 创建一个 boost shared_ptr

pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(coefficients, ros_coefficients);
pub.publish (ros_coefficients);
}

这里不再一一用程序举例,总结一下ROS中提供的pcl_conversions命名空间下点云转换关系

代码语言:javascript
复制
fromPCL (const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
toPCL (const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)

使用PCL中提供的函数实现到ROS的点云数据转换实例

代码语言:javascript
复制
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) {
sensor_msgs::PointCloud2 output;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
output = *input;
pcl::fromROSMsg(output,*cloud); pub.publish (output);
}

总结

无论是ROS还是PCL都相互提供了PCL到ROS,ROS到PCL的点云数据的转换。