将 Mujoco 深度图像转换为 Open3D 点云

Question

我正在尝试从 MuJoCo 中渲染的深度图像生成 Open3D 点云。我的代码在下面，MuJoCo 依赖项被注释掉，渲染的深度图像链接如下：

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()

我渲染并立即保存的图像可用here and shown here。距离相机0.5米处有一个平面，一个机器人手臂关节笔直地竖立在画面的中央。

最大实际深度 z 值是 0.5，所以我认为转换为深度是正确的。 x 和 y 值是 ~100，应该是 ~0.1，即使我在 create_from_depth_image 函数中包含 depth_scale=1.0 也是如此。我曾尝试使用以下方法手动将云中的 x 和 y 值缩小 1,000：

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

云看起来好多了，但仍然不正确，尤其是从其他角度看。这是 depth image of a door returned from MuJoCo's render function, with a rendered color image and depth image.

的另一个示例

我做错了什么？相机矩阵或比例尺是否有问题？ mujoco的render函数返回的深度图是480x640，但是o3d深度图的尺寸是640x480.

编辑：我尝试使用

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

相反，点云看起来好多了。 (Before and after)。我的焦距计算有误吗？ 525 和 525 似乎对 fx 和 fy 很有效，但我的值是 1300 和 1732。

Answer 1

我使用的焦距公式是错误的。正确的相机矩阵可以用以下公式计算：

# 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)

渲染 MuJoCo 深度图像和生成 Open3D 点云的 class 可用 here。

将 Mujoco 深度图像转换为 Open3D 点云

Convert Mujoco Depth Image to Open3D Point Cloud

point-clouds

depth-buffer

open3d

mujoco