Pcl:: fromrosmsg alarm failed to find match for field 'intensity'

1 Problem description

The original point cloud PointCloud2 Types of topics contain intensity data , And in Rviz The intensity information can be displayed in . Use pcl::fromROSMsg() Function will ROS Of PointCloud2 Messages of type are converted to pcl::PointXYZI After data of type , There is Failed to find match for field ‘intensity’ The problem of ¹.

2 Cause analysis

1. Emitted topic The data structure of point cloud is different from that of the received point cloud ;
   such as , Emitted topic Point cloud type is PointXYZI（ contain x, y, z, intensity）, What you receive is customized PointXYZIRT（ contain x, y, z, intensity, ring, timestamp）

Issued point cloud data structure

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

Received point cloud data structure

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

2. The data types in the point cloud structure are different ;
   In fact, if only the data structure is different, it may not lead to this problem , If intensity The data type is consistent ,pcl::fromROSMsg Function can still convert the previous consistent part , At least x, y, z, i It can be received normally . But in the example above PointXYZI Of intensity yes float, Self defined PointXYZIRTintensity nevertheless uint8_t, such pcl::fromROSMsg Function will report an error 【 reference 】.

3 Solution

3.1 get topic data structure

First of all, you need to know the data type of each member in the issued data structure , If you know in advance , This part can be skipped . But in many cases, you may not know the data structure when recording . Here are two ways to get the data structure ：

3.1.1 Use rviz get

As we mentioned earlier ,rviz It is the lidar reflectivity that can normally display point clouds intensity Of , That is to say rviz This type of point cloud can be accepted normally , have access to rviz adopt “by topic” Get the selected topic Point cloud , And then from Channel Name Click the drop-down list on the right , You can see all members . But this method cannot get the actual sorting , But the general sorting is XYZIRT.

3.1.2 sensor_msgs::PointCloud2 Data structure acquisition

manual Should be msg The data structure is as follows , among sensor_msgs/PointField[] fields Contains the data part of the point cloud .

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

Here is sensor_msgs/PointField.msg Of Source file , among name Save the member name of the point cloud structure （ Such as ：x, y, z, intensity etc. ……）,offset Indicates correspondence name The data of is stored in the storage location of this group of data ,datatype Store this name Of offset What is the data type , however datatype It's a uint8 Enumeration , The values defined above for each data type store the corresponding types . That is, we will correspond to name Of datatype You can know its type by printing it .

# 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

Just refer to the callback of the cloud below to print datatype The enumerated values . If printed as 2, Is due to uint8 UINT8 = 2 The definition of ,intensity Should be uint8_t type .

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 Manually convert point cloud format

Customize a fromROSMsg_DIY The function of is used to convert point clouds

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

The callback function can be written like this

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

You can also refer to a complete routine 【 Quote from here 】, But note that there is no point cloud in this routine hight and width assignment , Doing something else （ Such as ：pcl::io::savePCDFileASCII ()） There may be problems when .

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

Its CMakeLists.txt That's how it's written

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 ︎