如何有效地将ROS PointCloud2转换为pcl点云并在python中可视化

Question

我正试图从ROS中的kinect点云上做一些分割。现在我有了这个：

import rospy
import pcl
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
def on_new_point_cloud(data):
    pc = pc2.read_points(data, skip_nans=True, field_names=("x", "y", "z"))
    pc_list = []
    for p in pc:
        pc_list.append( [p[0],p[1],p[2]] )

    p = pcl.PointCloud()
    p.from_list(pc_list)
    seg = p.make_segmenter()
    seg.set_model_type(pcl.SACMODEL_PLANE)
    seg.set_method_type(pcl.SAC_RANSAC)
    indices, model = seg.segment()

rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/kinect2/hd/points", PointCloud2, on_new_point_cloud)
rospy.spin()

这似乎有效，但由于for循环，速度非常慢。 我的问题是：

1）如何有效地将PointCloud2消息转换为pcl pointcloud

2）我如何想象云。

Answer 1

这对我有用。我只是调整了点云的大小，因为我的是一台订购的电脑（512x x 512px）。我的代码改编自@Abdulbaki Aybakan-谢谢！

我的代码：

pc = ros_numpy.numpify(pointcloud)
height = pc.shape[0]
width = pc.shape[1]
np_points = np.zeros((height * width, 3), dtype=np.float32)
np_points[:, 0] = np.resize(pc['x'], height * width)
np_points[:, 1] = np.resize(pc['y'], height * width)
np_points[:, 2] = np.resize(pc['z'], height * width)

要使用ros-numpy，必须克隆repo：http://wiki.ros.org/ros_numpy

Answer 2

import rospy
import pcl
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import ros_numpy

def callback(data):
    pc = ros_numpy.numpify(data)
    points=np.zeros((pc.shape[0],3))
    points[:,0]=pc['x']
    points[:,1]=pc['y']
    points[:,2]=pc['z']
    p = pcl.PointCloud(np.array(points, dtype=np.float32))

rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/velodyne_points", PointCloud2, callback)
rospy.spin()

我更希望使用ros_numpy模块首先转换成numpy数组并从该数组初始化点云。