Convert Pixel Coordinates (U,V) to Pointcloud2 (X, Y Z) (Python)

edit

Thanks for your response Ranjit, it seems very close to the result I am looking for.

After implementing your suggestion, as seen below, I am stuck on the last part (part e). Parts a and b are done in different functions, but I believe I have done those bits correctly

### Callback for converting a pixel XY and depth to XYZ ###
def xyzCallback(self, data):
    print (intY)
    print (intX)

    gen = point_cloud2.read_points(data, field_names='z', skip_nans=False, uvs=[(intX, intY)])

    nextGen = next (gen)

    XY = np.array(cam_model.projectPixelTo3dRay((intX,intY)))

    print (XY)

    print(XY*nextGen)

The initial results just for XY print out an X, Y and Z coordinate which don't seem correct, I am assuming this is because I haven't normalised them. When I then multiply them by the next(Gen) I get an array of three NaN's.

Have you got any ideas about what I am doing wrong?

Hello LeeAClift,

XY = np.array(cam_model.projectPixelTo3dRay((intX,intY)))

In this code line, you are obtaining the vector that means on point cloud you are getting X and Y from 3d space. The output of this line should be in X, Y, Z Note: over here your Z always be 1 or around 1.

But before performing this did you check that you are getting distance for a particular pixel from point cloud by using the get_depth() function which was in point C? If you received the distance then start finding the vector which is your XY = np.array(cam_model.projectPixelTo3dRay((intX,intY))) .

For your reference, I am attaching a link that can be helpful to you. If you have any questions feel free to ask.

Continue.

As I have maintained in the above comment about the Z value that it will be 1 or near to 1. For Eg. 0.98 or any random number. To make this number to 1 you first have to normalize this Z value to 1 and then multiply your X and Y with your obtained depth.

vector = self.camera_model.projectPixelTo3dRay((X,Y))
ray_z = [el / vector[2] for el in vector]
positions = [el * depth[0] for el in ray_z]

Here, this position will be X, Y, Z of the point cloud.

If you want to check that your output in 3dspace means point cloud is true or false. You can check it by following doing reverse engineering by obtaining your U, V from X, Y, Z.

For that you can use the function called camera_model.project3dToPixel((positions[0], positions[1], positions[2])) and pass your X,Y,Z and you will get the U, V and compare it with your previous U,V value. If your code seems to be right then both values should be similar.

Hi Ranjit, thank you for the continuation.

Just so my understanding is correct, in the code above, vector gives the nono-normalised X and Y.

From here, we then normalise the Z with ray_Z, and then finally combine the now normalised vector with the depth we had gotten from get_depth?

After running this, I am still getting the issue that the result of gen = point_cloud2.read_points(data, field_names='z', skip_nans=False, uvs=[(intX, intY)]) is always giving a NaN, and when combined by doing positions = [el * depth[0] for el in ray_z], I get an array of 3 NaN's.

Am I doing something wrong? Does Get_distance have to be its own function, and not just a line of code in my current function?

vector gives the nono-normalised X and Y

Yes, you will get X, Y, Z but make a note: Z will be 1 or around 1. If you are getting Z around 0.89, or 0.98 it is normal so for that you have to normalize Z by ray_z = [el / vector[2] for el in vector] here vector[2] is your Z value.

Now to obtain your Z value you need to call your gen = point_cloud2.read_points(data, field_names='z', skip_nans=False, uvs=[(intX, intY)])nextGen = next (gen) it will not be NaN. This line of code will always give you the exact distance in millimeters from the camera to the U,V pixel. I think you are doing something wrong if you are getting NaN values over here.

positions = [el * depth[0] for el in ray_z]

By multiplying, you will able to get pixel on 3d world(point cloud).

Thank you once again for your quick responses Ranjit, it is very much appreciated.

It seems I am doing something wrong here, but what, I do not know. I am quite new to robotic vision as a general field, hence many of my questions. I seem to be getting NaN whenever I access my pointcloud via point_cloud2.read_points, but if I use point_cloud2.get_xyz_points I can see them.

I know its a big ask, and you've done so much already, but is there any chance you could provide a complete class or function from the many snippets you've provided me, as I'm certain I am doing something wrong somewhere, and something like that would probably solve it.

Hey LeeAclift,

I was giving you a code snippet from below link but this code is of finding 3d point from the depth camera and plot it into a point cloud. In place of the point cloud, you can also use a depth camera for obtaining the distance.