point-clouds - Mujoco Depth Image を Open3D Point Cloud に変換

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 であるため、深さへの変換は正しいと思います。create_from_depth_image 関数に depth_scale=1.0 を含めた場合でも、x と y の値は ~100 であり、~0.1 である必要があります。以下を使用して、雲の 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 の render 関数から返される深度画像は 480x640 ですが、o3d 深度画像のサイズは 640x480 です。

編集：使ってみました

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

代わりに、点群ははるかに良く見えます。（前後）。_ 焦点距離の計算に誤りはありますか? 525 と 525 は fx と fy に適しているようですが、私の値は 1300 と 1732 です。