ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to to convert point UV in image to XYZ in pointcloud2 (python)?

asked 2017-11-23 17:25:23 -0600

db gravatar image

Given the pixel position, say (u,v) in the RGB image, how do I get the corresponding (X,Y,Z) from pointcloud2 data, using a sensor like kinetic camera.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2017-11-23 17:26:54 -0600

db gravatar image

updated 2017-11-23 17:27:57 -0600

Couldn't find answer anywhere, so I'm sharing my solution. If you have a better option, please post.

This worked for me (using asus Xtion Pro):

def pixelTo3DPoint(cloud, u, v):
    width = cloud.width
    height = cloud.height
    point_step = cloud.point_step
    row_step = cloud.row_step

    array_pos = v*row_step + u*point_step

    bytesX = [ord(x) for x in cloud.data[array_pos:array_pos+4]]
    bytesY = [ord(x) for x in cloud.data[array_pos+4: array_pos+8]]
    bytesZ = [ord(x) for x in cloud.data[array_pos+8:array_pos+12]]

    byte_format=struct.pack('4B', *bytesX)
    X = struct.unpack('f', byte_format)[0]

    byte_format=struct.pack('4B', *bytesY)
    Y = struct.unpack('f', byte_format)[0]

    byte_format=struct.pack('4B', *bytesZ)
    Z = struct.unpack('f', byte_format)[0]

    return [X, Y, Z]
edit flag offensive delete link more

Comments

Could you explain further how this works?

TC gravatar image TC  ( 2021-03-17 14:57:31 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2017-11-23 17:25:23 -0600

Seen: 1,312 times

Last updated: Nov 23 '17