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

309 lines
15 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""点云感知:从胸口相机的 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
产出:<capture>/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()