沉默杀手HomeArchivesTagsCategories
ROS点云消息的解析和点云类型的转换
|PCL点云|
Word count: 714|Reading time: 3 min

消息类型

sensor_msgs/PointCloud2类型如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# N维的点云，点的数据以二进制blob形式保存，其layout用fields数组描述

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

Header header

点云的2D结构，如果点云无序，height是1，width就是点云的length
uint32 height
uint32 width

# 用二进制数据blob的形式，描述channels 及其layout
PointField[] fields

bool    is_bigendian
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[]   data       # 点的数据，size = row_step * height
bool is_dense        # True if there are no invalid points

PointCloud2的data是序列化后的数据，直接看不到物理意义。只能转为点云1类型

sensor_msgs/PointCloud类型如下:

1
2
3
4
5
6
7
8
9
Header header

# 在header坐标系中的3D点云
geometry_msgs/Point32[]  points

# Each channel should have the same number of elements as points array,
# and the data in each channel should correspond 1:1 with each point.
# Channel names in common practice are listed in ChannelFloat32.msg.
ChannelFloat32[] channels

坐标系和点云的位置关系如下，我们感兴趣的是z，也就是相机坐标系到点云的距离

从sensor_msgs::PointCloud2类型转为sensor_msgs::PointCloud，而且获知坐标

1
2
3
4
5
6
7
8
9
10
11
12
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 in_pcl;
sensor_msgs::PointCloud out_pcl;
sensor_msgs::convertPointCloud2ToPointCloud(in_pcl, out_pcl);
for (int i=0; i < out_pcl.points.size(); i++)
{
	ROS_INFO("out_pcl x: %f, y: %f, z: %f", 
          out_pcl.points[i].x, out_pcl.points[i].y, out_pcl.points[i].z);
}

消息的发布

意外发现ROS可以直接发布pcl的类型，不必转为ROS的消息类型

1
2
typedef pcl::PointCloud<PointT> PointCloudT;
PCL2_publisher_ = nh.advertise<PointCloudT>("pointcloud2", 1, this);

ros在pcl_ros中做了Publisher的多态。使得ros的标准Publisher可以接收pcl::PointCloud<T>类型的消息，并自动转换成sensor_msgs::PointCloud2的消息类型，再发布出去。

几种点云类型的转换

点云格式主要有四种，sensor_msgs::PointCloud，也就是点云1已经弃用，主要用点云2，但是点云1有时还有用。比如点云2转为点云1消息，可看到包含多少个点，rostopic echo --noarr pcl1/points，结果可以是<array type: geometry_msgs/Point32, length: 256000>，可看到每个点的坐标值。

  • sensor_msgs::PointCloud

  • sensor_msgs::PointCloud2

  • pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)

  • pcl::PointCloud — standard PCL data structure

相互转换关系
find_package里添加sensor_msgs, pcl_ros, pcl_conversions

ROS PointCloud2 和 PointCloud的转换

1
2
sensor_msgs::PointCloud out_pcl;
sensor_msgs::convertPointCloud2ToPointCloud(*pcl_dist_msg, out_pcl);

反过来就用sensor_msgs::convertPointCloudToPointCloud2

可以使用point_cloud_converter在点云1和点云2 之间转换

1
2
PointCloudConverter initialized to transform from PointCloud (/points_in) to PointCloud2 (/points2_out).
PointCloudConverter initialized to transform from PointCloud2 (/camera_/depth/points) to PointCloud (/pcl)

比如 rosrun point_cloud_converter point_cloud_converter_node points2_in:=/camera/depth/points points_out:=/pcl1

PointCloud2 转 pcl::PCLPointCloud2

1
2
3
4
sensor_msgs::PointCloud2 cloud;
pcl::PCLPointCloud2  pcl_pc2;

pcl_conversions::toPCL(cloud, pcl_pc2

pcl::PCLPointCloud2 转 pcl::PointCloud < pcl::PointXYZ >

1
2
3
4
pcl::PCLPointCloud2  pcl_pc2;
pcl::PointCloud < pcl::PointXYZ >  pcl_cloud;

pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);

pcl::PointCloud < pcl::PointXYZ > 转 sensor_msgs::PointCloud2

1
2
3
4
5
#include <pcl_conversions/pcl_conversions.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr inCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(*inCloud, cloud);

反过来就是fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T>&)

#include <sensor_msgs/point_cloud2_iterator.h>

参考: pcl_conversions