Files
a7l20-hug-sim/scripts/view_pointcloud.py
T

116 lines
5.1 KiB
Python

"""把胸口相机(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()