Convert Mujoco Depth Image to Open3D Point Cloud

Question

I'm trying to generate an Open3D point cloud from a depth image rendered in MuJoCo. My code is below, with the MuJoCo dependency commented out and rendered depth images linked below:

import math
import numpy as np
import open3d as o3d

def generatePointCloud():

    img_width = 640
    img_height = 480

    aspect_ratio = img_width/img_height
    # sim.model.cam_fovy[0] = 60
    fovy = math.radians(60)
    fovx = 2 * math.atan(math.tan(fovy / 2) * aspect_ratio)
    fx = 1/math.tan(fovx/2.0)
    fy = 1/math.tan(fovy/2.0)
    cx = img_width/2
    cy = img_height/2
    cam_mat = o3d.camera.PinholeCameraIntrinsic(img_width, img_height, fx, fy, cx, cy)

    depth_img = captureImage()

    o3d_depth = o3d.geometry.Image(depth_img)
    o3d_cloud = o3d.geometry.PointCloud.create_from_depth_image(o3d_depth, cam_mat)
    #o3d_cloud = scaleCloudXY(o3d_cloud)

    o3d.visualization.draw_geometries([o3d_cloud])

# Render and process an image
def captureImage():
    #img, depth = sim.render(img_width, img_height, camera_name=sim.model.camera_names[0], depth=True)
    # 480x640 np array
    depth = np.loadtxt("depth_image_rendered.npy").astype(np.float32)

    flipped_depth = np.flip(depth, axis=0)
    real_depth = depthimg2Meters(flipped_depth)
    return real_depth

# https://github.com/htung0101/table_dome/blob/master/table_dome_calib/utils.py#L160
def depthimg2Meters(depth):
    # sim.model.stat.extent = 1.6842802984193577
    # sim.model.vis.map.znear = 0.1
    # sim.model.vis.map.zfar = 12.0
    extent = 1.6842802984193577
    near = 0.1 * extent
    far = 12. * extent
    image = near / (1 - depth * (1 - near / far))
    return image

if __name__ == '__main__':
    generatePointCloud()

The image I rendered and immediately saved is available here and shown here . There's a plane 0.5 meters from the camera, and a robot arm joint sticking straight up in the center of the frame.

The maximum real depth z-value is 0.5, so I believe the conversion to depth is correct. The x and y values are ~100 and should be ~0.1, even when I include the depth_scale=1.0 in the create_from_depth_image function. I had tried scaling the x and y values in the cloud down by 1,000 manually using:

def scaleCloudXY(cloud):
    xy_scaler = np.array([1/1000., 1/1000., 1.])

    np_cloud = np.asarray(cloud.points)
    scaled_np_cloud = np_cloud*xy_scaler
    scaled_cloud = o3d.geometry.PointCloud()
    scaled_cloud.points = o3d.utility.Vector3dVector(scaled_np_cloud)
    return scaled_cloud

The clouds looked better, but still aren't correct, especially from other angles. Here's another example of a depth image of a door returned from MuJoCo's render function, with a rendered color image and depth image .

What am I doing wrong? Is there an issue with the camera matrix or scale? The depth image returned from mujoco's render function is 480x640, but the dimensions of the o3d depth image are 640x480.

Edit: I tried using

cam_mat = o3d.camera.PinholeCameraIntrinsic(o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)

instead, and the point clouds look much better. ( Before and after ). Is there an error in my focal length calculation? 525 and 525 seem to work well for fx and fy, but my values are 1300 and 1732.

Answer 1

The focal length equation I was using was wrong. The correct camera matrix can be calculated with:

# sim.model.cam_fovy[0] = 60
fovy = math.radians(60)
f = img_height / (2 * math.tan(fovy / 2))
cx = img_width/2
cy = img_height/2
cam_mat = o3d.camera.PinholeCameraIntrinsic(img_width, img_height, f, f, cx, cy)

A class to render MuJoCo depth images and generate Open3D point clouds is available here .