![](/img/trans.png)
[英]pointcloud2 stream visualization in open3d or other possibility to visualize pointcloud2 in python
[英]how to effeciently convert ROS PointCloud2 to pcl point cloud and visualize it in python
我正在嘗試對來自 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 點雲
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 數組並從該數組初始化點雲。
在帶有 Python3 的 Ubuntu 20.04 上,我使用以下內容:
import numpy as np
import pcl # pip3 install python-pcl
import ros_numpy # apt install ros-noetic-ros-numpy
import rosbag
import sensor_msgs
def convert_pc_msg_to_np(pc_msg):
# Fix rosbag issues, see: https://github.com/eric-wieser/ros_numpy/issues/23
pc_msg.__class__ = sensor_msgs.msg._PointCloud2.PointCloud2
offset_sorted = {f.offset: f for f in pc_msg.fields}
pc_msg.fields = [f for (_, f) in sorted(offset_sorted.items())]
# Conversion from PointCloud2 msg to np array.
pc_np = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pc_msg, remove_nans=True)
pc_pcl = pcl.PointCloud(np.array(pc_np, dtype=np.float32))
return pc_np, pc_pcl # point cloud in numpy and pcl format
# Use a ros subscriber as you already suggested or is shown in the other
# answers to run it online :)
# To run it offline on a rosbag use:
for topic, msg, t in rosbag.Bag('/my/rosbag.bag').read_messages():
if topic == "/my/cloud":
pc_np, pc_pcl = convert_pc_msg_to_np(msg)
這對我有用。 我只是調整點雲的大小,因為我的是一個有序的 pc (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
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.