Point clouds from depth images, getCameraImage documentation

[atirehgram]

Hi, I am trying to get point clouds from depth images in PyBullet but I do not understand how the transformations work so I don't know how to invert them.

Basically what I'm doing is:

1. I load an object from a .urdf file (for example, a cube), and place it on a plane. EDIT: I also scale the cube down to 0.1 [m] because the objects I have to work with are in the range of 5-15 cm.
2. I set the projection matrix according to the intrinsic parameters of my camera; I have to do this manually because my camera has nonzero o_x, o_y and skew. My intrinsic camera matrix has f_x, f_y, o_x, o_y in pixels.
3. I set the view matrix using computeViewMatrixFromYawPitchRoll
4. I use getCameraImage to extract the depth buffer
5. I linearise the depth buffer using this formula: depth = far * near / (far - (far - near) * depth_buffer that I found here. But either the values that I get are not consistent with the geometry of the scene, or I do not understand what they represent and how to transform them in world Z coordinates, or in camera z coordinates. (Edit: I had another formula here but I realised they were the same)
6. Once I have the linearised depth map, I multiply projection matrix and view matrix and invert the transformation, and I would expect to be able to apply it to the pixel coordinates directly to obtain X,Y global coordinates, but this is not the case. EDIT: Apparently, I had forgotten the viewport matrix. I have included it in the code but I am not sure it is correct: should I take the skew into account as well?
7. The point cloud that I obtain does not have the same dimensions as the object in any direction, and the top surface is not as far from the plane.

What am I doing wrong? Is there some documentation about what transformations the getCameraImage function applies and in which order, and expecting which format? What is in my depth variable and how do I use it?

Thank you in advance.

This is some of my code:

import pybullet as pb
import pybullet_data
import numpy as np

physicsClient = pb.connect(pb.GUI)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())


### Desired image size and intrinsic camera parameters
width = 640
height = 480
f_x = 543.820
f_y = 546.691
c_x = 314.424
c_y = 237.466
skew = 0.283204

### The far and near values depend on the min,max desired distance from the object
far = 1.1
near = 0.5

plane_id = pb.loadURDF("plane.urdf")
obj_urdf = "cube.urdf"
obj_initial_position = [0.,0.,0.5]
obj_initial_orientation = pb.getQuaternionFromEuler([0.,0.,0.])
obj_pb_id =  pb.loadURDF(obj_urdf,
                        basePosition=obj_initial_position,
                        baseOrientation=obj_initial_orientation,
                        globalScaling=0.1
                        )

### Step simulation to make the cube fall on the plane
pb.setGravity(0, 0, -9.81)
for step in range(2000):
    pb.stepSimulation()
    time.sleep(1./2000)

### Calculate bounding boxes
aabb = pb.getAABB(obj_pb_id)
aabb_min = np.array(aabb[0])
aabb_max = np.array(aabb[1])
obj_center = list((aabb_max + aabb_min)/2)
dim = aabb_max - aabb_min
print("Object model dimensions [m]: ", dim[0], dim[1])
print("Maximum elevation from the plane [m]: ", dim[2])

### Camera position and orientation
camera_look_at = obj_center
camera_distance = 0.65
camera_yaw = 0
camera_pitch = 180 # To have the camera looking down along the global Z axis
camera_roll = 0

### Compute view matrix
view_matrix = pb.computeViewMatrixFromYawPitchRoll(
    cameraTargetPosition=camera_look_at,
    distance = camera_distance,
    yaw = camera_yaw,
    pitch = camera_pitch,
    roll = camera_roll,
    upAxisIndex = 1
)

### Camera intrinsic:
camera_intrinsic_matrix = (f_x,   skew,   c_x,
                    0,      f_y,    c_y,
                    0,      0,      1)

### OpenGL-style projection matrix:
opengl_projection_matrix = (2*f_x/width,            0,                      0,                      0,
                            skew,                   2*f_y/height,           0,                      0,
                            2*(c_x+0.5)/width-1,    2*(c_y+0.5)/height-1,   -(far+near)/(far-near), -1,
                            0,                      0,                      -2*far*near/(far-near), 0)

### Get depth values using Tiny renderer
images = pb.getCameraImage(height=height,
                        width=width,
                        viewMatrix=view_matrix,
                        projectionMatrix=opengl_projection_matrix,
                        shadow=True,
                        renderer=pb.ER_TINY_RENDERER)
