Files
worldmodel/plans/PRISM/14_mvp.md
T

538 lines
19 KiB
Markdown
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.
# Chapter 14 — 最小可复现 demo (MVP)
> 本章目标:把 PRISM 浓缩成 **5 个核心步骤**,让任何具备基础工程能力的开发者,**用一个周末** 就能跑通"iPhone 扫描 → ZED 重定位 → 实时感知 → 一次记忆事件 → 简单查询"的全闭环。
---
## 14.1 MVP 的边界
MVP **是什么**
- ✅ 1 个房间
- ✅ iPhone + ZED 2i + 一台带 GPU 的笔记本(或 Jetson
- ✅ 跑通"建图 → 重定位 → 在线感知 → 记一条变化 → LLM 查询"
- ✅ 全部依赖均开源或可白嫖
MVP **不是什么**
- ❌ 不要求 30 Hz 稳定
- ❌ 不要求多房间
- ❌ 不要求 Consolidate(手工触发即可)
- ❌ 不要求 3DGS(用 mesh 足够)
- ❌ 不要求 ROS 2(可用纯 Python
---
## 14.2 准备清单(30 分钟)
```yaml
硬件:
- iPhone Pro (12 Pro 起,建议 15 Pro Max)
- ZED 2i (借一台也行)
- 一台 GPU 笔记本/台式(RTX 3060+ 即可)
- 30 cm × 30 cm 打印的 ArUco DICT_5X5_100 id=42
- 一卷胶带(贴 ArUco 到地面)
软件:
- Python 3.10 + conda env
- PyTorch 2.3 (CUDA 12)
- 装包:
pip install pyzed open3d open_clip_torch ultralytics \
trimesh networkx opencv-contrib-python click
iPhone App:
- Polycam (App Store 免费版即可) 或
- 3D Scanner App (导出 USDZ 模式)
场地:
- 一个 ~15 m² 的房间,光线均匀,无大面镜子
- 床/桌/椅 至少 3 件家具
```
---
## 14.3 Step 1iPhone 扫描(15 分钟)
```
1. 把 ArUco 贴在房间地面正中(朝向不重要,记住即可)
2. 打开 Polycam,选 "Room (LiDAR)" 模式
3. 从门口开始顺时针环绕一圈,速度 0.3 m/s
4. 重点区域回扫(床、桌、椅)
5. 结束并导出:选 "USDZ"
6. AirDrop / scp 到笔记本:保存为 mvp/scan/room.usdz
```
同时拍一张能看见 ArUco 的 RGB(用 iPhone 标准相机),保存为 `mvp/scan/aruco_ref.jpg`
> **没有 RoomPlan App 也没关系**Polycam 导出 USDZ 即可。我们退化版的解析器(见 14.4)从 USDZ 直接读 mesh,按高度切层得到地面 polygon。
---
## 14.4 Step 2Ingest 到 SpatialMemory20 分钟)
```python
# mvp/ingest.py
"""
极简版 ingest:从 Polycam 导出的 USDZ 读 mesh
按 z=0~1.5m 切片做 2D 房间 polygon
用 YOLO-World 离线检测家具;
对齐 ArUco 得到 T_iphone→map。
"""
import open3d as o3d
import numpy as np
import cv2, json, uuid
from pathlib import Path
from PIL import Image
import torch, open_clip
from ultralytics import YOLO
SCAN_DIR = Path("mvp/scan")
OUT_DIR = Path("mvp/ltm")
OUT_DIR.mkdir(exist_ok=True, parents=True)
# 1) 读 USDZ → meshtrimesh 能解 usdz;或先 unzip
import trimesh
mesh = trimesh.load(SCAN_DIR/"room.usdz", force="mesh")
print(f"mesh: {len(mesh.vertices)} verts, {len(mesh.faces)} faces")
mesh.export(OUT_DIR/"room.glb")
# 2) ArUco 对齐
img = cv2.imread(str(SCAN_DIR/"aruco_ref.jpg"))
aruco = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_100)
corners, ids, _ = cv2.aruco.detectMarkers(img, aruco)
assert ids is not None and 42 in ids.flatten(), "ArUco 42 not found!"
# 用 iPhone EXIF 内参做 solvePnP(精度无所谓,MVP 只要不歪)
fx = fy = 1500.0; cx, cy = img.shape[1]/2, img.shape[0]/2
K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]])
obj = np.array([[0,0,0],[0.3,0,0],[0.3,0.3,0],[0,0.3,0]], dtype=np.float32)
i = list(ids.flatten()).index(42)
ok, rvec, tvec = cv2.solvePnP(obj, corners[i][0], K, None)
R_cm = cv2.Rodrigues(rvec)[0]
T_cam_to_map = np.eye(4); T_cam_to_map[:3,:3]=R_cm; T_cam_to_map[:3,3]=tvec.flatten()
# Polycam 没给 ARKit pose,简化:把 T_iphone→map 直接当 T_cam_to_map
T_iphone_to_map = T_cam_to_map
mesh.apply_transform(T_iphone_to_map)
mesh.export(OUT_DIR/"room_aligned.glb")
# 3) 切层得到房间 polygonz 0.050.15m 上的占用 → footprint
pts = mesh.sample(200_000)
floor_mask = (pts[:,2] > 0.05) & (pts[:,2] < 0.15)
xy = pts[floor_mask][:, :2]
# 用 alpha-shape 或 convex hull 都行
from scipy.spatial import ConvexHull
hull = ConvexHull(xy)
polygon = xy[hull.vertices]
print("room polygon vertices:", len(polygon))
# 4) YOLO-World 在 mesh 渲染图上检家具
import pyrender
scene = pyrender.Scene()
scene.add(pyrender.Mesh.from_trimesh(mesh))
cam = pyrender.PerspectiveCamera(yfov=np.pi/3)
# 在房间上方 2.5 m 多视角渲染
furniture = []
yolo = YOLO("yolov8x-worldv2.pt")
yolo.set_classes(["bed","chair","desk","sofa","table","tv",
"lamp","wardrobe","bathtub","toilet"])
for yaw in np.linspace(0, 2*np.pi, 8, endpoint=False):
cam_pose = np.eye(4)
cam_pose[:3,3] = [np.cos(yaw)*0.1, np.sin(yaw)*0.1, 1.6]
cam_pose[:3,:3] = look_at_yaw(yaw)
scene.add(cam, pose=cam_pose)
r = pyrender.OffscreenRenderer(640, 480)
color, depth = r.render(scene)
for det in yolo.predict(color, verbose=False)[0].boxes:
u,v,w,h = det.xywh[0].cpu().numpy()
# 用 depth 把 2D 中心反投到 3D
z = depth[int(v), int(u)]
if z <= 0: continue
x = (u - 320) * z / 525
y = (v - 240) * z / 525
center = (cam_pose @ np.array([x,y,z,1]))[:3]
furniture.append({
"uid": f"{yolo.names[int(det.cls)]}_{uuid.uuid4().hex[:6]}",
"label": yolo.names[int(det.cls)],
"center": center.tolist(),
"conf": float(det.conf)})
scene.clear_camera_nodes(); r.delete()
# 5) CLIP 房间向量
clip_model, _, prep = open_clip.create_model_and_transforms("ViT-B-32")
clip_model.eval().cuda()
room_rgb = Image.open(SCAN_DIR/"aruco_ref.jpg")
with torch.no_grad():
room_feat = clip_model.encode_image(
prep(room_rgb).unsqueeze(0).cuda()).cpu().numpy()[0]
room_feat /= np.linalg.norm(room_feat)
# 6) 落盘
mem = {
"schema_version": "1.0.0",
"world_frame": "map",
"nodes": {
"room_001": {
"uid":"room_001", "label":"Room", "level":"L3",
"source":"iphone", "polygon_2d": polygon.tolist(),
"clip_embedding": room_feat.tolist(),
"confidence": 0.9}
},
"anchors": [],
"edges": []
}
for f in furniture:
uid = f["uid"]
mem["nodes"][uid] = {
"uid": uid, "label": f["label"], "level":"L4",
"source":"iphone", "category":"furniture",
"pose": {"position": f["center"], "quaternion":[1,0,0,0]},
"confidence": f["conf"], "parent_room":"room_001",
"attributes":{"mobile": f["label"] in ("chair",)},
}
mem["edges"].append({"src_uid":"room_001","dst_uid":uid,
"relation":"contains"})
if not mem["nodes"][uid]["attributes"]["mobile"]:
mem["anchors"].append({"anchor_uid": uid, "node_label": f["label"]})
with open(OUT_DIR/"spatial_memory.json","w") as f:
json.dump(mem, f, indent=2)
print(f"✅ LTM written: {len(mem['nodes'])} nodes, "
f"{len(mem['anchors'])} anchors")
```
运行:
```bash
python mvp/ingest.py
```
期望输出:`✅ LTM written: 510 nodes, 2+ anchors`
---
## 14.5 Step 3ZED 重定位(30 分钟)
```python
# mvp/relocalize.py
"""极简两段式重定位"""
import pyzed.sl as sl, numpy as np, open3d as o3d, json
import open_clip, torch
from PIL import Image
mem = json.load(open("mvp/ltm/spatial_memory.json"))
room_feat = np.array(mem["nodes"]["room_001"]["clip_embedding"])
room_feat /= np.linalg.norm(room_feat)
mesh = o3d.io.read_triangle_mesh("mvp/ltm/room_aligned.glb")
tgt_pc = mesh.sample_points_uniformly(80_000).voxel_down_sample(0.05)
tgt_pc.estimate_normals()
# ZED 启动
cam = sl.Camera()
init = sl.InitParameters()
init.depth_mode = sl.DEPTH_MODE.QUALITY
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP
cam.open(init)
rt = sl.RuntimeParameters()
img_mat = sl.Mat(); depth_mat = sl.Mat(); pc_mat = sl.Mat()
# 抓 1 帧
assert cam.grab(rt) == sl.ERROR_CODE.SUCCESS
cam.retrieve_image(img_mat, sl.VIEW.LEFT)
cam.retrieve_measure(pc_mat, sl.MEASURE.XYZRGBA)
rgb = img_mat.get_data()[:,:,:3]
pc_xyz = pc_mat.get_data()[:,:,:3].reshape(-1, 3)
pc_xyz = pc_xyz[~np.isnan(pc_xyz).any(axis=1)]
# Stage 1: CLIP 验证(MVP 只有 1 个房间,跳过 Top-K)
clip_model, _, prep = open_clip.create_model_and_transforms("ViT-B-32")
clip_model.eval().cuda()
with torch.no_grad():
f = clip_model.encode_image(
prep(Image.fromarray(rgb)).unsqueeze(0).cuda()).cpu().numpy()[0]
f /= np.linalg.norm(f)
sim = float(np.dot(f, room_feat))
print(f"[Stage1] CLIP sim={sim:.3f}", "" if sim>0.3 else "")
# Stage 2: ICP
src = o3d.geometry.PointCloud()
src.points = o3d.utility.Vector3dVector(pc_xyz)
src = src.voxel_down_sample(0.05); src.estimate_normals()
src_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
src, o3d.geometry.KDTreeSearchParamHybrid(0.25, 100))
tgt_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
tgt_pc, o3d.geometry.KDTreeSearchParamHybrid(0.25, 100))
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
src, tgt_pc, src_fpfh, tgt_fpfh, True, 0.075,
o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
4, [o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(0.075)],
o3d.pipelines.registration.RANSACConvergenceCriteria(100_000, 0.999))
icp = o3d.pipelines.registration.registration_icp(
src, tgt_pc, 0.02, result.transformation,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(f"[Stage2] ICP fitness={icp.fitness:.3f}, RMSE={icp.inlier_rmse:.4f}")
np.save("mvp/T_zed_to_map.npy", icp.transformation)
print(f"✅ T_zed→map saved")
cam.close()
```
运行:
```bash
python mvp/relocalize.py
```
期望:`fitness > 0.5`RMSE < 5 cm。若不达标,多试几次(换站位)。
---
## 14.6 Step 4:在线感知 + 一次差异事件(30 分钟)
```python
# mvp/online.py
"""跑 60 秒:YOLO 检测家具 → 与 LTM 关联 → 把"新物"写 delta"""
import pyzed.sl as sl, numpy as np, json, time
from ultralytics import YOLO
from pathlib import Path
T = np.load("mvp/T_zed_to_map.npy")
mem = json.load(open("mvp/ltm/spatial_memory.json"))
delta_path = Path("mvp/delta/pending.jsonl"); delta_path.parent.mkdir(exist_ok=True)
yolo = YOLO("yolov8x-worldv2.pt")
all_labels = list(set(n["label"] for n in mem["nodes"].values()
if n["level"]=="L4")) + ["cup","book","backpack"]
yolo.set_classes(all_labels)
cam = sl.Camera()
init = sl.InitParameters()
init.depth_mode = sl.DEPTH_MODE.QUALITY
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP
cam.open(init)
cam.enable_positional_tracking(sl.PositionalTrackingParameters())
rt = sl.RuntimeParameters()
img_mat = sl.Mat(); pose = sl.Pose(); xyz_mat = sl.Mat()
t0 = time.time()
events = {}
while time.time() - t0 < 60:
if cam.grab(rt) != sl.ERROR_CODE.SUCCESS: continue
cam.retrieve_image(img_mat, sl.VIEW.LEFT)
cam.retrieve_measure(xyz_mat, sl.MEASURE.XYZRGBA)
cam.get_position(pose, sl.REFERENCE_FRAME.WORLD)
rgb = img_mat.get_data()[:,:,:3].copy()
# 当前 ZED 帧位姿 → map
T_robot_zed = pose.pose_data().numpy()
T_robot_map = T @ T_robot_zed
for det in yolo.predict(rgb, conf=0.4, verbose=False)[0].boxes:
label = yolo.names[int(det.cls)]
u,v,w,h = det.xywh[0].cpu().numpy()
z = xyz_mat.get_data()[int(v), int(u), 2]
if np.isnan(z) or z <= 0: continue
# 简化:把像素中心投回 3D 后乘 T_robot_map
# (这里跳过相机内参,使用 ZED 的 XYZ 直接读)
xyz_cam = xyz_mat.get_data()[int(v), int(u), :3]
if np.isnan(xyz_cam).any(): continue
xyz_map = (T_robot_map @ np.array([*xyz_cam, 1]))[:3]
# 找 LTM 中同 label 最近节点
cand = [n for n in mem["nodes"].values()
if n.get("label")==label and n["level"]=="L4"]
if not cand:
# 新物
sig = f"new|{label}|{tuple(np.round(xyz_map/0.3).astype(int))}"
ev = events.setdefault(sig, dict(event_type="object_added",
label=label, pos=xyz_map.tolist(),
count=0))
ev["count"] += 1
else:
nearest = min(cand, key=lambda n: np.linalg.norm(
np.array(n["pose"]["position"]) - xyz_map))
d = np.linalg.norm(np.array(nearest["pose"]["position"]) - xyz_map)
if d > 0.3: # 偏移 > 30 cm,记 moved
sig = f"moved|{nearest['uid']}"
ev = events.setdefault(sig, dict(event_type="object_moved",
target=nearest["uid"],
new_pos=xyz_map.tolist(),
count=0))
ev["count"] += 1
cam.close()
# 落盘累计观测 ≥ 3 的事件
with open(delta_path, "w") as f:
for sig, ev in events.items():
if ev["count"] >= 3:
f.write(json.dumps(ev) + "\n")
print(f"✅ wrote {sum(1 for e in events.values() if e['count']>=3)} delta events")
```
测试方法:**故意拿一个新物(如水杯)放到桌上**,跑 `online.py`,应看到至少 1 条 `object_added`
---
## 14.7 Step 5LLM 查询(10 分钟)
```python
# mvp/query.py
"""用 OpenAI / Qwen 调 PRISM API"""
import json
from openai import OpenAI
mem = json.load(open("mvp/ltm/spatial_memory.json"))
delta = [json.loads(l) for l in open("mvp/delta/pending.jsonl")]
# 把 LTM 序列化成自然语言
def describe_memory():
desc = ["The robot has memorized the following objects in this room:"]
for n in mem["nodes"].values():
if n["level"] == "L4":
p = n["pose"]["position"]
desc.append(f"- {n['label']} at ({p[0]:.2f}, {p[1]:.2f}, {p[2]:.2f})")
if delta:
desc.append("\nRecent changes detected:")
for ev in delta:
desc.append(f"- {ev['event_type']}: {json.dumps(ev)}")
return "\n".join(desc)
client = OpenAI() # 也可换 Qwen 的 base_url
prompt = f"""You are a service robot's memory assistant.
{describe_memory()}
User asks: "桌上现在有什么不在原始扫描里的东西?"
Answer in Chinese, concise."""
resp = client.chat.completions.create(
model="gpt-4o-mini",
messages=[{"role":"user","content": prompt}])
print(resp.choices[0].message.content)
```
期望输出:`检测到一个新增的物体"cup"…`
---
## 14.8 一键运行脚本
```bash
# mvp/run_all.sh
#!/usr/bin/env bash
set -e
echo "== Step 2: ingest =="
python mvp/ingest.py
echo "== Step 3: relocalize =="
python mvp/relocalize.py
echo "== Step 4: online 60s =="
python mvp/online.py
echo "== Step 5: query =="
python mvp/query.py
echo "✅ MVP done. See mvp/ltm/ and mvp/delta/"
```
---
## 14.9 验收清单
| 阶段 | 通过标准 |
|------|----------|
| Step 1 扫描 | `mvp/scan/room.usdz` 文件存在,> 5 MB |
| Step 2 ingest | `spatial_memory.json` 含 ≥ 5 个 L4 节点 |
| Step 3 relocalize | `T_zed_to_map.npy` 存在,fitness > 0.5 |
| Step 4 online | `delta/pending.jsonl` 至少 1 条 event |
| Step 5 query | LLM 给出含"cup"或新物名称的回答 |
跑通这 5 步 → **PRISM 核心闭环验证完成**
---
## 14.10 从 MVP 到生产的下一步
MVP 跑通后,按 [`09_roadmap.md`](09_roadmap.md) 的 W2 起逐步升级:
| MVP 简化 | 生产版本对应章节 |
|----------|------------------|
| 单房间 | [`04_pipeline_A_iphone_offline.md`](04_pipeline_A_iphone_offline.md) §4.2.3 多 session 拼接 |
| 无 RoomPlan | 替换 ingest 为 RoomPlan 解析(§4.5 |
| 单帧重定位 | [`05_pipeline_B_relocalization.md`](05_pipeline_B_relocalization.md) §5.7 在线维护 |
| YOLO 简单关联 | [`06_pipeline_C_online_perception.md`](06_pipeline_C_online_perception.md) §6.5 hybrid associate |
| 无 TSDF | §6.4 Open3D VoxelBlockGrid 实时融合 |
| 无差异检测 | §6.6 TSDF vs LTM 几何差异 |
| 无巩固 | [`07_pipeline_D_consolidation.md`](07_pipeline_D_consolidation.md) 完整 6 步 |
| 无 ROS 2 | [`10_tech_stack.md`](10_tech_stack.md) §10.5 整套部署 |
| 无评测 | [`13_evaluation.md`](13_evaluation.md) 指标 + 自动化 |
---
## 14.11 常见踩坑 (FAQ)
**Q1: ZED grab 一直失败?**
A: 检查 USB 3.0USB-C 必须是 3.x,2.0 会带宽不足),换条线试。
**Q2: ICP fitness 永远 < 0.3**
A: 检查:
- ZED 当前视野是否真的在房间里(不是看着门外)
- mesh 是否已 `apply_transform(T_iphone_to_map)`
- 单位是否都是米(USDZ 可能是 cm)
**Q3: ArUco 检不到?**
A: 打印尺寸要正好 30 cm(不是 A4 缩放);侧光避免反光;正面拍摄。
**Q4: YOLO 没检出任何家具?**
A: `yolo.set_classes(...)` 必须在 `predict` 之前;conf 阈值降到 0.25 再试;图像分辨率 ≥ 640。
**Q5: Polycam 导出的 USDZ trimesh 打不开?**
A: 用 `unzip room.usdz -d room/`,找里面的 `.usdc/.usda`,再用 `pxr` 读;或在 Polycam 里改导出为 `.glb`
**Q6: GPU 内存不够?**
A: ZED 用 720pYOLO 换 `yolov8s-worldv2.pt`small);CLIP 用 `laion/CLIP-ViT-B-16-laion2B-s34B-b88K`(更省)。
**Q7: 跑 60 s `online.py` 没出任何 delta**
A: 检查 `T_zed_to_map.npy` 是否正确加载;手动在 ZED 视野里走过,并确认新物(杯子)有连续 3 帧被检出。
---
## 14.12 把 MVP 录成 demo 视频
强烈建议跑完后录 < 3 min 视频:
```
0:000:30 iPhone 扫房间(手持移动)
0:300:45 AirDrop 文件到笔记本
0:451:15 python ingest.py(看 LTM 节点输出)
1:151:45 把 ZED 放进房间,跑 relocalize.py
1:45–2:15 放一个杯子到桌上,跑 online.py60 s
2:152:45 跑 query.py,看 LLM 中文给出"杯子是新出现的"
2:453:00 打开 spatial_memory.json + delta/pending.jsonl 截图
```
这段视频就是你拿给老板 / 合作方 / 审稿人看的 PRISM **第一版 proof-of-concept**
---
## 14.13 本章小结
| MVP 的 5 步 | 一句话 |
|------------|--------|
| **Step 1** | iPhone + Polycam 扫一个房间,导 USDZ |
| **Step 2** | 解析 USDZ + ArUco 对齐 → spatial_memory.json |
| **Step 3** | ZED 拍当前帧 → CLIP + ICP → T_zed_to_map |
| **Step 4** | YOLO 跑 60 s → 检测到新物,写 delta |
| **Step 5** | LLM 读 LTM + delta → 自然语言回答 |
**MVP 用时**:一个工程师 12 个工作日。
**硬件成本**iPhone (借) + ZED 2i (借) + 自有 GPU 笔记本,**04 k 元**。
读完本章你应能:
- ✅ 今天就开工
- ✅ 周五前给团队演示 PRISM 核心闭环
- ✅ 评估"是否值得投入 8 周做完整原型"
---
**章节版本**v1.0
**估计阅读时间**15 分钟(不含动手时间)
**关键收获**:把 14 章理论压缩成"一个周末 + 5 个 Python 脚本"
---
> 🌈 **跑完 MVP 的那一刻,你就拥有了 PRISM v0.01——一个会用 iPhone 当教科书、用 ZED 当眼睛、还会自己记笔记的小机器人大脑。剩下 8 周路线图([`09_roadmap.md`](09_roadmap.md))就是把它从"小学生"变成"上岗员工"。**