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

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

最大实际深度 z 值为 0.5，因此我相信深度转换是正确的。 x 和 y 值约为 100，应该约为 0.1，即使我在 create_from_depth_image 函数中包含 height_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

云看起来好多了，但仍然不正确，尤其是从其他角度看。这是从 MuJoCo 的渲染函数返回的门的 深度图像 的另一个示例，其中包含渲染的 和 。

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

编辑：我尝试使用

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

相反，点云看起来好多了。 （和）。我的焦距计算有错误吗？ 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)