基类
import cv2
import numpy as np
from scipy.spatial.transform import Rotation as R
import open3d as o3d
import math
class ImageToPointCloud:
def __init__(self, height, width, fovy):
self._width = width
self._height = height
self._fovy = fovy
def get_point_cloud(self, rgb, depth):
fovy = math.radians(self._fovy)
f = self._height / (2 * math.tan(fovy / 2))
cam_mat = np.array(((f, 0, self._width / 2), (0, f, self._height / 2), (0, 0, 1)))
cx = cam_mat[0,2]
fx = cam_mat[0,0]
cy = cam_mat[1,2]
fy = cam_mat[1,1]
od_cammat = o3d.camera.PinholeCameraIntrinsic(self._width, self._height, fx, fy, cx, cy)
od_depth = o3d.geometry.Image(depth)
od_rgb = o3d.geometry.Image(rgb)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
od_rgb, od_depth, depth_scale=1.0, depth_trunc=100.0, convert_rgb_to_intensity=False
)
# 生成点云（带颜色）
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, od_cammat)
return pcd
使用
def _get_depth_image(self, camera_id, d):
if self._depth_render is None:
renderer = mujoco.Renderer(self._m, self._img_height, self._img_width)
renderer.enable_depth_rendering()
self._depth_render = renderer
else:
renderer = self._depth_render
renderer.enable_depth_rendering()
renderer.update_scene(d, camera=camera_id)
depth = renderer.render()
return depth
m = mujoco.MjModel.from_xml_path(config["xml_path"])
d = mujoco.MjData(m)
m.opt.timestep = config['simulation_dt']
self._camera_id = m.camera(camera_name).id
self._point_cloud = ImageToPointCloud(self._img_height, self._img_width, m.cam_fovy[self._camera_id])
rgb_img = self._get_rgb_image(camera_id, d)
depth_img = self._get_depth_image(camera_id, d)
cv2.namedWindow("H1 Cameras", cv2.WINDOW_AUTOSIZE | cv2.WINDOW_GUI_NORMAL)
cv2.moveWindow("H1 Cameras", 1100, 0)
cv2.imshow("H1 Cameras", rgb_img)
cv2.namedWindow("Depth", cv2.WINDOW_AUTOSIZE | cv2.WINDOW_GUI_NORMAL)
cv2.moveWindow("Depth", 0, 0)
cv2.imshow("Depth", depth_img)
cv2.waitKey(1)
pcd = self._point_cloud.get_point_cloud(rgb_img, depth_img)
xml
<worldbody>
<body name="pelvis" pos="-20 -3 0.793">
<camera name="depth_camera" pos="0.1 0 0.3" euler="1.5708 -1.5708 0" mode="fixed" fovy="110"/>
</body>
</worldbody>