Revision history [back]

Comparing my code to the link sent by @Mike Scheutzow above and other links, I could found the mistake. Here's the corrected code

c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
z = np.float32(depth) #depth/1000 ==> float point cloud represent points in meters
x = np.float32(z * (c - cx) * pixel_width / fx )
y = np.float32(z * (r - cy) * pixel_higth / fy )
z = z.reshape(1,-1)
x = x.reshape(1,-1)
y = y.reshape(1,-1)
brightness = brightness.reshape(1,-1)
ptcloud = np.dstack([x, y, z, np.float32(brightness)])
return ptcloud.reshape(-1,4).tolist()

and changed msg.fields count to 1 instead of 20800

msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('intensity', 12, PointField.FLOAT32, 1) ]

and created the pointcloud2 using

pc2 = point_cloud2.create_cloud(msg.header, msg.fields, data)

Comparing my code to the link sent by @Mike Scheutzow above and other links, I could found the mistake. Here's the corrected code

c, r = np.meshgrid(np.arange(-cols/2,cols/2), np.arange(-rows/2,rows/2), sparse=True)
z = np.float32(depth) #depth/1000 ==> float point cloud represent points in meters
x = np.float32(z * (c - cx) * pixel_width / fx )
y = np.float32(z * (r - cy) * pixel_higth / fy )
z = z.reshape(1,-1)
x = x.reshape(1,-1)
y = y.reshape(1,-1)
brightness = brightness.reshape(1,-1)
ptcloud = np.dstack([x, y, z, np.float32(brightness)])
return ptcloud.reshape(-1,4).tolist()

and changed msg.fields count to 1 instead of 20800

msg.fields = [
    PointField('x', 0, PointField.FLOAT32, 1),
    PointField('y', 4, PointField.FLOAT32, 1), 
    PointField('z', 8, PointField.FLOAT32, 1), 
    PointField('intensity', 12, PointField.FLOAT32, 1) ]
]

pc2 = point_cloud2.create_cloud(msg.header, msg.fields, data)