Ask Your Question
0

Read colours from a pointcloud2 python

asked 2015-05-08 04:05:37 -0600

panos gravatar image

Hi all,

My issue is the following:

I have a kinect sensor and I want to parse its point cloud using Python. I have subscribed to the topic and I am able to retrieve positional information of the pointcloud(x,y,z)

I now also need colour information but the read_points API does not seem to provide such functionality

Is there any other tool I can use to also extract the colours in python? It seems I am doing something wrong since taking the point in cpp is, and I cannot believe I'm saying this, easier.

My code so far in the point extraction part is the following:

import sensor_msgs.point_cloud2 as pc2
...............
# subscribe to pointcloud topic define the function that does the parsing
def PointCloud(self, PointCloud2):
    self.lock.acquire()
    gen = pc2.read_points(PointCloud2, skip_nans=True, field_names=("x" , "y" , " z" ))
    int_data = list(gen)
    print int_data[0] #Gives first point etc
    self.lock.release()

I tried adding field_names but the read_points API made it clear it does not work that way. Any thoughts on a fast way to also extract colour info from my pointcloud?

Thanks in advance,

Panos

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-05-08 06:37:03 -0600

panos gravatar image

The fix was actually to first remove the field_names from the read_points input parameters.

That way the function output 4 values per line instead of 3.

Taking the last value of that list and doing the required bitwise operations to retrieves the individual r,g,b values.

My code in case someone needs it in the future is the following:

import sensor_msgs.point_cloud2 as pc2
import ctypes
import struct
...............
# subscribe to pointcloud topic define the function that does the parsing
def PointCloud(self, PointCloud2):
    self.lock.acquire()
    gen = pc2.read_points(PointCloud2, 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 & 0x0000FF00)
        print r,g,b # prints r,g,b values in the 0-255 range
                    # x,y,z can be retrieved from the x[0],x[1],x[2]
    self.lock.release()
edit flag offensive delete link more

Comments

Is it possible to do it in matrix format, instead of element form.

kapoor_amita gravatar imagekapoor_amita ( 2016-12-14 08:38:01 -0600 )edit

@panos Thanks for the code!!

I think there is a typo here. It looks like the last bit mask should be:

b = (pack & 0x000000FF)

instead of:

b = (pack & 0x0000FF00)
MartyMcFly gravatar imageMartyMcFly ( 2016-12-20 10:10:04 -0600 )edit

This worked for me. Thank you!

Srijal gravatar imageSrijal ( 2018-06-07 03:45:16 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-05-08 04:05:37 -0600

Seen: 1,919 times

Last updated: May 08 '15