pcl::fromROSMsg报警告Failed to find match for field ‘intensity‘.
pcl::fromROSMsg报警告Failed to find match for field 'intensity'
- 1 问题描述
- 2 原因分析
- 3 解决方案
-
- 3.1 获得topic数据结构
-
- 3.1.1 使用rviz获得
- 3.1.2 sensor_msgs::PointCloud2数据结构获得
- 3.2 手动转换点云格式
1 问题描述
原始点云PointCloud2类型的话题中含有intensity数据，并且在Rviz中可以将强度信息显示出来。使用pcl::fromROSMsg()函数将ROS的PointCloud2类型的消息转成pcl::PointXYZI类型的数据后，出现了Failed to find match for field ‘intensity’ 的问题1。
2 原因分析
- 发出的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;
- 点云结构中数据类型不同；
其实如果只是数据结构不同还不一定能导致该问题出现，如果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 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})
https://blog.csdn.net/yuteng12138/article/details/124794822 ↩︎