Robotics StackExchange | Archived questions

How o save a PointCloud2 data in Python

Hi! I am subscribing to a data of Type: sensor_msgs/PointCloud2. How do I save he point cloud data in a python file. How do I convert it to ROS daa. Please share links of tutorial if any. I am not able to find one.

Asked by Joy16 on 2017-02-22 19:35:28 UTC

Comments

Support for PCL datatypes in Python is rather thin. If you search around, you'll find a few people who have written some conversion methods themselves (ie: PCL -> numpy) but it's all unofficial. PCL currently works best from C++ I'm afraid.

Asked by gvdhoorn on 2017-02-23 01:45:20 UTC

Answers

Have you heard of open3d ?, that is a Python libabry which can help you save cloud with popular formats(e.g .ply). lets say you have pointcloud2 topic and you want to read and save the Pointcloud

from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import open3d as o3d
import sensor_msgs.point_cloud2 as pc2
import ctypes
import struct

def callback(self, ros_point_cloud):
        xyz = np.array([[0,0,0]])
        rgb = np.array([[0,0,0]])
        #self.lock.acquire()
        gen = pc2.read_points(ros_point_cloud, skip_nans=True)
        int_data = list(gen)

        for x in int_data:
            test = x[3] 
            # cast float32 to int so that bitwise operations are possible
            s = struct.pack('>f' ,test)
            i = struct.unpack('>l',s)[0]
            # you can get back the float value by the inverse operations
            pack = ctypes.c_uint32(i).value
            r = (pack & 0x00FF0000)>> 16
            g = (pack & 0x0000FF00)>> 8
            b = (pack & 0x000000FF)
            # prints r,g,b values in the 0-255 range
                        # x,y,z can be retrieved from the x[0],x[1],x[2]
            xyz = np.append(xyz,[[x[0],x[1],x[2]]], axis = 0)
            rgb = np.append(rgb,[[r,g,b]], axis = 0)

        out_pcd = o3d.geometry.PointCloud()    
        out_pcd.points = o3d.utility.Vector3dVector(xyz)
        out_pcd.colors = o3d.utility.Vector3dVector(rgb)
        o3d.io.write_point_cloud("/home/atas/cloud.ply",out_pcd)   

I have used this code before and it works but you may adjust some parts of the code for your needs

Asked by Fetullah Atas on 2020-04-12 20:01:24 UTC

Comments

This is good solution. However, when I tried running it, it took too long to save only one pcl/pcd file.

Considering that the average point number in a PointCloud message is usually too big, and we already know it from the length of int_data array, we can eliminate the np.append() operations by pre-allocating the space for numpy arrays as below:

xyz = np.empty((len(int_data), 3))
rgb = np.empty((len(int_data), 3))

Then we can assign each point in the loop with an index. E.g. xyz[idx] = x[:3]

This way the function completes way faster.

Asked by harun-loodos on 2022-07-07 10:09:46 UTC