XY-VLA / algorithms /phase_recording.py
LinJinwei's picture
Upload 52 files
d2f1e32 verified
Raw History Blame Contribute Delete
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