Pointcloud value at a given index

asked 2019-06-16 10:52:26 -0500

anirban
64 ●20 ●25 ●29

Hi,

I am using RealSense-415 camera to get RGB color image of size(480X640) and pointcloud data of length (307200 i.e., 480X640). I achieve this using the Python code below.

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2 

def callback_ptclud(ptcloud_data):
    assert isinstance(ptcloud_data, PointCloud2)
    pt_gen = point_cloud2.read_points(ptcloud_data)
    for pt in pt_gen:
        print(pt)

def listener():
    rospy.Subscriber("/camera/depth_registered/points", PointCloud2, callback_ptclud)
    rospy.spin()

if __name__ == '__main__':
    rospy.init_node("realsense_subscriber", anonymous=True)
   listener()

Now my questions are as follows,

As you can see that inside the callback_ptclud() function, I need to loop through all the data points of pt_gen to access point-cloud data. Suppose, I need to access only the data in pt_gen at index i=1000. Is there a way to get to index value at i directly instead of looping through all 999 points?
Suppose that pointcloud data is aligned with color image and I want to retrieve the pointcloud value corresponding to the color image pixel index px=240, py=320. Then what will be the corresponding index value of pt_gen?

Any help is appreciated.

answered 2019-06-16 11:55:37 -0500

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

updated 2019-06-16 14:41:10 -0500

1) If you're not too bothered about performance you can use the read_points_list method instead this returns all the points in the point cloud as a list which you can index the one you want. This won't be very efficient though so you may need to write your own code to access individual values directly from the raw data of the point cloud.

points = point_cloud2.read_points_list(ptcloud_data)
print points[50]

However this function internally will loop through all points to make the list. You can access the point directly but you'll have to use the point_step, row_step and data members directly then decode the type of point used in your point cloud. I'm not sure if any easier method than this exists unfortunately.

2) The points in structured point cloud like will be ordered using Row-Major ordering, so you can calculate the index of a particular pixel using the formula below:

PointIndex = PixelX + ( PixelY * CloudWidth)

You can also find the width and height of this structured cloud from the PointCloud2 message itself by accessing the width and height members. I recommend you use this approach so your code will be compatible with all structured point cloud messages.

Hope this helps.

Update:

In order to extract the point data from the data buffer you'll want to use the python struct.unpack_from function, this with convert the byte buffer into the actual point data. If you just want your code to work with float 32 XYZ type point clouds then its fairly simple to hard code the structure in. The full PointCloud2 decoder can detect and work with all the different point types, but that is probably more complex than necessary in your case.

The code will look something like this:

index = (row*ptcloud_data.row_step) + (col*ptcloud_data.point_step)
(X, Y, Z) = struct.unpack_from('fff', ptcloud_data.data, offset=index)

Hope this helps.

edit flag offensive delete link

Comments

I actually tried to use point_step, row_step and 'data' to retrieve individual values but unable to get the data as they are in bytes and I did not know how to get numerical values from a byte array. Can you please point me to some documentation on this. Thank you for your answer though.