如何有效地将ROS PointCloud2转换为pcl点云并在python中可视化
问题描述:
我正在尝试从ROS中的一个Kinect节点对点云进行一些分割.到目前为止,我有这个:
I'm trying to do some segmentation on a pointcloud from a kinect one in ROS. As of now i have this:
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循环而非常慢. 我的问题是:
This seems to work but is very slow because of the for loop. My questions are:
1)我如何有效地将PointCloud2消息转换为pcl pointcloud
1) How do i effeciently convert from the PointCloud2 message to a pcl pointcloud
2)我如何可视化云.
2) How do i visualize the clouds.
答
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数组,然后从该数组初始化点云.
I would prefer using ros_numpy module to first convert to numpy array and initialize Point Cloud from that array.