pcl::fromROSMsg报警告Failed to find match for field ‘intensity‘.

1 问题描述

原始点云PointCloud2类型的话题中含有intensity数据，并且在Rviz中可以将强度信息显示出来。使用pcl::fromROSMsg()函数将ROS的PointCloud2类型的消息转成pcl::PointXYZI类型的数据后，出现了Failed to find match for field ‘intensity’ 的问题¹。

2 原因分析

1. 发出的topic点云与接收的点云数据结构不同；
   比如，发出的topic点云类型是PointXYZI（包含x, y, z, intensity），而接收的却是自定义的PointXYZIRT（包含x, y, z, intensity, ring, timestamp）

发出的点云数据结构

struct PointXYZI
{
  PCL_ADD_POINT4D;
  float intensity;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

接收的点云数据结构

struct PointXYZIRT
{
  PCL_ADD_POINT4D;
  uint8_t intensity;
  uint16_t ring;
  double timestamp;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

2. 点云结构中数据类型不同；
   其实如果只是数据结构不同还不一定能导致该问题出现，如果intensity数据类型前后一致，pcl::fromROSMsg函数还是可以将前面一致的部分进行转换的，也就是至少x, y, z, i是可以正常被接收。但是上面的例子中PointXYZI的intensity是float，自定义的 PointXYZIRTintensity却是uint8_t，这样pcl::fromROSMsg函数就会报错【参考文献】。

3 解决方案

3.1 获得topic数据结构

首先要知道所发出的数据结构中各成员的数据类型，如果提前知道，这部分可以略过。但很多情况下你或许并不知道录制时的数据结构。这里写出两个方法获得数据结构：

3.1.1 使用rviz获得

前面我们提到，rviz是可以正常显示点云的激光雷达反射率intensity的，也就是rviz可以正常接受该类型点云，可以使用rviz通过“by topic”获得选中发出的topic点云，然后从Channel Name中点击右侧下拉列表，就可以看到所有成员。但是此方法无法获得实际上的排序，但是一般排序都是XYZIRT。

3.1.2 sensor_msgs::PointCloud2数据结构获得

手册中该msg的数据结构如下，其中sensor_msgs/PointField[] fields包含点云的数据部分。

std_msgs/Header header
uint32 height
uint32 width
sensor_msgs/PointField[] fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

下面是sensor_msgs/PointField.msg的源文件，其中name保存点云结构体的成员名称（如：x, y, z, intensity等……），offset表示对应name的数据在本组数据的存储位置，datatype存储该name的offset是什么数据类型，但是datatype是一个uint8的枚举，上边对各数据类型的定义的值存储对应类型。即我们将对应name的datatype打印出来就可以知道其类型。

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

只要参照下面写点云的回调就可以打印出datatype的枚举值。如打印为2，则由于uint8 UINT8 = 2的定义，intensity应为uint8_t类型。

void MapGenerationNode::lidarCallback(const sensor_msgs::PointCloud2::ConstPtr& lidar)
{
	for (int f=0; f<lidar->fields.size(); ++f)
	{
			if (lidar->fields[f].name == "intensity")
				std::cout << "intensity lidar->fields[f].datatype = " << (int)lidar->fields[f].datatype << std::endl;
	}

3.2 手动转换点云格式

自定义一个fromROSMsg_DIY的函数用于转化点云

void fromROSMsg_DIY(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, 
												pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
{
	// Get the field structure of this point cloud
	int pointBytes = cloud_msg->point_step;
	int offset_x;
	int offset_y;
	int offset_z;
	int offset_int;
	for (int f=0; f<cloud_msg->fields.size(); ++f)
	{
		if (cloud_msg->fields[f].name == "x")
			offset_x = cloud_msg->fields[f].offset;
		if (cloud_msg->fields[f].name == "y")
			offset_y = cloud_msg->fields[f].offset;
		if (cloud_msg->fields[f].name == "z")
			offset_z = cloud_msg->fields[f].offset;
		if (cloud_msg->fields[f].name == "intensity")
			offset_int = cloud_msg->fields[f].offset;
	}

	// populate point cloud object
	for (int p=0; p<cloud_msg->width*cloud_msg->height; ++p)
	{
		pcl::PointXYZI newPoint;

		newPoint.x = *(float*)(&cloud_msg->data[0] + (pointBytes*p) + offset_x);
		newPoint.y = *(float*)(&cloud_msg->data[0] + (pointBytes*p) + offset_y);
		newPoint.z = *(float*)(&cloud_msg->data[0] + (pointBytes*p) + offset_z);
		newPoint.intensity = *(unsigned char*)(&cloud_msg->data[0] + (pointBytes*p) + offset_int);

		cloud->points.push_back(newPoint);
	}
	cloud->height = cloud_msg->height;
	cloud->width = cloud_msg->width;
}

回调函数里面可以这么写

void MapNode::lidarCallback(const sensor_msgs::PointCloud2::ConstPtr& lidar)
{
	pcl::PointCloud<pcl::PointXYZI>::Ptr lidar_cloud3(new pcl::PointCloud<pcl::PointXYZI>);
	fromROSMsg_DIY(lidar, lidar_cloud3);
	//...
}

也可以参考完整的一个例程【引用自这里】，但是注意该例程中没有对点云的hight和width赋值，在进行其他操作（如：pcl::io::savePCDFileASCII ()）时可能会出问题。

#include <iostream>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/conversions.h> 
 
using namespace std;
 
/* ---[ */
int main (int argc, char** argv)
{
  if (argc < 4)
  {
    std::cerr << "Syntax is: " << argv[0] << " <file_in.pcd> <file_out.pcd> 0/1/2 (ascii/binary/binary_compressed) [precision (ASCII)]" << std::endl;
    return (-1);
  }
 
  pcl::PCLPointCloud2 cloud_msg;
  Eigen::Vector4f origin; Eigen::Quaternionf orientation;
  pcl::PointCloud<pcl::PointXYZI> cloud_xyzi;
  
 
  if (pcl::io::loadPCDFile (string (argv[1]), cloud_msg, origin, orientation) < 0)
  {
    std::cerr << "Unable to load " << argv[1] << std::endl;
    return (-1);
  }
  
  // pcl::fromPCLPointCloud2(cloud,*cloud_xyzi);
  int pointBytes = cloud_msg.point_step;
  int offset_x;
  int offset_y;
  int offset_z;
  int offset_int;
  for (int f=0; f<cloud_msg.fields.size(); ++f)
  {
    if (cloud_msg.fields[f].name == "x")
      offset_x = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "y")
      offset_y = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "z")
      offset_z = cloud_msg.fields[f].offset;
    if (cloud_msg.fields[f].name == "intensity")
      offset_int = cloud_msg.fields[f].offset;
  }
  std::cout << offset_x << offset_y << offset_z << offset_int << std::endl;
  // populate point cloud object
  for (int p=0; p<cloud_msg.width*cloud_msg.height; ++p)
  {
      pcl::PointXYZI newPoint;
 
      newPoint.x = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_x);
      newPoint.y = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_y);
      newPoint.z = *(float*)(&cloud_msg.data[0] + (pointBytes*p) + offset_z);
      newPoint.intensity = *(unsigned char*)(&cloud_msg.data[0] + (pointBytes*p) + offset_int);
 
