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

如何解决将 Mujoco 深度图像转换为 Open3D 点云

我正在尝试从 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,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 并显示为 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

云看起来更好，但仍然不正确，尤其是从其他角度看。这是从 MuJoCo 的渲染函数返回的门的 depth image 的另一个示例，带有渲染的 color image 和 depth image。

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

编辑：我尝试使用

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

相反，点云看起来好多了。（Before 和 after）。我的焦距计算有错误吗？ 525 和 525 似乎适用于 fx 和 fy，但我的值为 1300 和 1732。

解决方法

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

# 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,cx,cy)

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