前言

使用ROS处理点云数据时，经常需要进行数据格式转换，这里做个记录。

数据格式

首先要了解点云常见的两种格式sensor_msgs/PointCloud2和pcl::PointT。

sensor_msgs/PointCloud2是传感器中用的，像订阅激光点云或者相机点云topic返回的格式。格式如下

header:            // 点云的头信息
  seq: 963         // 次数
  stamp:           // 时间戳
    secs: 1541143772
    nsecs: 912011000
  frame_id: "/camera_init"
height: 1          // 如果cloud 是无序的 height 是 1
width: 852578      // 点云的长度
fields:            // sensor_msgs/PointField[] fields
  -
    name: "x"
    offset: 0
    datatype: 7          // uint8 INT8    = 1
                         // uint8 UINT8   = 2
                         // uint8 INT16   = 3
                         // uint8 UINT16  = 4
                         // uint8 INT32   = 5
                         // uint8 UINT32  = 6
                         // uint8 FLOAT32 = 7
                         // uint8 FLOAT64 = 8
    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
is_bigendian: False
point_step: 32      //  一个点占的比特数
row_step: 27282496  // 一行的长度占用的比特数，就是width*point_step的大小
data: [ ................... ] // Actual point data, size is (row_step*height)
is_dense: True      // 没有非法数据点

pcl::PointT是一类格式，包括PointXYZ、PointXYZI、PointXYZRGBA、PointXYZRGB、PointXY等。一般处理点云就要将PointCloud2转为PCL格式，再进行操作。PointXYZI格式如下

union
{
    float data[4];
    struct
    {
       float x;
       float y;
       float z;
    };
};
union
{
    struct
    {
       float intensity;
     };
    float data_c[4];
};

PointCloud2转PointXYZI

有两种方法，一种是直接调用pcl自带的函数，一种是采用mencpy的方式，直接从地址提取部分点云。

方法一

直接调用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& cloud) {
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cloud(
      new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*cloud, *pcl_cloud);
}

我们可以看下pcl::fromROSMsg做了啥

namespace pcl {

/** Provide to/fromROSMsg for sensor_msgs::PointCloud2 <=> pcl::PointCloud<T>
 * **/
// pcl::PointCloud 与 sensor_msgs::PointCloud2 间的转换

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

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);
}

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

}  // namespace pcl

根据这个，其实我们也可以直接用pcl::fromPCLPointCloud2来实现

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

方法二

当订阅得到的初始点云话题数据量过大时，使用pcl::fromROSMsg转成pcl点云再做处理，会用去大量的时间。
因此采用mencpy的方式，直接从地址提取部分点云，可以有效节省时间。
mencpy格式为void *memcpy(void *dest, const void *src, size_t n);第一个参数是接收数据的参数，第二个参数是数据的地址，第三个参数是数据的大小。
根据上面sensor_msgs/PointCloud2的格式，datatype是7，所以对应的是FLOAT32。也就是说每个点都有x,y,z,intensity四个数据，数据类型都是float32，占四个字节。
另外，point_step是32，即4x8。所以x,y,z占用前面12个字节，再空出4个字节后，存储intensity，再空出12个字节
这四个数据的存储方式如下所示：

x	y	z		intensity

所有点云的数据是保存在uint8[] data中的，所以我们只要每隔32个字节，把其中对应的数据取出，就完成了ROS话题转换pcl话题：

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

PointXYZI转PointCloud2

和上面类似，直接调用pcl自带的函数函数pcl::toROSMsg(const pcl::PointCloud<T>& pcl_cloud, sensor_msgs::PointCloud2& cloud)。
使用举例：

void callback(const const pcl::PointCloud<pcl::PointXYZI>::ConstPtr pcl_cloud) {
  sensor_msgs::PointCloud2::Ptr cloud(new sensor_msgs::PointCloud2);
  pcl::toROSMsg(*pcl_cloud, *cloud);
}

同样，也可以直接调用pcl::toPCLPointCloud2来实现

void fromPCL(const pcl::PointCloud<pcl::PointXYZI>& pcl_cloud,
             sensor_msgs::PointCloud2& cloud) {
  pcl::PCLPointCloud2 pcl_pc2;
  pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
  pcl_conversions::moveFromPCL(pcl_pc2, cloud);
}