309 lines
15 KiB
Python
309 lines
15 KiB
Python
"""点云感知:从胸口相机的 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()
|