Question

I want to use convertpointcloudto_image.cpp to convert an unorganized point cloud to a 2D image, but the function only accepts an organized point cloud.

rosrun pcl_ros convert_pointcloud_to_image input:=/unorganized_pc_object_topic output:=/image_from_pc_topic

Input point cloud is not organized, ignoring!

It seems like there should be a pcl function for such a common operation that I can use to convert the unorganized point cloud to an organized one. I would expect there to be a way to do something like this, or an existing C++ function where camera_info is used to organize the pointcloud:

rosrun pcl_ros convert_unorganized_to_organized input:=/unorganized_pc_object_topic cam_info:=/camera/color/camera_info output:=/organized_pc_topic

To clarify, I already understand conceptually what needs to happen, and I have been able to do this in Python, using the code provided below. I'm looking specifically for an existing node or exisiting .cpp function to do the equivalent.

points = pc2.read_points(msg_pointcloud, skip_nans=True, field_names=("x", "y", "z"))
camera_model.fromCameraInfo(intrinsics)

organized_points = []
for point in points:
    p = [point[0], point[1], point[2]]
    pp = camera_model.project3dToPixel(pc)
    organized_points.append(pp)

organized_points = np.array(organized_points, dtype=int)

Asked by ceres on 2019-06-14 14:34:50 UTC

Answer 1

I agree it would be a useful function to include in PCL, I've seen this question come up on a few occasions now so I'm considering adding the functionality myself. The function is not as straightforward as it first seems though which may be why it hasn't been included before.

A structured point cloud is a 3D grid with axes [image height, image rows, point cloud channel], importantly they are sparse so can contain null points, represented as either NaN values or the zero point depending on the datatype used. The algorithm you show in the code snippet in your question is creating a list of 2D pixel locations in the same order as the original unorganised points, this is not the same thing.

The pseudo code of your basic algorithm will look like this:

Create structured pointcloud array [width, height, channels] 
initialize the x, y and z values of the array to NAN, or [0, 0, 0] as appropriate

for p in points:
    calculate pixel position of point
    if pixel position lies within the image extents:
        if the existing point in the structure cloud is Null or the new point is closer than the existing point:
            add this point into the pixel position of the structured cloud

A complicating factor in this algorithm is deciding how to deal with point occlusions when the projected points have a lower resolution than their projected pixel resolution. Sometimes you will be able to partially see a background object through the gaps in a foreground object. This may not be a problem in your application, but if it is the fix is to convert the original point cloud to a mesh and then project the mesh to the structured cloud. This solution adds a significant amount of complexity and run time to the algorithm though.

Hope this gives you enough information to get this working.

Asked by PeteBlackerThe3rd on 2019-06-15 07:57:11 UTC

Comments

Thanks @PeteBlackerThe3rd - this is answer is really helpful for explaining the conceptual process of organizing a point cloud. I suppose I was hoping that even if there were some caveats to the accuracy of resulting point cloud, ROS would still have a native function to convert between common formats rather than requiring me to learn C++ and write my own function.

Asked by ceres on 2019-06-17 11:37:16 UTC

Any updates on adding such a function? The functionality has become even necessary for machine learning purposes!!!

Asked by hesham on 2021-08-21 05:58:28 UTC

How to create organized pointcloud from unorganized pointcloud?

Comments

Answers

Comments