使用PCL库、iai_kinect2读取kinect2的点云信息

1.添加pcl依赖

CMakeLists.txt文件中加入：

find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  rospy
  sensor_msgs
  std_msgs
  pcl_conversions
  pcl_ros
)

include_directories(${PCL_INCLUDE_DIRS})

link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${PCL_DEFINITIONS})

package.xml文件中加入：

libpcl-all-dev
libpcl-all

 

 

2.查看iai_kinect2发布的数据

最开始我并不清楚iai_kinect2发布了哪些话题，我可以直接使用的。于是我运行了kinect2_bridge的launch文件。

roslaunch kinect2_bridge kinect2_bridge.launch

在运行：

rostopic list

可以看到有如下话题:

/kinect2/bond
/kinect2/hd/camera_info
/kinect2/hd/image_color
/kinect2/hd/image_color/compressed
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_depth_rect
/kinect2/hd/image_depth_rect/compressed
/kinect2/hd/image_mono
/kinect2/hd/image_mono/compressed
/kinect2/hd/image_mono_rect
/kinect2/hd/image_mono_rect/compressed
/kinect2/hd/points
/kinect2/qhd/camera_info
/kinect2/qhd/image_color
/kinect2/qhd/image_color/compressed
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_depth_rect
/kinect2/qhd/image_depth_rect/compressed
/kinect2/qhd/image_mono
/kinect2/qhd/image_mono/compressed
/kinect2/qhd/image_mono_rect
/kinect2/qhd/image_mono_rect/compressed
/kinect2/qhd/points
/kinect2/sd/camera_info
/kinect2/sd/image_color_rect
/kinect2/sd/image_color_rect/compressed
/kinect2/sd/image_depth
/kinect2/sd/image_depth/compressed
/kinect2/sd/image_depth_rect
/kinect2/sd/image_depth_rect/compressed
/kinect2/sd/image_ir
/kinect2/sd/image_ir/compressed
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/points
/rosout
/rosout_agg
 

于是一脸懵逼的看到这么多话题，不知道选择哪一个。

打开kinect2_bridge下面的README.md文件可以看到：

## Topics

### HD Topics

The images in this topics have a FullHD resolution (1920x1080).

*Note: For correct registration of the depth image to the color image it is needed to perform a calibration.*

 

### Quater HD Topics

The images in this topics have a quarter FullHD resolution (960x540).

*Note: For correct registration of the depth image to the color image it is needed to perform a calibration.*

 

### IR/Depth Topics

This are the raw IR and depth images from the sensor (512x424).

*Note: The registration of the color image is available without a calibration. Parameters for the registration are provided by the sensor itself.*

hd为高分辨率，qhd为中分辨率，sd代表低分辨率，ir代表红外图/深度图。我所要找的就是/kinect2/sd/points发布的内容。

 

于是我查看/kinect2/sd/points发布的格式是什么：

 rostopic type /kinect2/sd/points 

xp@xp-X150M-PLUS-WS:~$ rostopic type /kinect2/sd/points 
sensor_msgs/PointCloud2

得到sensor_msgs/PointCloud2信息。

 

3.解析sensor_msgs/PointCloud2

我们打开/opt/ros/kinetic/share/sensor_msgs/msg可以看到PoinCloud2.msg文件:

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.

# 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.

# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

我们所需要查看的数据为uint8[] data，但是仔细研究可以得到，一个点是由32个data，也是32个字节进行秒速，最终通过PointField[] fields里的内容对齐解析。我没有去解析它，而是绕道使用pcl库进行转换。

4.pcl库转换PointCloud2数据

#include "ros/ros.h"

#include

#include

#include

#include

#include

#include

#include

 

#include

#include

#include

 

pcl::visualization::CloudViewer viewer("cloud_show");

void callback(const sensor_msgs::PointCloud2& msg)

{

pcl::PointCloud::Ptr cloud (new pcl::PointCloud);

pcl::fromROSMsg(msg,*cloud);

for (size_t i = 0; i < cloud11->points.size(); ++i)

{

    //cloud->points[i].x /= 2;

    //cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);

    //cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);

}

//显示点云

viewer.showCloud(cloud);

pub_.publish(msg);

}

标注红色信息两行是最关键转换的两行，如果只想得到深度点云值，只需要将改为即可。

 

有问题欢迎留言！