Files
a7l20-hug-sim/scripts/capture_chest_rgbd.py

74 lines
3.8 KiB
Python

"""把胸口相机在指定姿态下的 RGB-D 按 HUG 输入格式落盘:rgb.png / depth.png(uint16, mm) / intrinsics.txt / camera.json
用法:MUJOCO_GL=glfw .venv/bin/python scripts/capture_chest_rgbd.py [--pose ready] [--out data/capture_ready] [--w 848 --h 480]
camera.json 记录 OpenCV 约定(+z 前、+y 下)的相机→世界外参,供 HUG 输出的腕位姿换到世界系。
"""
import argparse, json, os
os.environ.setdefault("MUJOCO_GL", "glfw"); os.environ.setdefault("DISPLAY", ":1")
import os
from pathlib import Path
import mujoco, numpy as np, cv2
ROOT = Path(__file__).resolve().parents[1]
SCENE = os.environ.get("A7L20_SCENE", "a7l20_scene")
def set_pose(m, d, info, pose):
mujoco.mj_resetData(m, d)
for side in ("left", "right"):
for jn, an, q in zip(info["arms"][side]["joints"], info["arms"][side]["actuators"], pose["arm_qpos"][side]):
d.qpos[m.jnt_qposadr[m.joint(jn).id]] = q; d.ctrl[m.actuator(an).id] = q
for an in info["hands"][side]["actuators"]:
d.ctrl[m.actuator(an).id] = pose.get("hand_ctrl", 0.0)
mujoco.mj_forward(m, d)
def intrinsics(m, cam_name, w, h):
fovy = float(m.cam_fovy[m.camera(cam_name).id])
fy = (h / 2) / np.tan(np.deg2rad(fovy) / 2); fx = fy
return np.array([[fx, 0, w / 2], [0, fy, h / 2], [0, 0, 1]])
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--pose", default="ready"); ap.add_argument("--out", default="data/capture_ready")
ap.add_argument("--w", type=int, default=848); ap.add_argument("--h", type=int, default=480)
ap.add_argument("--settle", type=int, default=500)
ap.add_argument("--qpos", default=None, help="用给定 .npy 的 qpos 作为当前状态(实时演示用),不再沉降")
a = ap.parse_args()
m = mujoco.MjModel.from_xml_path(str(ROOT / f"scene/{SCENE}.xml")); d = mujoco.MjData(m)
info = json.loads((ROOT / f"scene/{SCENE}.json").read_text())
if a.qpos:
d.qpos[:] = np.load(a.qpos); mujoco.mj_forward(m, d)
else:
set_pose(m, d, info, info[a.pose])
for _ in range(a.settle): mujoco.mj_step(m, d)
r = mujoco.Renderer(m, a.h, a.w)
r.update_scene(d, camera="chest_cam"); rgb = r.render().copy()
r.enable_depth_rendering(); r.update_scene(d, camera="chest_cam"); dep = r.render().copy(); r.disable_depth_rendering()
dep_mm = np.where(np.isfinite(dep) & (dep < 65.0), np.round(dep * 1000.0), 0).astype(np.uint16)
K = intrinsics(m, "chest_cam", a.w, a.h)
cid = m.camera("chest_cam").id
R_mj = d.cam_xmat[cid].reshape(3, 3); t = d.cam_xpos[cid].copy()
R_cv = R_mj @ np.diag([1.0, -1.0, -1.0]) # MuJoCo 相机 -z 看/+y 上 → OpenCV +z 看/+y 下
T_world_cam = np.eye(4); T_world_cam[:3, :3] = R_cv; T_world_cam[:3, 3] = t
out = ROOT / a.out; out.mkdir(parents=True, exist_ok=True)
cv2.imwrite(str(out / "rgb.png"), cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR))
cv2.imwrite(str(out / "depth.png"), dep_mm)
(out / "intrinsics.txt").write_text(f"{K[0,0]:.4f} {K[1,1]:.4f} {K[0,2]:.4f} {K[1,2]:.4f}\n")
objs = {}
for o in info["objects"]:
p = d.xpos[m.body(o["body"]).id]; pc = R_cv.T @ (p - t)
uv = K @ pc; uv = uv[:2] / uv[2]
objs[o["name"]] = {"world": p.round(4).tolist(), "cam": pc.round(4).tolist(), "uv": uv.round(1).tolist()}
meta = {"pose": a.pose, "width": a.w, "height": a.h, "K": K.tolist(), "T_world_cam_opencv": T_world_cam.tolist(),
"cam_pos_world": t.tolist(), "depth_unit": "mm(uint16)", "objects": objs}
(out / "camera.json").write_text(json.dumps(meta, indent=2))
v = dep_mm[dep_mm > 0]
print(f"wrote {out}: rgb {rgb.shape}, depth mm min/median/max = {v.min()}/{int(np.median(v))}/{v.max()}, K fx={K[0,0]:.1f} cx={K[0,2]:.1f} cy={K[1,2]:.1f}")
for k, o in objs.items(): print(f" {k:7s} uv={o['uv']} cam_z={o['cam'][2]:.3f}")
if __name__ == "__main__":
main()