How to create organized pointcloud from unorganized pointcloud?

edit

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.