rgb = np.reshape(images[2], (height,width, 4)) # RGB+alpha
seg = np.reshape(images[4], [height,width])
depth_buffer = np.reshape(images[3], [height,width]) # What is in this buffer?
depth = far * near / (far - (far - near) * depth_buffer) # What is done here? Why the values I get are so small?
obj_segmentation = seg > 0
depth_buffer_seg = depth_buffer * obj_segmentation
depth_seg = depth*obj_segmentation

### Set transformation matrix
opengl_projection_matrix_np = np.transpose(np.reshape(np.array(opengl_projection_matrix), (4,4)))  # needs to be transposed because OpenGL is column-based
projection_matrix_np = np.reshape(np.array(projection_matrix), (3,3))
view_matrix_inv_np = np.transpose(np.reshape(np.array(view_matrix), (4,4))) # From world to camera, needs to be transposed because OpenGL is column-based
view_matrix_np = np.linalg.inv(view_matrix_inv_np) # From camera to world

### Put transform together, from projection to world coordinates
T = np.linalg.inv(np.matmul(opengl_projection_matrix_np,view_matrix_inv_np))

### Generate point cloud
pc = []
for u in range(height):
    for v in range(width):
        if obj_segmentation[u,v]:
            point_pixel = np.array([2*v/width-1,(-2*u/height+1),2*depth[u,v]-1,1]) # depth or depth_buffer here?
            point_camera = np.matmul(np.linalg.inv(opengl_projection_matrix_np),point_pixel)
            point_world = np.matmul(view_matrix_np,point_camera)
            point_world /= point_world[3]
            pc.append(point_world[:3])
pc = np.array(pc)

