#!/usr/bin/env python3 """依頼140：寄りのカメラごとに、比べるための数を出す（見た目の好みではない）。 python3 vis140.py <回のフォルダ> <出力.csv> 隠れの判定：カメラの位置から点までの線分が、動かない物の箱（look_static.csv）のうち「部屋の箱・壁」 （group rooms の全部・waiting の WallBack*/WallWest・world の高さ 0.5 m 超えのもの）を通るかで決める。 長いす・カウンター・座った人・人・ロボットは隠すものに入れない。画面の外（0≤u≤w・0≤v≤h の外）も「写っていない」に数える。 1 見える廊下の割合：x9〜31・z27〜31 の大廊下の床（床の上の面＝高さ 0）を 0.1 m のマスに切り、マスの中心が写っている割合 2 見える輪の数：静止画の時刻（8862 秒）の前 10 秒（8852〜8862 秒）に出ている輪（events.csv）の中心（床の上）が写っている数／全部 3 ロボットが見える割合：8845〜8945 秒の 0.2 秒ごとのコマで、白い円柱の上の面の中心が写っているコマの割合 （上の面の 8 つの縁の点も数え、すべて写っているコマの割合も出す。画面の中にいたコマ数と、画面の中で箱に隠れたコマ数も出す） 4 人の見分け：範囲の真ん中 (20, 29) に立つカプセル（体：直径 0.4・高さ 1.59）が、パネル付きの絵（左 1360×1080）で何画素の高さか （カプセルの表面の点を画面に移したときの縦の幅）。あわせて足元から頭の上（1.59 m）の縦の線の長さ（画素） """ import csv, json, math, os, sys import numpy as np FLOOR_Y = 0.0 # 床の上の面（look_static.csv の Floor01〜04 の max_y） ROBOT_H = 1.081 ROBOT_D = 0.549 STILL_T, SHOW_S = 8862.0, 10.0 CX, CZ = 20.0, 29.0 SW, SH = 1360, 1080 class Cam: def __init__(self, path): j = json.load(open(path, encoding="utf-8")) self.name, self.w, self.h = j["name"], j["width"], j["height"] self.V, self.P = np.array(j["worldToCameraMatrix"]), np.array(j["projectionMatrix"]) self.pos = np.array(j["position_unity"]) self.spec = j["h139Cam"].split(",") def uv(self, pts): pts = np.atleast_2d(pts) P4 = np.c_[pts, np.ones(len(pts))] q = (self.P @ (self.V @ P4.T)).T w = q[:, 3] u = (q[:, 0] / w + 1) / 2 * self.w v = (1 - q[:, 1] / w) / 2 * self.h return u, v, w def inframe(self, pts): u, v, w = self.uv(pts) return (w > 1e-6) & (u >= 0) & (u <= self.w) & (v >= 0) & (v <= self.h) def load_boxes(run): path = os.path.join(run, "look139", "look_static.csv") lines = open(path, encoding="utf-8").read().splitlines() head = dict(kv.split("=") for kv in lines[0][2:].split()) boxes, kinds = [], {"rooms": 0, "waiting_wall": 0, "world": 0} for r in csv.DictReader(lines[1:]): mn = [float(r["min_x"]), float(r["min_y"]), float(r["min_z"])] mx = [float(r["max_x"]), float(r["max_y"]), float(r["max_z"])] name = r["path"].split("/")[-1] g = r["group"] if g == "rooms": k = "rooms" elif g == "waiting" and name.startswith(("WallBack", "WallWest")): k = "waiting_wall" elif g == "world" and mx[1] - mn[1] > 0.5: k = "world" else: continue if r["axis_aligned"] != "1": raise SystemExit("軸にそろっていない箱があります: " + r["path"]) boxes.append(mn + mx) kinds[k] += 1 return np.array(boxes), kinds, float(head["robot_base_y"]), float(head["walker_base_y"]) def visible(cam, boxes, pts): """pts (N,3) の点がカメラから見えるか（画面の中・箱に隠れていない）""" pts = np.atleast_2d(pts).astype(float) ok = cam.inframe(pts) o = cam.pos d = pts - o # (N,3) with np.errstate(divide="ignore", invalid="ignore"): inv = 1.0 / d bmn, bmx = boxes[:, :3], boxes[:, 3:] hid = np.zeros(len(pts), bool) for i in range(0, len(pts), 2000): dd = inv[i:i + 2000, None, :] # (n,1,3) t1 = (bmn[None] - o) * dd t2 = (bmx[None] - o) * dd t1 = np.where(np.isnan(t1), -np.inf, t1); t2 = np.where(np.isnan(t2), np.inf, t2) tmin = np.max(np.minimum(t1, t2), axis=2) tmax = np.min(np.maximum(t1, t2), axis=2) hit = (tmax >= tmin) & (tmax > 1e-6) & (tmin < 1 - 1e-6) hid[i:i + 2000] = hit.any(axis=1) return ok & ~hid def capsule_px(cam, base_y): pts = [] r, h = 0.2, 1.59 for a in np.linspace(0, 2 * math.pi, 48, endpoint=False): for y in np.linspace(r, h - r, 12): pts.append((CX + r * math.cos(a), base_y + y, CZ + r * math.sin(a))) for b in np.linspace(0, math.pi / 2, 8): pts.append((CX + r * math.cos(b) * math.cos(a), base_y + h - r + r * math.sin(b), CZ + r * math.cos(b) * math.sin(a))) pts.append((CX + r * math.cos(b) * math.cos(a), base_y + r - r * math.sin(b), CZ + r * math.cos(b) * math.sin(a))) u, v, _ = cam.uv(np.array(pts)) sy = SH / cam.h ext = (v.max() - v.min()) * sy _, va, _ = cam.uv(np.array([(CX, base_y, CZ), (CX, base_y + h, CZ)])) return ext, abs(va[0] - va[1]) * sy def main(): run, out = sys.argv[1], sys.argv[2] boxes, kinds, rby, wby = load_boxes(run) ev = [e for e in csv.DictReader(open(os.path.join(run, "events.csv"), encoding="utf-8")) if STILL_T - SHOW_S - 1e-6 <= float(e["t_s"]) <= STILL_T + 1e-6] frames = list(csv.DictReader(open(os.path.join(run, "look139", "look_frames.csv"), encoding="utf-8"))) names = sorted({r["cam"] for r in frames if r["cam"].startswith("n_")}) xs = np.arange(9.05, 31.0, 0.1); zs = np.arange(27.05, 31.0, 0.1) gx, gz = np.meshgrid(xs, zs) floor = np.c_[gx.ravel(), np.full(gx.size, FLOOR_Y), gz.ravel()] rows = [] for n in names: cam = Cam(os.path.join(run, "look139", "cam_%s.json" % n)) a = visible(cam, boxes, floor) fr_in = cam.inframe(floor) ring_pts = np.array([(float(e["x"]), FLOOR_Y, float(e["z"])) for e in ev]) rv = visible(cam, boxes, ring_pts) vf = [r for r in frames if r["kind"] == "vid" and r["cam"] == n] top = rby + ROBOT_H c_pts = np.array([(float(r["robot_x"]), top, float(r["robot_z"])) for r in vf]) c_vis = visible(cam, boxes, c_pts) c_in = cam.inframe(c_pts) rim_all = np.ones(len(vf), bool) for k in range(8): ang = k * math.pi / 4 p = c_pts + np.array([ROBOT_D / 2 * math.cos(ang), 0, ROBOT_D / 2 * math.sin(ang)]) rim_all &= visible(cam, boxes, p) ext, axis = capsule_px(cam, wby) yaw, pitch, tx, tz, dist = (float(v) for v in cam.spec[1:6]) rows.append({ "cam": n, "yaw": yaw, "pitch": pitch, "tx": tx, "tz": tz, "dist": dist, "cam_h": round(float(cam.pos[1]), 2), "cam_x": round(float(cam.pos[0]), 2), "cam_z": round(float(cam.pos[2]), 2), "hall_visible_pct": round(100 * float(a.mean()), 1), "hall_inframe_pct": round(100 * float(fr_in.mean()), 1), "rings_visible": int(rv.sum()), "rings_total": len(ev), "rings_hidden_ids": " ".join(e["event_id"] for e, v in zip(ev, rv) if not v), "robot_top_center_visible_pct": round(100 * float(c_vis.mean()), 1), "robot_top_all_visible_pct": round(100 * float(rim_all.mean()), 1), "robot_frames": len(vf), "robot_inframe_frames": int(c_in.sum()), "robot_hidden_inframe_frames": int((c_in & ~c_vis).sum()), "person_px": round(float(ext), 1), "person_axis_px": round(float(axis), 1), }) with open(out, "w", encoding="utf-8", newline="") as f: w = csv.DictWriter(f, fieldnames=list(rows[0].keys())) w.writeheader(); w.writerows(rows) print("隠すもの:", kinds, "robot_base_y", rby, "walker_base_y", wby, "輪", len(ev)) for r in rows: print(r) if __name__ == "__main__": main()