"""把胸口相机(D405)采集的 RGB-D 反投影成世界点云并渲染成图(无需 open3d/matplotlib,纯 numpy+cv2)。 左:相机原生视角的彩色点云(≈D405 看到的);中:斜视角彩色点云(看 3D 结构);右:去桌面后按感知检测结果上色 + click 点。 用法:A7L20_SCENE=ycb1 .venv/bin/python scripts/view_pointcloud.py --capture data/capture_pcd_ycb1 [--out out/pointcloud.png] """ import argparse, json, os from pathlib import Path import numpy as np, cv2 ROOT = Path(__file__).resolve().parents[1] SCENE = os.environ.get("A7L20_SCENE", "a7l20_scene") DET_COLORS = [(60, 76, 231), (219, 152, 52), (94, 185, 90), (43, 180, 231), (182, 89, 155), (96, 174, 39)] # BGR def backproject(cap): dep = cv2.imread(str(cap / "depth.png"), cv2.IMREAD_UNCHANGED).astype(np.float64) / 1000.0 rgb = cv2.cvtColor(cv2.imread(str(cap / "rgb.png")), cv2.COLOR_BGR2RGB) fx, fy, cx, cy = map(float, (cap / "intrinsics.txt").read_text().split()) cam = json.loads((cap / "camera.json").read_text()) Twc = np.array(cam["T_world_cam_opencv"]); cam_pos = np.array(cam.get("cam_pos_world", Twc[:3, 3])) h, w = dep.shape us, vs = np.meshgrid(np.arange(w), np.arange(h)); m = (dep > 0) & (dep < 2.0) z = dep[m]; x = (us[m] - cx) / fx * z; y = (vs[m] - cy) / fy * z Pc = np.stack([x, y, z], 1); Pw = Pc @ Twc[:3, :3].T + Twc[:3, 3] col = rgb[m] # RGB per point return Pw, col, cam_pos def voxel(P, C, extra=None, v=0.004): key = np.floor(P / v).astype(np.int64) _, idx = np.unique(key, axis=0, return_index=True) idx = np.sort(idx) return (P[idx], C[idx], None if extra is None else extra[idx]) def render(P, C, eye, target, W=560, H=620, focal=900, pt=2, bg=(24, 26, 32)): """把世界点 P(Nx3)、颜色 C(Nx3 RGB) 从虚拟相机(eye→target, up=+z)投影,远→近画成彩色点。返回 BGR 图。""" eye = np.asarray(eye, float); target = np.asarray(target, float) f = target - eye; f /= np.linalg.norm(f) r = np.cross(f, [0, 0, 1.0]); r /= np.linalg.norm(r) u = np.cross(r, f) d = P - eye zc = d @ f; xc = d @ r; yc = -(d @ u) ok = zc > 0.05 zc, xc, yc, Cc = zc[ok], xc[ok], yc[ok], C[ok] upx = (focal * xc / zc + W / 2).astype(int); vpx = (focal * yc / zc + H / 2).astype(int) img = np.full((H, W, 3), bg, np.uint8) order = np.argsort(-zc) # 远的先画(画家算法) for i in order: x0, y0 = upx[i], vpx[i] if 0 <= x0 < W and 0 <= y0 < H: cv2.circle(img, (x0, y0), pt, (int(Cc[i][2]), int(Cc[i][1]), int(Cc[i][0])), -1) return img, (eye, f, r, u, focal, W, H) def project_pt(p, cam): eye, f, r, u, focal, W, H = cam d = np.asarray(p) - eye; zc = d @ f if zc <= 0.05: return None return int(focal * (d @ r) / zc + W / 2), int(focal * (-(d @ u)) / zc + H / 2) def label(img, s, xy=(10, 24), c=(235, 235, 235)): cv2.putText(img, s, xy, cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 0), 3, cv2.LINE_AA) cv2.putText(img, s, xy, cv2.FONT_HERSHEY_SIMPLEX, 0.55, c, 1, cv2.LINE_AA) def main(): ap = argparse.ArgumentParser() ap.add_argument("--capture", required=True); ap.add_argument("--out", default=None) a = ap.parse_args() cap = Path(a.capture).expanduser().resolve() Pw, C, cam_pos = backproject(cap) Pw, C, _ = voxel(Pw, C) perc = json.loads((cap / "perception.json").read_text()) if (cap / "perception.json").exists() else {"objects": {}} tgt = np.array([0.40, 0.0, 0.80]) # 1) 相机原生视角(≈D405 直接看到的彩色点云) img_cam, C1 = render(Pw, C, cam_pos, tgt, focal=560) label(img_cam, f"D405 camera view ({len(Pw)} pts)") # 2) 斜视角(看 3D 结构) eye2 = tgt + np.array([-0.35, -0.75, 0.55]) img_obl, C2 = render(Pw, C, eye2, tgt) label(img_obl, "oblique view") # 3) 去桌面后按检测结果上色(+ click 点) table = 0.755 keep = Pw[:, 2] > table + 0.008 Po, Co = Pw[keep], np.full((int(keep.sum()), 3), (90, 90, 90), np.uint8) # 给每个检测物体的邻域点染色 for k, (nm, o) in enumerate(perc["objects"].items()): c = np.array(o["world"]); col = DET_COLORS[k % len(DET_COLORS)] near = np.linalg.norm(Po[:, :2] - c[:2], axis=1) < 0.06 Co[near] = (col[2], col[1], col[0]) # 存 RGB img_det, C3 = render(Po, Co, eye2, tgt) for k, (nm, o) in enumerate(perc["objects"].items()): col = DET_COLORS[k % len(DET_COLORS)] p = project_pt(o.get("click_world", o["world"]), C3) if p: cv2.drawMarker(img_det, p, col, cv2.MARKER_CROSS, 16, 2) label(img_det, nm, (min(p[0] + 8, 400), max(p[1], 14)), col[::-1]) label(img_det, f"perception: {len(perc['objects'])} objs, click=+") gap = np.full((img_cam.shape[0], 8, 3), 255, np.uint8) out_img = np.concatenate([img_cam, gap, img_obl, gap, img_det], 1) out = Path(a.out) if a.out else ROOT / f"out/pointcloud_{SCENE}.png" out.parent.mkdir(exist_ok=True) cv2.imwrite(str(out), out_img) print(f"wrote {out} ({out_img.shape[1]}x{out_img.shape[0]})") if __name__ == "__main__": main()