### Find dimensions of the point cloud and compare with mesh dimensions and depth map
xmin, ymin, zmin = pc.min(axis=0)
xmax, ymax, zmax = pc.max(axis=0)
dim_pc = [xmax-xmin,ymax-ymin,zmax-zmin]
print("Dimensions of the point cloud [m]: ", dim_pc[0], dim_pc[1])
print("Depth of the point cloud [m]: ", dim_pc[2])`

[wengzehang]

Hi, I have the same issue, did you figure it out?

[atirehgram]

Yes, here is part of my code:

import os
import numpy as np

import open3d as o3d

import pybullet as pb
import pybullet_data

### Desired image size and intrinsic camera parameters
### Intrinsic camera matrix from 4th Oct 2021:    543.820     0.283204    314.424;
###                                               0.00000     546.691     237.466;
width = 640
height = 480
f_x = 543.820
f_y = 546.691
c_x = 314.424
c_y = 237.466
skew = 0.283204

### The far and near values depend on the min,max desired distance from the object (from the size of the objects!)
far = 1.1
near = 0.1

### Camera intrinsic:
opengl_projection_matrix = (f_x/width,            0,                      0,                      0,
                            skew/width,           f_y/height,           0,                      0,
                            2*(c_x+0.5)/width-1,  2*(c_y+0.5)/height-1,   -(far+near)/(far-near), -1,
                            0,                    0,                      -2*far*near/(far-near), 0)


plane_id = pb.loadURDF(fileName="plane.urdf",physicsClientId=process_id)
obj_initial_orientation_euler = np.random.uniform(low=0.,high=360.,size=3)
scale = np.random.uniform(low=1.,high=3.)

obj_initial_orientation = pb.getQuaternionFromEuler(obj_initial_orientation_euler,physicsClientId=process_id)
obj_pb_id =  pb.loadURDF(obj_urdf,
                        basePosition=obj_initial_position,
                        baseOrientation=obj_initial_orientation,
                        globalScaling=scale,
                        physicsClientId=process_id
                        )

pb.setGravity(gravX=0, gravY=0, gravZ=-9.81, physicsClientId=process_id)
for _ in range(2000):
    pb.stepSimulation(physicsClientId=process_id)
    time.sleep(1./2000)

obj_position, obj_orientation = pb.getBasePositionAndOrientation(obj_pb_id,physicsClientId=process_id)
obj_position_np = np.array(obj_position)

### Matrix to change from object to world coordinates
T_w_o = np.empty((4,4))
T_w_o[:3,:3] = np.eye(3)
T_w_o[:3,3] = obj_position_np
T_w_o[3,:] = np.array([0,0,0,1])

### Matrix to change from world to object coordinates
T_o_w = np.linalg.inv(T_w_o)

aabb = pb.getAABB(obj_pb_id,physicsClientId=process_id)
aabb_min = np.array(aabb[0])
aabb_max = np.array(aabb[1])
obj_center = list((aabb_max + aabb_min)/2)

### Initialize point cloud and its normals
pc = []
normals = []

phis = [1, 45, 90, 135, 179]
thetas = [1, 10, 45]

### Camera position and orientation
camera_look_at = obj_center

for phi_deg in phis:
    for theta_deg in thetas:

        camera_distance = np.random.uniform(0.35,0.55)

        phi = np.deg2rad(phi_deg)
        theta = np.deg2rad(theta_deg)
        camera_eye_position = []
        camera_eye_position.append(camera_distance*np.cos(phi)*np.sin(theta) + obj_center[0])
        camera_eye_position.append(camera_distance*np.sin(phi)*np.sin(theta) + obj_center[1])
        camera_eye_position.append(camera_distance*np.cos(theta) + obj_center[2])

        view_matrix = pb.computeViewMatrix(
            cameraEyePosition= camera_eye_position,
            cameraTargetPosition=camera_look_at,
            cameraUpVector = [0,0,1],
            physicsClientId=process_id
        )

        ### Get depth values using Tiny renderer
        images = pb.getCameraImage(height=height,
                width=width,
                viewMatrix=view_matrix,
                projectionMatrix=opengl_projection_matrix,
                shadow=True,
                renderer=pb.ER_TINY_RENDERER,
                physicsClientId=process_id
                )
        rgb = np.reshape(images[2], (height,width, 4)) # RGB+alpha
        seg = np.reshape(images[4], [height,width])
        depth_buffer = np.reshape(images[3], [height,width]) # What is in this buffer?
        depth = far * near / (far - (far - near) * depth_buffer) # What is done here?
        obj_segmentation = seg > 0
        depth_buffer_seg = depth_buffer * obj_segmentation
        depth_seg = depth*obj_segmentation

        opengl_projection_matrix_np = np.transpose(np.reshape(np.array(opengl_projection_matrix), (4,4)))
        view_matrix_inv_np = np.transpose(np.reshape(np.array(view_matrix), (4,4))) # From world to camera, needs to be transposed because OpenGL is column-based
        view_matrix_np = np.linalg.inv(view_matrix_inv_np) # From camera to world

        pc_view = []            

        ### Generate point cloud
        for u in range(height):
            for v in range(width):
                if obj_segmentation[u,v]:
                    point_pixel = np.array([(2*v/width-1),(-2*u/height+1),2*depth_buffer[u,v]-1,1])
                    point_camera = np.matmul(np.linalg.inv(opengl_projection_matrix_np),point_pixel)
                    point_world = np.matmul(view_matrix_np,point_camera)
                    point_world /= point_world[3]
                    pc_view.append(point_world[:3])
                    pc.append(point_world[:3])

        pc_ply = o3d.geometry.PointCloud()
        pc_ply.points = o3d.utility.Vector3dVector(pc_view)
        pc_ply.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=4, max_nn=300))
        pc_ply.orient_normals_towards_camera_location(camera_eye_position)
        normal = np.reshape(np.array(pc_ply.normals),np.shape(pc_view))
        #o3d.visualization.draw_geometries([pc_ply])
        pc_ply.clear()
        normal = normal.tolist()
        normals.extend(normal)

### Point cloud and normals in object coordinates (the center of the object is at 0,0,0)
pc = np.array(pc)
ones = np.ones(2048)
pc = np.c_[pc,ones]
pc = np.matmul(T_o_w,pc.T)[:3,:].T

normals = np.array(normals)
zeros = np.zeros(2048)
normals = np.c_[normals,zeros]
normals = np.matmul(T_o_w,normals.T)[:3,:].T

### Point cloud with normals per point
pc_normals = np.concatenate((pc, normals),axis=1)

np.save(dir_name_pc + "/" + str(1) + "_" + obj_filename + ".npy", pc_normals)

pc_ply = o3d.geometry.PointCloud()
pc_ply.points = o3d.utility.Vector3dVector(pc)
pc_ply.normals = o3d.utility.Vector3dVector(normals)
o3d.io.write_point_cloud(dir_name_pc + "/" + str(1) + "_" + obj_filename + ".ply", pc_ply)