      //std::cout << p << " [" << newPoint.x << "," << newPoint.y << "," << newPoint.z << "," << newPoint.intensity << "]" << std::endl;
      cloud_xyzi.points.push_back(newPoint);
  }
  cloud_xyzi.height = 1;
  cloud_xyzi.width = cloud_msg.width*cloud_msg.height
  int type = atoi (argv[3]);
 
  // std::cerr << "Loaded a point cloud with " << cloud_msg.width * cloud_msg.height << 
  //              " points (total size is " << cloud_msg.data.size () << 
  //              ") and the following channels: " << pcl::getFieldsList (cloud_msg) << std::endl;
 
  pcl::PCDWriter w;
  if (type == 0)
  {
    std::cerr << "Saving file " << argv[2] << " as ASCII." << std::endl;
    w.writeASCII (string (argv[2]), cloud_xyzi, (argc == 5) ? atoi (argv[4]) : 7);
  }
  else if (type == 1)
  {
    std::cerr << "Saving file " << argv[2] << " as binary." << std::endl;
    w.writeBinary (string (argv[2]), cloud_xyzi);
  }
  else if (type == 2)
  {
    std::cerr << "Saving file " << argv[2] << " as binary_compressed." << std::endl;
    w.writeBinaryCompressed (string (argv[2]), cloud_xyzi);
  }
}

其CMakeLists.txt这么写

cmake_minimum_required(VERSION 2.6)
project(pcl_test)
 
find_package(PCL 1.8 REQUIRED)
 
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
 
add_executable(save_pcd save_pcd2.cpp)
 
add_library(save_pcd_lib SHARED save_pcd.cpp)   
 
target_link_libraries (save_pcd ${PCL_LIBRARIES})

---

1. https://blog.csdn.net/yuteng12138/article/details/124794822 ↩︎