Get 3D point from Depth/RGB

[msieb1]

Hello,

how to obtain the 3D point of a corresponding pixel, i.e. the 3D point cloud from projection and view matrix? I looked at the PointCloud example, but this only does this with the debugger camera, accessing parameters such as camForward which I do not have access to for generic camera setups.

I used a couple of transformations, but none seem to work. In essence, what is the formula to go from a pixel pair of the results array of p.getCameraImage to the corresponding 3D point in world frame?

[erwincoumans]

You need the information that is used in the pointCloudFromCameraImage.py
Try figusing out how to get/compute the camForward vector (the target position of the camera - world position of the camera) etc.

PyBullet uses the common OpenGL camera model for rgb/depth. Try OpenGL forums/search (gluUnProject), this is not specific to PyBullet. Good luck!

[ondrejbiza]

This issue is four years old but it shows up on Google.
Here's simple vectorized code for getting point clouds in world coordinates:

import numpy as np
import pybullet as pb


def get_point_cloud(width, height, view_matrix, proj_matrix):
    # based on https://stackoverflow.com/questions/59128880/getting-world-coordinates-from-opengl-depth-buffer

    # get a depth image
    # "infinite" depths will have a value close to 1
    image_arr = pb.getCameraImage(width=width, height=height, viewMatrix=view_matrix, projectionMatrix=proj_matrix)
    depth = image_arr[3]

    # create a 4x4 transform matrix that goes from pixel coordinates (and depth values) to world coordinates
    proj_matrix = np.asarray(proj_matrix).reshape([4, 4], order="F")
    view_matrix = np.asarray(view_matrix).reshape([4, 4], order="F")
    tran_pix_world = np.linalg.inv(np.matmul(proj_matrix, view_matrix))

    # create a grid with pixel coordinates and depth values
    y, x = np.mgrid[-1:1:2 / height, -1:1:2 / width]
    y *= -1.
    x, y, z = x.reshape(-1), y.reshape(-1), depth.reshape(-1)
    h = np.ones_like(z)

    pixels = np.stack([x, y, z, h], axis=1)
    # filter out "infinite" depths
    pixels = pixels[z < 0.99]
    pixels[:, 2] = 2 * pixels[:, 2] - 1

    # turn pixels to world coordinates
    points = np.matmul(tran_pix_world, pixels.T).T
    points /= points[:, 3: 4]
    points = points[:, :3]

    return points