how to effeciently convert ROS PointCloud2 to pcl point cloud and visualize it in python

Question

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()

This seems to work but is very slow because of the for loop. My questions are:

1) How do i effeciently convert from the PointCloud2 message to a pcl pointcloud

2) How do i visualize the clouds.

Ulterior

Answer 1

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()

I would prefer using ros_numpy module to first convert to numpy array and initialize Point Cloud from that array.

Armillary

This wouldn't work for ROS melodic since there is no ros_numpy for it. Could we somehow use something else to interpret the variable data? Or is this the only way? – Knowle

where is the module pcl – Fordo

Answer 2

On Ubuntu 20.04 with Python3 I use the following:

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)

Mousterian

Answer 3

This works for me. I just resize the point cloud since mine is an ordered pc (512x x 512px). My code is adapted from @Abdulbaki Aybakan - thanks for this!

My code:

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)

To use ros_numpy one has to clone the repo: http://wiki.ros.org/ros_numpy

Indecipherable

Does this work if pointcloud has holes in it? Will these just become NaNs in np_points? – Lamrouex

Not sure if this works with an ordered pointcloud2, but it might work if you set every value of each point that is a hole to NaN. The "is_dense" flag should then be set to False, see: docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html – Indecipherable