"""点云感知:从胸口相机的 RGB-D 采集出发,识别桌上物体并估计其世界位姿,投影成像素 uv 供 HUG 使用, 替换掉此前直接从仿真真值投影的 uv。移植自 DexGraspBench 的 perceive_object.py(08-27),核心算法保持一致: 深度反投影 → 【自体滤除】 → 工作区/桌面裁剪 → 体素降采样 → DBSCAN 聚类 → 每簇对每个模板做平面 SE(2) ICP(x/y/yaw,12 个 yaw 初值) → 内点率 × 覆盖率(仅计面向相机的可见模板点)判类 → 贪心互斥分配到各物体 与 DexGraspBench 的差异:① 本项目是单胸口相机 + OpenCV 约定外参(非它的双相机 + MuJoCo 约定), 反投影用自己的 OpenCV 版;② 一次认出场景中全部物体(它只返回单个目标);③ 无 sklearn, DBSCAN 用 scipy cKDTree 自己实现;④ 模板由 build_templates.py 从场景 mesh 预先采好(带法线)。 ⑤ 增加自体点云滤除(--self-filter),移除机器人双臂双手的点(真机部署必需)。 诚实边界(同 DexGraspBench):是"已知类模板匹配"(分类),非开放集检测;用 home 沉降稳定面当先验, 物体翻到别的稳定面会检测失败(按失败处理,不给错答案);仿真理想深度,无传感器噪声模型。 用法: 仿真:A7L20_SCENE=ycb1 .venv/bin/python scripts/perceive_object.py --capture data/live_ycb1 [--eval] [--verbose] 真机:同上 + --self-filter --arm-joints-file data/real_align/arm_joints.json 产出:/perception.json """ import argparse, json, os, sys from pathlib import Path import numpy as np, cv2 from scipy.spatial import cKDTree # 自体滤除 sys.path.insert(0, str(Path(__file__).resolve().parent)) try: from self_filter import SelfFilter except ImportError: SelfFilter = None ROOT = Path(__file__).resolve().parents[1] SCENE = os.environ.get("A7L20_SCENE", "a7l20_scene") # ---- 参数(对齐 DexGraspBench,尺度按本场景微调)---- VOXEL = 0.005 # 体素降采样边长 (m) DBSCAN_EPS = 0.018; DBSCAN_MIN = 10 # 聚类 MIN_CLUSTER = 40 # 有效簇最少点数(降采样后) ICP_YAW_INITS = 12; ICP_ITERS = 25 ICP_INLIER_D = 0.008 # 内点判定距离 (m):观测点到模板最近点 ICP_COVER_D = 0.012 # 覆盖判定距离 (m):可见模板点到观测点 MIN_INLIER = 0.50; MIN_COVER = 0.35 SIZE_GATE_DZ = 0.05; SIZE_GATE_DDIAG = 0.10 # 尺寸预筛:z 高度差 / 包围盒对角线差超过则跳过 GRASP_CLICK_FRAC = 0.45 # click 取物体中部高度的近表面点(而非投影中心):高瓶避免 click 打顶→抓太高滑脱 ROI_X = (0.28, 0.58); ROI_Y = (-0.40, 0.40) # 工作区 xy(排除躯干/侧放的双臂) TABLE_MARGIN = 0.006; ROI_Z_TOP = 0.45 # 桌面以上 margin 起算,到桌上 0.45 m def load_capture(cap: Path): dep = cv2.imread(str(cap / "depth.png"), cv2.IMREAD_UNCHANGED).astype(np.float64) / 1000.0 fx, fy, cx, cy = map(float, (cap / "intrinsics.txt").read_text().split()) cam = json.loads((cap / "camera.json").read_text()) K = np.array(cam["K"]); T_wc = np.array(cam["T_world_cam_opencv"]) cam_pos = np.array(cam.get("cam_pos_world", T_wc[:3, 3])) return dep, (fx, fy, cx, cy), K, T_wc, cam_pos, cam def unproject(depth, intr, T_wc): """OpenCV 约定(+z 前、+x 右、+y 下),深度是相机 z-depth(m)。返回世界系 Nx3。""" fx, fy, cx, cy = intr h, w = depth.shape us, vs = np.meshgrid(np.arange(w), np.arange(h)) m = depth > 0 z = depth[m]; x = (us[m] - cx) / fx * z; y = (vs[m] - cy) / fy * z Pc = np.stack([x, y, z], 1) return Pc @ T_wc[:3, :3].T + T_wc[:3, 3] def voxel_down(pts, voxel=VOXEL): if len(pts) == 0: return pts key = np.floor(pts / voxel).astype(np.int64) _, idx = np.unique(key, axis=0, return_index=True) return pts[np.sort(idx)] def dbscan(pts, eps=DBSCAN_EPS, min_samples=DBSCAN_MIN): """cKDTree 实现的 DBSCAN(无 sklearn 依赖)。返回每点簇标签,-1 为噪声。""" n = len(pts) labels = -np.ones(n, int) if n == 0: return labels tree = cKDTree(pts) neigh = tree.query_ball_point(pts, eps) # 含自身 visited = np.zeros(n, bool); cid = 0 for i in range(n): if visited[i]: continue visited[i] = True if len(neigh[i]) < min_samples: continue # 暂判噪声(后续可能被并入某簇为边界点) labels[i] = cid; seeds = list(neigh[i]); k = 0 while k < len(seeds): j = seeds[k]; k += 1 if labels[j] == -1: labels[j] = cid if not visited[j]: visited[j] = True if len(neigh[j]) >= min_samples: seeds.extend(neigh[j]) cid += 1 return labels class Template: def __init__(self, name, meta, npz): d = np.load(npz) self.name = name self.pts = d["pts"].astype(np.float64) # xy 归心到 body 原点、z 为世界高度 self.nrm = d["nrm"].astype(np.float64) self.body_z = float(meta["body_z"]) self.z_extent = float(meta["z_extent"]); self.diag = float(meta["diag"]) self.tree = cKDTree(self.pts) def _kabsch2d(A, B): """求把源点 A(Nx2) 刚性映射到目标 B(Nx2) 的 2D 旋转角 yaw 和平移 t:B ≈ Rz(yaw)@A + t。""" Ac = A - A.mean(0); Bc = B - B.mean(0) num = float(np.sum(Ac[:, 0] * Bc[:, 1] - Ac[:, 1] * Bc[:, 0])) den = float(np.sum(Ac[:, 0] * Bc[:, 0] + Ac[:, 1] * Bc[:, 1])) yaw = np.arctan2(num, den) c, s = np.cos(yaw), np.sin(yaw) R = np.array([[c, -s], [s, c]]) t = B.mean(0) - R @ A.mean(0) return yaw, t def icp_planar(scene, tmpl: Template, cam_pos, yaw_inits=ICP_YAW_INITS, iters=ICP_ITERS): """平面 SE(2) ICP:把模板配到观测簇 scene(Nx3, 世界系)。返回 (x, y, yaw, inlier, coverage)。 位姿 (x,y,yaw):模板点 p 映射到世界 = Rz(yaw)@p + [x,y,0](z 不动,物体躺桌上)。""" Sxy = scene[:, :2]; Sz = scene[:, 2:3] c0 = Sxy.mean(0) scene_tree = cKDTree(scene) best = None for ki in range(yaw_inits): yaw = 2 * np.pi * ki / yaw_inits t = c0.copy() # 模板质心(≈0)对到簇质心 for _ in range(iters): c, s = np.cos(yaw), np.sin(yaw) Rz = np.array([[c, -s], [s, c]]) scene_local_xy = (Sxy - t) @ Rz # = Rz^T @ (Sxy - t):观测点转到模板系 scene_local = np.concatenate([scene_local_xy, Sz], 1) dist, idx = tmpl.tree.query(scene_local) Pnn = tmpl.pts[idx] # 匹配到的模板点(模板系) new_yaw, new_t = _kabsch2d(Pnn[:, :2], Sxy) # 模板点 → 世界观测点 if abs(new_yaw - yaw) < 1e-5 and np.linalg.norm(new_t - t) < 1e-5: yaw, t = new_yaw, new_t; break yaw, t = new_yaw, new_t # 指标 c, s = np.cos(yaw), np.sin(yaw); Rz = np.array([[c, -s], [s, c]]) scene_local = np.concatenate([(Sxy - t) @ Rz, Sz], 1) dist, _ = tmpl.tree.query(scene_local) inlier = float(np.mean(dist < ICP_INLIER_D)) # 覆盖率:模板点映射到世界,取面向相机的可见点,看有多少落在观测点附近 Pw_xy = tmpl.pts[:, :2] @ Rz.T + t # Rz@p_xy + t Pw = np.concatenate([Pw_xy, tmpl.pts[:, 2:3]], 1) Nw_xy = tmpl.nrm[:, :2] @ Rz.T # 法线只转 yaw Nw = np.concatenate([Nw_xy, tmpl.nrm[:, 2:3]], 1) vis = np.sum(Nw * (cam_pos - Pw), 1) > 0 # 法线朝相机 if vis.sum() < 30: coverage = 0.0 else: cd, _ = scene_tree.query(Pw[vis]) coverage = float(np.mean(cd < ICP_COVER_D)) score = inlier * coverage if best is None or score > best[-1]: best = (float(t[0]), float(t[1]), float(yaw % (2 * np.pi)), inlier, coverage, score) return best[:5] def detect_all(pts_ds, templates, cam_pos, verbose=False): """对降采样点云聚类 + 逐簇逐模板 ICP + 贪心互斥分配。返回检测列表(每物体一条)。""" labels = dbscan(pts_ds) cands = [] n_clusters = 0 for cl in range(labels.max() + 1 if labels.max() >= 0 else 0): S = pts_ds[labels == cl] if len(S) < MIN_CLUSTER: continue n_clusters += 1 cz = float(np.ptp(S[:, 2])); cdiag = float(np.linalg.norm(S.max(0) - S.min(0))) for tmpl in templates: if abs(cz - tmpl.z_extent) > SIZE_GATE_DZ or abs(cdiag - tmpl.diag) > SIZE_GATE_DDIAG: continue x, y, yaw, inlier, cover = icp_planar(S, tmpl, cam_pos) if verbose: print(f" cluster{cl}({len(S)}pt,h{cz:.3f},d{cdiag:.3f}) vs {tmpl.name:20s} " f"inlier={inlier:.2f} cover={cover:.2f} score={inlier*cover:.2f} @({x:.3f},{y:.3f},{np.rad2deg(yaw):.0f}°)") if inlier >= MIN_INLIER and cover >= MIN_COVER: cands.append({"cluster": cl, "name": tmpl.name, "x": x, "y": y, "yaw": yaw, "z": tmpl.body_z, "inlier": inlier, "coverage": cover, "score": inlier * cover, "n_pts": len(S), "pts": S}) # 贪心互斥:每簇/每模板至多用一次 cands.sort(key=lambda d: -d["score"]) used_cl, used_nm, out = set(), set(), [] for d in cands: if d["cluster"] in used_cl or d["name"] in used_nm: continue used_cl.add(d["cluster"]); used_nm.add(d["name"]); out.append(d) return out, n_clusters def pick_click(S, cam_pos, frac=GRASP_CLICK_FRAC): """从簇的观测点里挑一个 click 点:目标高度 = 底+frac·高(≈中部),在该高度带内取最靠近相机的近表面点。 这样高瓶不会 click 到顶部、矮物体也≈中心,且一定是有效表面点(深度可读)。""" z_bot, z_top = float(S[:, 2].min()), float(S[:, 2].max()) z_g = z_bot + frac * (z_top - z_bot) band = np.abs(S[:, 2] - z_g) < 0.02 C = S[band] if band.sum() >= 5 else S return C[np.argmin(np.linalg.norm(C - cam_pos, axis=1))] # 近表面(面向相机) def world_to_uv(p, K, T_wc): R, t = T_wc[:3, :3], T_wc[:3, 3] pc = R.T @ (np.asarray(p) - t) uv = K @ pc return (uv[:2] / uv[2]) def main(): ap = argparse.ArgumentParser() ap.add_argument("--capture", required=True) ap.add_argument("--eval", action="store_true", help="对照 camera.json 真值打印分类/位置误差") ap.add_argument("--verbose", action="store_true") ap.add_argument("--self-filter", action="store_true", help="启用自体点云滤除(真机部署必需)") ap.add_argument("--arm-joints-file", default=None, help="臂关节角文件(JSON,含 left/right 各 7 关节)") a = ap.parse_args() cap = Path(a.capture).expanduser().resolve() tdir = ROOT / f"data/templates_{SCENE}" tmeta = json.loads((tdir / "templates.json").read_text())["objects"] templates = [Template(nm, m, tdir / f"{nm}.npz") for nm, m in tmeta.items()] dep, intr, K, T_wc, cam_pos, cam = load_capture(cap) Pw = unproject(dep, intr, T_wc) # 自体滤除(可选) if a.self_filter: if SelfFilter is None: print("[perceive] 警告:self_filter.py 未找到,跳过自体滤除") else: # 读取臂关节角 if a.arm_joints_file: joints_data = json.loads(Path(a.arm_joints_file).read_text()) Jl = np.array(joints_data["left"]) Jr = np.array(joints_data["right"]) else: # 从 camera.json 读取(如果有) if "arm_joints" in cam: Jl = np.array(cam["arm_joints"]["left"]) Jr = np.array(cam["arm_joints"]["right"]) elif "arm_joints_left" in cam and "arm_joints_right" in cam: # 兼容旧版 realsense_capture 写出的扁平字段。 Jl = np.array(cam["arm_joints_left"]) Jr = np.array(cam["arm_joints_right"]) else: print("[perceive] 错误:--self-filter 需要提供 --arm-joints-file 或在 camera.json 中包含 arm_joints") raise SystemExit(2) sf = SelfFilter(margin=0.02) Pw_before = len(Pw) Pw = sf.filter(Pw, Jl, Jr, use_mujoco=True, verbose=a.verbose) sf.close() if a.verbose: print(f"[perceive] 自体滤除: {Pw_before} → {len(Pw)} 点") table = min(t.body_z - t.z_extent / 2 for t in templates) # 桌面 ≈ 最矮物体底 z_min = table + TABLE_MARGIN roi = ((Pw[:, 0] > ROI_X[0]) & (Pw[:, 0] < ROI_X[1]) & (Pw[:, 1] > ROI_Y[0]) & (Pw[:, 1] < ROI_Y[1]) & (Pw[:, 2] > z_min) & (Pw[:, 2] < table + ROI_Z_TOP)) P = voxel_down(Pw[roi]) print(f"[perceive] scene={SCENE} raw={len(Pw)} roi={int(roi.sum())} downsampled={len(P)} (table≈{table:.3f}, z_min={z_min:.3f})") dets, n_clusters = detect_all(P, templates, cam_pos, verbose=a.verbose) objs = {} for d in dets: world = [d["x"], d["y"], d["z"]] # 检测出的物体中心(评估精度用) click = pick_click(d["pts"], cam_pos) # 喂 HUG 的 click 点 = 中部高度近表面 uv = world_to_uv(click, K, T_wc) objs[d["name"]] = {"uv": uv.round(1).tolist(), "world": [round(v, 4) for v in world], "click_world": [round(float(v), 4) for v in click], "yaw_deg": round(np.rad2deg(d["yaw"]), 1), "inlier": round(d["inlier"], 3), "coverage": round(d["coverage"], 3), "score": round(d["score"], 3), "n_pts": d["n_pts"]} missing = [t.name for t in templates if t.name not in objs] result = {"scene": SCENE, "n_clusters": n_clusters, "objects": objs, "missing": missing} (cap / "perception.json").write_text(json.dumps(result, indent=2)) print(f"[perceive] clusters={n_clusters} detected={len(objs)}/{len(templates)} missing={missing}") for nm, o in objs.items(): print(f" {nm:22s} uv={o['uv']} world={o['world']} inlier={o['inlier']} cover={o['coverage']}") if a.eval: gt = cam.get("objects", {}) print("\n[eval] 对照真值 (仅评被检测出的物体):") ok_cls = 0 for nm, o in objs.items(): if nm in gt: gw = np.array(gt[nm]["world"]); pe = np.array(o["world"]) exy = np.linalg.norm(gw[:2] - pe[:2]) * 1000 euv = np.linalg.norm(np.array(gt[nm]["uv"]) - np.array(o["uv"])) ok_cls += 1 print(f" {nm:22s} xy误差={exy:5.1f}mm uv误差={euv:5.1f}px (真值uv={gt[nm]['uv']})") else: print(f" {nm:22s} 真值里无此名(可能是识别错类)") print(f"[eval] 识别 {len(objs)}/{len(gt)} 个真值物体,漏检 {missing}") if __name__ == "__main__": main()