Download algorithms/phase_recording.py from LinJinwei/XY-VLA: direct link, hf CLI and curl.
- Browser
- Download file 12.4 kB
-
https://huggingface.co/LinJinwei/XY-VLA/resolve/main/algorithms/phase_recording.py
- Command line
-
hf download hf://LinJinwei/XY-VLA/algorithms/phase_recording.py
-
curl -L -o phase_recording.py https://huggingface.co/LinJinwei/XY-VLA/resolve/main/algorithms/phase_recording.py
12.4 kB
| from __future__ import annotations | |
| import datetime | |
| import os | |
| from pathlib import Path | |
| from typing import Any, Callable | |
| import numpy as np | |
| from .portal_geometry import portal_number_billboard_center_and_size | |
| PHASE1_DIR = "phase1" | |
| PHASE2_DIR = "phase2" | |
| PHASE3_DIR = "phase3" | |
| TOPDOWN_PNG = "topdown.png" | |
| STEREO45_PNG = "stereo45deg.png" | |
| def recording_experiment_folder_name(dt: datetime.datetime | None = None) -> str: | |
| dt = dt or datetime.datetime.now() | |
| time_sep = "-" if os.name == "nt" else ":" | |
| return ( | |
| f"{dt.year}_{dt.month}_{dt.day}_" | |
| f"{dt.hour:02d}{time_sep}{dt.minute:02d}{time_sep}{dt.second:02d}" | |
| ) | |
| _RECORDING_PREFER_OPENGL = False | |
| _RECORDING_RENDERER_LOGGED = False | |
| def set_recording_prefer_opengl(enabled: bool) -> None: | |
| global _RECORDING_PREFER_OPENGL, _RECORDING_RENDERER_LOGGED | |
| _RECORDING_PREFER_OPENGL = bool(enabled) | |
| _RECORDING_RENDERER_LOGGED = False | |
| def pybullet_connection_is_gui(p: Any) -> bool: | |
| try: | |
| info = p.getConnectionInfo() | |
| return int(info.get("connectionMethod", -1)) == int(p.GUI) | |
| except Exception: | |
| return False | |
| def resolve_pybullet_camera_renderer(p: Any, *, prefer_opengl: bool = False) -> int: | |
| if prefer_opengl and pybullet_connection_is_gui(p): | |
| return int(p.ER_BULLET_HARDWARE_OPENGL) | |
| return int(p.ER_TINY_RENDERER) | |
| def configure_pybullet_gui_opengl_transparency(p: Any) -> None: | |
| if not pybullet_connection_is_gui(p): | |
| return | |
| try: | |
| p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) | |
| p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) | |
| p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) | |
| p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0) | |
| p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) | |
| p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) | |
| except Exception: | |
| pass | |
| def render_camera_rgb( | |
| p: Any, | |
| view: list[float], | |
| proj: list[float], | |
| *, | |
| width: int, | |
| height: int, | |
| prefer_opengl: bool | None = None, | |
| ) -> np.ndarray: | |
| global _RECORDING_RENDERER_LOGGED | |
| use_gl = _RECORDING_PREFER_OPENGL if prefer_opengl is None else bool(prefer_opengl) | |
| renderer = resolve_pybullet_camera_renderer(p, prefer_opengl=use_gl) | |
| if not _RECORDING_RENDERER_LOGGED: | |
| tag = "ER_BULLET_HARDWARE_OPENGL" if renderer == int(p.ER_BULLET_HARDWARE_OPENGL) else "ER_TINY_RENDERER" | |
| print(f"[render] PyBullet camera renderer: {tag} (GUI alpha={'on' if tag.endswith('OPENGL') else 'off'})") | |
| _RECORDING_RENDERER_LOGGED = True | |
| try: | |
| _, _, rgba, _, _ = p.getCameraImage( | |
| width=int(width), | |
| height=int(height), | |
| viewMatrix=view, | |
| projectionMatrix=proj, | |
| renderer=renderer, | |
| ) | |
| except Exception: | |
| _, _, rgba, _, _ = p.getCameraImage( | |
| width=int(width), | |
| height=int(height), | |
| viewMatrix=view, | |
| projectionMatrix=proj, | |
| renderer=int(p.ER_TINY_RENDERER), | |
| ) | |
| return np.reshape(rgba, (int(height), int(width), 4))[..., :3].astype(np.uint8) | |
| def resolve_experiment_base_folder( | |
| *, | |
| rec_folder: Any | None = None, | |
| recording_folder: Any | None = None, | |
| root_dir: Any | None = None, | |
| ) -> Path: | |
| if rec_folder is not None: | |
| return Path(rec_folder) | |
| if recording_folder is not None: | |
| return Path(recording_folder) | |
| ts = recording_experiment_folder_name() | |
| root = Path(root_dir) if root_dir is not None else Path.cwd() | |
| return root / "recordings" / ts | |
| def resolve_phase_folder( | |
| experiment_base: Path | str, | |
| phase: str, | |
| *, | |
| leg_subfolder: str | None = None, | |
| mkdir: bool = True, | |
| ) -> Path: | |
| folder = Path(experiment_base) / phase | |
| if leg_subfolder: | |
| folder = folder / leg_subfolder | |
| if mkdir: | |
| folder.mkdir(parents=True, exist_ok=True) | |
| return folder | |
| _PORTAL_LABEL_OVERLAY_BG_BGR = (220, 236, 250) | |
| def draw_portal_number_labels_overlay_bgr( | |
| img_bgr: np.ndarray, | |
| wproj: Callable[[float, float, float], tuple[int, int] | None], | |
| placed_cubes: list[dict], | |
| ) -> None: | |
| import cv2 | |
| if not placed_cubes: | |
| return | |
| h_img, w_img = int(img_bgr.shape[0]), int(img_bgr.shape[1]) | |
| font = cv2.FONT_HERSHEY_SIMPLEX | |
| for c in placed_cubes: | |
| if c.get("shape") != "rect_frame" or c.get("portal_label") is None: | |
| continue | |
| num = int(c["portal_label"]) | |
| center, side_m = portal_number_billboard_center_and_size(c) | |
| cx, cy, cz = float(center[0]), float(center[1]), float(center[2]) | |
| pc = wproj(cx, cy, cz) | |
| if pc is None: | |
| continue | |
| pe = wproj(cx + 0.5 * side_m, cy, cz) | |
| pn = wproj(cx, cy + 0.5 * side_m, cz) | |
| if pe is not None and pn is not None: | |
| r_px = int( | |
| max( | |
| 10, | |
| min(140, abs(pe[0] - pc[0]), abs(pn[1] - pc[1])), | |
| ) | |
| ) | |
| elif pe is not None: | |
| r_px = int(max(10, min(140, abs(pe[0] - pc[0])))) | |
| else: | |
| r_px = 18 | |
| x0 = int(np.clip(pc[0] - r_px, 0, w_img - 1)) | |
| y0 = int(np.clip(pc[1] - r_px, 0, h_img - 1)) | |
| x1 = int(np.clip(pc[0] + r_px, 0, w_img - 1)) | |
| y1 = int(np.clip(pc[1] + r_px, 0, h_img - 1)) | |
| if x1 <= x0 or y1 <= y0: | |
| continue | |
| cv2.rectangle( | |
| img_bgr, | |
| (x0, y0), | |
| (x1, y1), | |
| _PORTAL_LABEL_OVERLAY_BG_BGR, | |
| thickness=-1, | |
| lineType=cv2.LINE_8, | |
| ) | |
| cv2.rectangle( | |
| img_bgr, | |
| (x0, y0), | |
| (x1, y1), | |
| (0, 0, 0), | |
| thickness=max(1, r_px // 24), | |
| lineType=cv2.LINE_8, | |
| ) | |
| text = str(num) | |
| scale = float(np.clip(r_px / 22.0, 0.45, 2.8)) | |
| thickness = max(2, int(round(scale * 2.2))) | |
| (tw, th), _bl = cv2.getTextSize(text, font, scale, thickness) | |
| org = (pc[0] - tw // 2, pc[1] + th // 2) | |
| cv2.putText( | |
| img_bgr, | |
| text, | |
| org, | |
| font, | |
| scale, | |
| (0, 0, 0), | |
| thickness, | |
| cv2.LINE_8, | |
| ) | |
| def sharpen_topdown_portal_labels_rgb( | |
| rgb: np.ndarray, | |
| placed_cubes: list[dict], | |
| wproj: Callable[[float, float, float], tuple[int, int] | None], | |
| ) -> np.ndarray: | |
| import cv2 | |
| bgr = cv2.cvtColor(np.asarray(rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR) | |
| draw_portal_number_labels_overlay_bgr(bgr, wproj, placed_cubes) | |
| return cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB) | |
| def save_bgr_views( | |
| output_folder: Path, | |
| topdown_bgr: np.ndarray, | |
| stereo_bgr: np.ndarray | None, | |
| *, | |
| topdown_name: str = TOPDOWN_PNG, | |
| stereo_name: str = STEREO45_PNG, | |
| ) -> dict[str, str]: | |
| import cv2 | |
| output_folder.mkdir(parents=True, exist_ok=True) | |
| saved: dict[str, str] = {} | |
| top_path = output_folder / topdown_name | |
| cv2.imwrite(str(top_path), topdown_bgr) | |
| saved["topdown"] = str(top_path) | |
| if stereo_bgr is not None: | |
| st_path = output_folder / stereo_name | |
| cv2.imwrite(str(st_path), stereo_bgr) | |
| saved["stereo45deg"] = str(st_path) | |
| return saved | |
| def render_phase_scene_bgr_views( | |
| p: Any, | |
| *, | |
| scene_lo: np.ndarray, | |
| scene_hi: np.ndarray, | |
| world_recording_view_proj_fn: Callable[..., tuple[list[float], list[float]]], | |
| render_world_recording_rgb_fn: Callable[..., np.ndarray], | |
| world_xyz_to_recording_image_pixel_fn: Callable[ | |
| [np.ndarray, list[float], list[float], int, int], tuple[int, int] | None | |
| ], | |
| recording_scene_fov: float, | |
| recording_scene_margin: float, | |
| recording_camera_distance_scale: float, | |
| recording_topview_distance_scale: float, | |
| recording_stereo45_distance_scale: float = 1.5, | |
| width: int = 800, | |
| height: int = 800, | |
| placed_cubes: list[dict] | None = None, | |
| ) -> tuple[np.ndarray, np.ndarray, Callable[[float, float, float], tuple[int, int] | None], Callable[[float, float, float], tuple[int, int] | None]]: | |
| import cv2 | |
| lo = np.asarray(scene_lo, dtype=np.float32) | |
| hi = np.asarray(scene_hi, dtype=np.float32) | |
| fov = float(recording_scene_fov) | |
| margin = float(recording_scene_margin) | |
| dist_scale = float(recording_camera_distance_scale) | |
| top_dist_scale = float(recording_topview_distance_scale) | |
| stereo45_dist_scale = float(recording_stereo45_distance_scale) | |
| tw, th = int(width), int(height) | |
| view_m, proj_m = world_recording_view_proj_fn( | |
| p, | |
| lo, | |
| hi, | |
| view_kind="top", | |
| width=tw, | |
| height=th, | |
| fov=fov, | |
| margin=margin, | |
| distance_scale=dist_scale, | |
| top_view_distance_scale=top_dist_scale, | |
| ) | |
| top_rgb = render_world_recording_rgb_fn(p, view_m, proj_m, width=tw, height=th) | |
| top_bgr = cv2.cvtColor(top_rgb, cv2.COLOR_RGB2BGR) | |
| view_st, proj_st = world_recording_view_proj_fn( | |
| p, | |
| lo, | |
| hi, | |
| view_kind="45deg", | |
| width=tw, | |
| height=th, | |
| fov=fov, | |
| margin=margin, | |
| distance_scale=dist_scale, | |
| top_view_distance_scale=top_dist_scale, | |
| stereo45_view_distance_scale=stereo45_dist_scale, | |
| ) | |
| stereo_rgb = render_world_recording_rgb_fn(p, view_st, proj_st, width=tw, height=th) | |
| stereo_bgr = cv2.cvtColor(stereo_rgb, cv2.COLOR_RGB2BGR) | |
| def wproj_top(wx: float, wy: float, wz: float) -> tuple[int, int] | None: | |
| return world_xyz_to_recording_image_pixel_fn( | |
| np.array([wx, wy, wz], dtype=np.float64), | |
| view_m, | |
| proj_m, | |
| width=tw, | |
| height=th, | |
| ) | |
| def wproj_stereo(wx: float, wy: float, wz: float) -> tuple[int, int] | None: | |
| return world_xyz_to_recording_image_pixel_fn( | |
| np.array([wx, wy, wz], dtype=np.float64), | |
| view_st, | |
| proj_st, | |
| width=tw, | |
| height=th, | |
| ) | |
| return top_bgr, stereo_bgr, wproj_top, wproj_stereo | |
| def render_and_save_phase_scene( | |
| p: Any, | |
| output_folder: Path, | |
| *, | |
| scene_lo: np.ndarray, | |
| scene_hi: np.ndarray, | |
| world_recording_view_proj_fn: Callable[..., tuple[list[float], list[float]]], | |
| render_world_recording_rgb_fn: Callable[..., np.ndarray], | |
| world_xyz_to_recording_image_pixel_fn: Callable[ | |
| [np.ndarray, list[float], list[float], int, int], tuple[int, int] | None | |
| ] | None = None, | |
| recording_scene_fov: float, | |
| recording_scene_margin: float, | |
| recording_camera_distance_scale: float, | |
| recording_topview_distance_scale: float, | |
| recording_stereo45_distance_scale: float = 1.5, | |
| width: int = 800, | |
| height: int = 800, | |
| phase_label: str = "Phase", | |
| overlay_fn: Callable[ | |
| [np.ndarray, Callable[[float, float, float], tuple[int, int] | None]], | |
| None, | |
| ] | |
| | None = None, | |
| placed_cubes: list[dict] | None = None, | |
| ) -> dict[str, str]: | |
| if world_xyz_to_recording_image_pixel_fn is None: | |
| raise ValueError("world_xyz_to_recording_image_pixel_fn is required") | |
| top_bgr, stereo_bgr, wproj_top, wproj_stereo = render_phase_scene_bgr_views( | |
| p, | |
| scene_lo=scene_lo, | |
| scene_hi=scene_hi, | |
| world_recording_view_proj_fn=world_recording_view_proj_fn, | |
| render_world_recording_rgb_fn=render_world_recording_rgb_fn, | |
| world_xyz_to_recording_image_pixel_fn=world_xyz_to_recording_image_pixel_fn, | |
| recording_scene_fov=recording_scene_fov, | |
| recording_scene_margin=recording_scene_margin, | |
| recording_camera_distance_scale=recording_camera_distance_scale, | |
| recording_topview_distance_scale=recording_topview_distance_scale, | |
| recording_stereo45_distance_scale=recording_stereo45_distance_scale, | |
| width=width, | |
| height=height, | |
| placed_cubes=placed_cubes, | |
| ) | |
| if overlay_fn is not None: | |
| overlay_fn(top_bgr, wproj_top) | |
| overlay_fn(stereo_bgr, wproj_stereo) | |
| if placed_cubes: | |
| draw_portal_number_labels_overlay_bgr(top_bgr, wproj_top, placed_cubes) | |
| saved = save_bgr_views(output_folder, top_bgr, stereo_bgr) | |
| print(f"[{phase_label}] top-down view saved: {saved.get('topdown')}") | |
| if "stereo45deg" in saved: | |
| print(f"[{phase_label}] stereo 45deg view saved: {saved['stereo45deg']}") | |
| return saved | |