mirror of
https://github.com/Tavish9/any4lerobot.git
synced 2026-05-16 14:39:41 +00:00
Jibby Nguyen
ef184e44be
* Support robocasa2lerobot * Support robocasa2lerobot * NIT: formatting * update to latest lerobot * update readme * Apply suggestion from @gemini-code-assist[bot] Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> * fix h5py open --------- Co-authored-by: Tavish <tavish9.chen@gmail.com> Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com>
79 lines
3.0 KiB
Python
79 lines
3.0 KiB
Python
import numpy as np
|
|
import robosuite.utils.transform_utils as T
|
|
|
|
|
|
def get_camera_intrinsic_matrix(sim, camera_name, camera_height, camera_width):
|
|
"""
|
|
Obtains camera intrinsic matrix.
|
|
|
|
Args:
|
|
sim (MjSim): simulator instance
|
|
camera_name (str): name of camera
|
|
camera_height (int): height of camera images in pixels
|
|
camera_width (int): width of camera images in pixels
|
|
Return:
|
|
K (np.array): 3x3 camera matrix
|
|
"""
|
|
cam_id = sim.model.camera_name2id(camera_name)
|
|
fovy = sim.model.cam_fovy[cam_id]
|
|
f = 0.5 * camera_height / np.tan(fovy * np.pi / 360)
|
|
K = np.array([[f, 0, camera_width / 2], [0, f, camera_height / 2], [0, 0, 1]])
|
|
return K
|
|
|
|
|
|
def get_camera_extrinsic_matrix(sim, camera_name):
|
|
"""
|
|
Returns a 4x4 homogenous matrix corresponding to the camera pose in the
|
|
world frame. MuJoCo has a weird convention for how it sets up the
|
|
camera body axis, so we also apply a correction so that the x and y
|
|
axis are along the camera view and the z axis points along the
|
|
viewpoint.
|
|
Normal camera convention: https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
|
|
|
|
Args:
|
|
sim (MjSim): simulator instance
|
|
camera_name (str): name of camera
|
|
Return:
|
|
R (np.array): 4x4 camera extrinsic matrix
|
|
"""
|
|
cam_id = sim.model.camera_name2id(camera_name)
|
|
camera_pos = sim.data.cam_xpos[cam_id]
|
|
camera_rot = sim.data.cam_xmat[cam_id].reshape(3, 3)
|
|
R = T.make_pose(camera_pos, camera_rot)
|
|
|
|
# IMPORTANT! This is a correction so that the camera axis is set up along the viewpoint correctly.
|
|
camera_axis_correction = np.array(
|
|
[[1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
|
|
)
|
|
R = R @ camera_axis_correction
|
|
return R
|
|
|
|
def get_camera_extrinsic_matrix_rel(sim, camera_name):
|
|
"""
|
|
Returns a 4x4 homogenous matrix corresponding to the camera pose in the
|
|
world frame. MuJoCo has a weird convention for how it sets up the
|
|
camera body axis, so we also apply a correction so that the x and y
|
|
axis are along the camera view and the z axis points along the
|
|
viewpoint.
|
|
Normal camera convention: https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
|
|
|
|
Args:
|
|
sim (MjSim): simulator instance
|
|
camera_name (str): name of camera
|
|
Return:
|
|
R (np.array): 4x4 camera extrinsic matrix
|
|
"""
|
|
cam_id = sim.model.camera_name2id(camera_name)
|
|
camera_pos = sim.model.cam_pos[cam_id]
|
|
camera_quat = sim.model.cam_quat[cam_id]
|
|
camera_rot = T.quat2mat(camera_quat)
|
|
R = T.make_pose(camera_pos, camera_rot)
|
|
|
|
# IMPORTANT! This is a correction so that the camera axis is set up along the viewpoint correctly.
|
|
camera_axis_correction = np.array(
|
|
[[1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
|
|
)
|
|
R = R @ camera_axis_correction
|
|
return R
|
|
|