一、问题描述
由于大多数开源SLAM算法中都基于ROS开发，各传感器采集的数据通常以ROS的消息类型（sensor_msgs）进行发布和订阅。就激光雷达（LiDAR）而言，采集的原始点云数据通常以sensor_msgs::PointCloud2的数据类型进行发布，在算法中对点云进行处理时，调用点云开源算法库（PCL）中的功能可以便捷的实现相应功能。PCL库内部也定义了自己的点云数据结构。因此，在处理前，首先需要将点云由ROS的数据类型转换为PCL的数据类型。
ROS中的点云数据类型
-
sensor_msgs::PointCloud：该类型属于较早的版本，以逐渐弃用。
-
sensor_msgs::PointCloud2：目前常用的点云数据类型
PCL 中的点云数据类型
-
二、ROS中点云数据结构：
官方文档说明：ROS sensor_msgs/PointCloud2
std_msgs/Header header：数据头，包含该帧点云的时间戳、坐标系等属性信息
uint32 height：data的高度，一帧点云通常height=1，即表示无序点云；
uint32 width：data的宽度，即每行对应点的数量；
sensor_msgs/PointField[] fields：包含每个点的字段属性信息，详见下文。
bool is_bigendian：点云是否按正序排列
uint32 point_step：每个点占用的比特数，1字节=8比特，与PointField里所有字节数之和相等。
uint32 row_step：每行占用的比特数，=点的数量*Point_step;
uint8[] data：序列化后的点云二进制数据，所有点信息都在一串字符中，无法通过data[i]取值。
bool is_dense：是否存在无效点。最值得关注的变量是：fields。
官方文档：sensor_msgs/PointField[] fields
string name # 点的字段名
uint32 offset # 相对于结构体起始地址的字节数
uint8 datatype # 该字段所占用的字节数
uint32 count # 该字段的数量，通常为1datatype 不同值与类型的对应如下：
uint8 INT8 = 1 // 1字节
uint8 UINT8 = 2 // 1字节
uint8 INT16 = 3 // 2字节
uint8 UINT16 = 4 // 2字节
uint8 INT32 = 5 // 4字节
uint8 UINT32 = 6 // 4字节
uint8 FLOAT32 = 7 // 4字节
uint8 FLOAT64 = 8 // 8字节以某开源数据包为例，回放bag包，编写一个ROS节点订阅其LiDAR数据，代码可参考示例。
VLP-16发布点云的类型为PointCloud2。其fields如下图所示:
其中：
name值为每个点对应的字段名称，
datatype
fields[0]、fields[1]、fields[2]分别为每个点的x、y、z坐标字段，均为float类型，4个字节。
fields[3]为点云反射强度、float类型，4个字节。
fields[4]为每个点对应的时间戳，double类型，8个字节，64比特，
fields[5]为每个点对应的线ID，值为0~15，2个字节。
三、PCL中的点云数据结构：
PCL中的功能函数多以模板的形式实现，可以使用多种点云类型，
一个典型的PCL点云类型为：X、Y、Z + 其他属性（反射强度、时间、RGB等等）在将ROS点云转换为PCL点云时，我们期望转换前后，点云拥有相同的属性值，而不是具有反射强度的点云，格式转换后只剩XYZ坐标了。对于普通的点云，PCL定义好的点云数据结构即可满足需求。但是对于特殊点云，我们需要自定义点云结构体。
以LIO-SAM中的一段代码为例，定义自己的PCL点云类型的代码如下：
//定义每个点的数据结构 struct VelodynePointXYZIRT { PCL_ADD_POINT4D //XYZ，PCL宏 PCL_ADD_INTENSITY; //反射强度，PCL宏 uint16_t ring; //线ID float time; //时间 EIGEN_MAKE_ALIGNED_OPERATOR_NEW //PCL宏，确保new操作符对其操作，无须理解，用就完了 } EIGEN_ALIGN16; //注册点类型宏 POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (uint16_t, ring, ring) (float, time, time) )在自定义点云结构体PointT时，需根据传入的sensor_msgs::PointCloud2的fields来定义，
两点要求：1、自定义结构体的字段命名、数量、顺序需要与fields中的name一致；
2、自定义结构体字段占用的字节数需与datatype一致。比如datatype=7，那么要用float。
若PointT的字段与fields name不一致，编译不会报错，但是运行时，会警告“Failed to find match for field *** ”。
若PointT的字节数与fields datatype不一致，则会取出错误的值，类似指针。本来取4个字节，结果取了8个字节，得到的点云信息肯定是错的，但编译器不提示。
四、类型转换
ROS中已经集成了对PCL的API接口，PCL中也集成了用于转换的结构体PCLPointCloud2。因此我们可以直接调用函数。
ROS的PCL转换接口主要在pcl_conversions.h中，里边提供了丰富的类型转换函数。实际上，该函数是基于pcl的功能实现了类型转换，对应pcl的函数文件conversions.h
函数名中带move和不带move的区别是，移动(剪切)和拷贝的区别。可以自行转到定义查看他复制data时的操作。
本人使用的是moveFromROSMsg() 函数。首先由ROS的sensor_msgs::PointCloud2类型转换为PCL的pcl::PCLPointCloud2类型，然后再由PCL的pcl::PCLPointCloud2类型转换为PCL的pcl::PointCloud<T>。
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); }先来看一下第一个函数：pcl_conversions::moveToPCL()
inline void moveToPCL(sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { copyPointCloud2MetaData(pc2, pcl_pc2); pcl_pc2.data.swap(pc2.data); } inline void copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2) { toPCL(pc2.header, pcl_pc2.header); pcl_pc2.height = pc2.height; pcl_pc2.width = pc2.width; toPCL(pc2.fields, pcl_pc2.fields); pcl_pc2.is_bigendian = pc2.is_bigendian; pcl_pc2.point_step = pc2.point_step; pcl_pc2.row_step = pc2.row_step; pcl_pc2.is_dense = pc2.is_dense; } inline void toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf) { pcl_pf.name = pf.name; pcl_pf.offset = pf.offset; pcl_pf.datatype = pf.datatype; pcl_pf.count = pf.count; }由于两个PointCloud2的定义几乎一样，所以这个转换只是简单的copy，field也是copy。
然后重点是第二个函数pcl::fromPCLPointCloud2()，第一步根据源点云的field表和目标点云类型PointT创建一个字段索引map（翻译比较蹩脚，建议直接按英文理解），第二步是从data里按照数据格式逐个memcpy点信息。
fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud<PointT>& cloud) { MsgFieldMap field_map; createMapping<PointT> (msg.fields, field_map); //创建一个field索引表 fromPCLPointCloud2 (msg, cloud, field_map); //转换点云 } template<typename PointT> void createMapping (const std::vector<pcl::PCLPointField>& msg_fields, MsgFieldMap& field_map) { // Create initial 1-1 mapping between serialized data segments and struct fields detail::FieldMapper<PointT> mapper (msg_fields, field_map); for_each_type< typename traits::fieldList<PointT>::type > (mapper); // Coalesce adjacent fields into single memcpy's where possible if (field_map.size() > 1) { std::sort(field_map.begin(), field_map.end(), detail::fieldOrdering); MsgFieldMap::iterator i = field_map.begin(), j = i + 1; while (j != field_map.end()) { // This check is designed to permit padding between adjacent fields. /// @todo One could construct a pathological case where the struct has a /// field where the serialized data has padding if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset) { i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size); j = field_map.erase(j); } else { ++i; ++j; } } } } // Return true if the PCLPointField matches the expected name and data type. // Written as a struct to allow partially specializing on Tag. template<typename PointT, typename Tag> struct FieldMatches { bool operator() (const pcl::PCLPointField& field) { return (field.name == traits::name<PointT, Tag>::value && field.datatype == traits::datatype<PointT, Tag>::value && (field.count == traits::datatype<PointT, Tag>::size || field.count == 0 && traits::datatype<PointT, Tag>::size == 1 /* see bug #821 */)); } };这个函数的主要作用是，程序也不知道你输入的PointT和原始点云的类型匹配不匹配，比如你的点云是XYZI类型的，非要转成XYZIRGB的也不行啊，所以要检核一下，找出那些匹配的点属性。createMapping()函数的关键在第一行：
FieldMapper<PointT> (msg_fields) (field_map)中，使用FieldMatches()函数，看看fields与PointT是否匹配：
1、命名要匹配，比如field中叫time，PointT中的也要有叫“time”的字段才行
2、字节大小匹配，field中time是4个字节的，PointT中就要定义time是float32类型的。
这对于后边用指针拷贝每个点的每个属性时，计算地址时很重要，千万不能错。
最后得到的field_map就是哪些匹配成功的字段及它的相对地址(struct_offset和serialized_offset)
如果相邻的field都能成功匹配，就把这两个字段对应的字节大小合并，
比如三个相邻字段分别对应4字节+4字节+8字节，那我不必一个字段一个字段拷贝，直接拷贝16个字节就好了。
最后就是拷贝点云了：fromPCLPointCloud2 (msg, cloud, field_map)，代码如下，
关键代码，首先cloud_data指向第1个点的地址，然后根据对应每个点字节大小步长，以及field_map索引表，用memcpy逐个field、逐个点拷贝。
template <typename PointT> void fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud<PointT>& cloud, const MsgFieldMap& field_map) { // Copy info fields cloud.header = msg.header; cloud.width = msg.width; cloud.height = msg.height; cloud.is_dense = msg.is_dense == 1; // Copy point data uint32_t num_points = msg.width * msg.height; cloud.points.resize (num_points); uint8_t* cloud_data = reinterpret_cast<uint8_t*>(&cloud.points[0]); // Check if we can copy adjacent points in a single memcpy if (field_map.size() == 1 && field_map[0].serialized_offset == 0 && field_map[0].struct_offset == 0 && msg.point_step == sizeof(PointT)) { balabala //太长了删掉这段没用的 } else { // If not, memcpy each group of contiguous fields separately for (uint32_t row = 0; row < msg.height; ++row) { const uint8_t* row_data = &msg.data[row * msg.row_step]; for (uint32_t col = 0; col < msg.width; ++col) { const uint8_t* msg_data = row_data + col * msg.point_step; BOOST_FOREACH (const detail::FieldMapping& mapping, field_map) { memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); } cloud_data += sizeof (PointT); } } } }结语
代码比较多，希望耐心看完，如文中有所纰漏，欢迎指点和交流。
希望上述内容对您有帮助。
可供参考的其他官方文档： 在ROS中使用PCL
文章出处登录后可见！
