chore: initial commit — import worldmodel workspace (plans/, research/)

2026-05-20 21:43:57 +08:00
commit bec8a9a4a3
98 changed files with 44128 additions and 0 deletions
@@ -0,0 +1,368 @@
+# Chapter 05 — 管线 B：ZED 2i 重定位握手
+
+> 本章目标：机器人上电后，**用 ZED 2i 当前观测把自己"安放"到 iPhone 先验地图的 `map` 坐标系里**——这是两个方案能合体的"握手时刻"。
+
+---
+
+## 5.1 为什么必须重定位
+
+ZED 2i 自己能跑 VIO，但 VIO 给出的位姿是相对于"**ZED 开机时所在的某点**"，与 iPhone `map` 帧无关。如果不做重定位：
+
+- ❌ 机器人查 LTM 时，「床」的全局坐标对它毫无意义
+- ❌ 多次开机后，每次的"原点"都不同
+- ❌ 长距离漂移无法用先验校正
+
+**重定位 = 计算一个一次性的 `T_zed→map`，把后续所有 ZED VIO 输出都左乘这个矩阵。**
+
+---
+
+## 5.2 触发条件
+
+| 触发场景 | 模式 |
+|----------|------|
+| 上电首次 | **冷启动**：无任何先验位姿 |
+| 长时间漂移（> 30 s 未匹配 anchor） | **温启动**：有粗略先验 |
+| 跟丢 / 绑架（被人抱起放下） | **绑架恢复**：可能瞬移到任意房间 |
+| 跨房间穿门 | **过门校正**（轻量）：仅刷新位姿不重头 |
+| 周期性（每 5 min） | **健康检查**：核对漂移 |
+
+冷启动与绑架恢复需要全局检索，温启动只在小范围验证。
+
+---
+
+## 5.3 两段式重定位策略：粗 → 精
+
+```mermaid
+flowchart TB
+    S1["<b>Stage 1: 粗匹配</b>（CLIP / DINO 视觉指纹）<br/>──────<br/>输入：当前 ZED RGB 帧<br/>输出：Top-K 候选房间 (L3 nodes)<br/>耗时：50–150 ms<br/>召回率目标：Top-3 > 95%"]
+    S2["<b>Stage 2: 精配准</b>（点云 ICP / TEASER++）<br/>──────<br/>输入：ZED 当前点云 vs 候选房间 mesh/anchor<br/>输出：T_zed→map 及其 fitness 分数<br/>耗时：200–800 ms<br/>位置误差目标：< 10 cm"]
+    Q{"fitness > 0.7 ?"}
+    OK(["接受，发布 TF"])
+    FB(["退到下一候选 / 走 Stage 1.b 兜底"])
+    S1 --> S2 --> Q
+    Q -- yes --> OK
+    Q -- no --> FB
+    style S1 fill:#e3f2fd,stroke:#1565c0
+    style S2 fill:#fff7d6,stroke:#c97a00
+    style OK fill:#d4f0d4,stroke:#2e7d32
+    style FB fill:#fde2e2,stroke:#a33
+```
+
+---
+
+## 5.4 Stage 1 — 视觉粗匹配
+
+### 5.4.1 CLIP 房间检索（默认方案）
+
+```python
+# spatial_memory/relocalize_coarse.py
+import open_clip, torch, numpy as np
+
+class CoarseRelocalizer:
+    def __init__(self, mem: SpatialMemory):
+        self.mem = mem
+        self.model, _, self.preprocess = open_clip.create_model_and_transforms("ViT-B-32")
+        self.model.eval().cuda()
+        # 预加载 L3 房间向量库
+        self.room_uids, self.room_feats = [], []
+        for uid, node in mem.nodes.items():
+            if node.level == "L3" and node.clip_embedding is not None:
+                self.room_uids.append(uid)
+                self.room_feats.append(node.clip_embedding)
+        self.room_feats = np.stack(self.room_feats).astype(np.float32)
+        # 归一化
+        self.room_feats /= np.linalg.norm(self.room_feats, axis=1, keepdims=True)
+
+    @torch.no_grad()
+    def __call__(self, zed_rgb: np.ndarray, top_k: int = 3) -> List[Tuple[str, float]]:
+        from PIL import Image
+        img = Image.fromarray(zed_rgb)
+        x = self.preprocess(img).unsqueeze(0).cuda()
+        feat = self.model.encode_image(x).cpu().numpy()[0]
+        feat /= np.linalg.norm(feat)
+        sims = self.room_feats @ feat                # (N,)
+        idx = np.argsort(-sims)[:top_k]
+        return [(self.room_uids[i], float(sims[i])) for i in idx]
+```
+
+### 5.4.2 增强方案：DINOv2 / NetVLAD
+
+CLIP 在"语义相似但几何不同"（如所有酒店客房都长得差不多）时会混。增强做法：
+
+```python
+# 用 DINOv2 给出更强的"场景几何指纹"
+import dinov2
+
+class HybridCoarse:
+    def __init__(self, mem):
+        self.clip_r  = CoarseRelocalizer(mem)
+        self.dino    = dinov2.load("dinov2_vitb14")
+        # 建议在 ingest 阶段同时离线提 DINO 特征存进 node.attributes['dino_emb']
+        ...
+    def __call__(self, rgb):
+        clip_top = self.clip_r(rgb, top_k=5)
+        # 再用 DINO 重排
+        dino_feat = extract_dino(self.dino, rgb)
+        reranked = []
+        for uid, _ in clip_top:
+            d = cosine(dino_feat, self.mem.nodes[uid].attributes['dino_emb'])
+            reranked.append((uid, d))
+        reranked.sort(key=lambda x: -x[1])
+        return reranked[:3]
+```
+
+### 5.4.3 投票（取连续多帧增强稳定性）
+
+```python
+def coarse_with_voting(coarse_fn, zed_stream, window=5) -> str:
+    votes = {}
+    for _ in range(window):
+        frame = zed_stream.get_rgb()
+        for uid, sim in coarse_fn(frame, top_k=3):
+            votes[uid] = votes.get(uid, 0) + sim
+    return max(votes, key=votes.get)
+```
+
+---
+
+## 5.5 Stage 2 — 几何精配准
+
+### 5.5.1 流程
+
+```python
+# spatial_memory/relocalize_fine.py
+import open3d as o3d
+import numpy as np
+
+def fine_register(zed_depth, zed_intrinsics, candidate_room_uid,
+                  mem: SpatialMemory,
+                  voxel: float = 0.05) -> Tuple[np.ndarray, float]:
+    # 1) ZED 帧 → 点云
+    src = depth_to_pointcloud(zed_depth, zed_intrinsics)
+    src = src.voxel_down_sample(voxel)
+    src.estimate_normals()
+
+    # 2) 房间 mesh 采样
+    room_mesh_path = f"robot_memory/ltm/meshes/{candidate_room_uid}.glb"  # 房间 mesh
+    # 若按房间没单独 mesh，则用 global mesh + 房间多边形裁剪
+    tgt = mesh_to_pointcloud(room_mesh_path, n=200_000)
+    tgt = tgt.voxel_down_sample(voxel)
+    tgt.estimate_normals()
+
+    # 3) 全局粗配准 (TEASER++ 或 RANSAC+FPFH)
+    src_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        src, o3d.geometry.KDTreeSearchParamHybrid(voxel*5, 100))
+    tgt_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        tgt, o3d.geometry.KDTreeSearchParamHybrid(voxel*5, 100))
+    result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
+        src, tgt, src_fpfh, tgt_fpfh, mutual_filter=True,
+        max_correspondence_distance=voxel*1.5,
+        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
+        ransac_n=4,
+        checkers=[o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+                  o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(voxel*1.5)],
+        criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
+
+    # 4) ICP 精化
+    result_icp = o3d.pipelines.registration.registration_icp(
+        src, tgt, voxel*0.5, result_ransac.transformation,
+        o3d.pipelines.registration.TransformationEstimationPointToPlane())
+
+    return result_icp.transformation, result_icp.fitness
+```
+
+### 5.5.2 候选轮询
+
+```python
+def relocalize(zed_frame, mem) -> RelocalizeResult:
+    coarse = CoarseRelocalizer(mem)
+    candidates = coarse(zed_frame.rgb, top_k=3)
+    best = None
+    for room_uid, _ in candidates:
+        T, fit = fine_register(zed_frame.depth, zed_frame.intrinsics,
+                               room_uid, mem)
+        if best is None or fit > best.fitness:
+            best = RelocalizeResult(T_zed_to_map=T, fitness=fit,
+                                     room_uid=room_uid)
+        if fit > 0.85:                # 高质量提前退出
+            break
+    return best
+```
+
+### 5.5.3 接受 / 拒绝阈值
+
+| fitness | 行动 |
+|---------|------|
+| > 0.85 | 接受，直接发布 |
+| 0.70–0.85 | 接受但标记 `confidence=medium`，触发 5 s 内复检 |
+| 0.50–0.70 | 拒绝，换 anchor 级精配（5.6）再试 |
+| < 0.50 | 失败，进入人工兜底 |
+
+---
+
+## 5.6 Anchor 级超精配（对付 fitness 偏低）
+
+当房间级配准 fitness < 0.7，可能是因为房间太大、视场只看到局部。退化为 **anchor 级**：
+
+```python
+def fine_register_anchor(zed_pc, anchor: Anchor, mem) -> Tuple[np.ndarray, float]:
+    node = mem.nodes[anchor.anchor_uid]
+    # 取该 anchor 的局部 mesh（如这张床）
+    tgt_pc = mesh_to_pointcloud(f"ltm/meshes/{node.uid}.glb", n=20_000)
+    # 把 tgt 变换到 map 帧
+    T_map = node.pose.to_matrix()
+    tgt_pc = tgt_pc.transform(T_map)
+    # 在 src 里**先用 YOLO 切出对应物体**，再 ICP
+    src_pc = crop_pc_by_yolo_detection(zed_pc, label=node.label)
+    if len(src_pc.points) < 500:
+        return None, 0.0
+    T_init = best_guess_init(node, zed_pose_estimate)
+    T, fit = open3d_icp(src_pc, tgt_pc, T_init)
+    return T, fit
+```
+
+→ 这种做法只用"床这个物体"对齐，比整个房间快也更鲁棒。
+
+---
+
+## 5.7 在线维护：连续 anchor 校正
+
+冷启动成功后，机器人开始巡逻。VIO 会慢慢漂，需要持续校正：
+
+```python
+class OnlineRelocalizer:
+    def __init__(self, mem, vio):
+        self.mem = mem
+        self.vio = vio
+        self.T_zed_to_map = np.eye(4)   # 上次重定位结果
+        self.last_correction_t = 0
+        self.drift_estimate = 0.0       # 累计漂移估计
+
+    def step(self, zed_frame):
+        # 1. 取 ZED VIO 当前位姿（zed 帧）
+        T_robot_zed = self.vio.current_pose()
+        # 2. 全局位姿
+        T_robot_map = self.T_zed_to_map @ T_robot_zed
+        # 3. 用 YOLO 看当前帧有没有 anchor 类家具
+        detections = yolo_world(zed_frame.rgb, classes=ANCHOR_LABELS)
+        for det in detections:
+            anchor = match_to_anchor(det, T_robot_map, self.mem)
+            if anchor is None: continue
+            # 4. 局部 ICP 校正
+            T_new, fit = fine_register_anchor(zed_frame.pc, anchor, self.mem)
+            if fit > 0.8:
+                # 用新 T 替换全局
+                self.T_zed_to_map = T_new @ np.linalg.inv(T_robot_zed)
+                self.last_correction_t = time.time()
+                self.drift_estimate = 0
+                return
+
+        # 没看到 anchor：用 IMU 估计漂移
+        self.drift_estimate += self.vio.expected_drift_per_sec * dt
+        if self.drift_estimate > 0.5:    # 50 cm 漂移触发主动重定位
+            self.trigger_full_relocalize()
+```
+
+---
+
+## 5.8 输出：ROS 2 TF 发布
+
+把握手结果发布到 TF 树，让导航栈/Agent 用：
+
+```python
+# nodes/relocalizer_node.py
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TransformStamped
+from tf2_ros import StaticTransformBroadcaster
+
+class RelocalizerNode(Node):
+    def __init__(self):
+        super().__init__("prism_relocalizer")
+        self.br = StaticTransformBroadcaster(self)
+
+    def publish(self, T_zed_to_map: np.ndarray, stamp):
+        msg = TransformStamped()
+        msg.header.stamp = stamp
+        msg.header.frame_id = "map"
+        msg.child_frame_id  = "zed2i_init"      # 锚定 ZED 起点
+        msg.transform.translation.x = float(T_zed_to_map[0,3])
+        msg.transform.translation.y = float(T_zed_to_map[1,3])
+        msg.transform.translation.z = float(T_zed_to_map[2,3])
+        q = matrix_to_quat(T_zed_to_map[:3,:3])
+        msg.transform.rotation.w, msg.transform.rotation.x, \
+        msg.transform.rotation.y, msg.transform.rotation.z = q
+        self.br.sendTransform(msg)
+```
+
+TF 树：
+```mermaid
+flowchart LR
+    MAP["map"] --> INIT["zed2i_init<br/><i>(相对固定，<br/>重定位时刷新)</i>"] --> VIO["zed2i_camera (VIO)<br/><i>(30 Hz VIO 输出)</i>"] --> BASE["base_link"]
+    style MAP fill:#fff7d6,stroke:#c97a00
+    style INIT fill:#ffe9b3,stroke:#c97a00
+```
+
+---
+
+## 5.9 失败兜底（人工 / 半自动）
+
+如果 fitness 一直 < 0.5：
+
+| 兜底方式 | 操作 | 适用 |
+|----------|------|------|
+| **二维码兜底** | 在每个房间门口贴 1 个 ArUco（与 iPhone 扫描时的一致） | 永久可靠，建议默认配置 |
+| **遥控引导** | 人手柄遥控机器人到某 anchor 前，按"我在这"按钮 | 应急 |
+| **iPhone 联动** | 用员工的 iPhone 走到机器人旁，App 计算两者相对位姿 | 黑科技兜底 |
+
+---
+
+## 5.10 评测协议
+
+每次部署后跑一遍：
+
+| 测试 | 方法 | 通过标准 |
+|------|------|----------|
+| 冷启动重定位 | 机器人随机放在 10 个位置开机 | 9/10 成功，位置误差 < 15 cm |
+| 跨房间一致性 | 跑同一条路径 3 次，比较返回时位姿 | 漂移 < 20 cm |
+| 绑架恢复 | 机器人运行中被抱到另一房间放下 | 30 s 内恢复 |
+| 高反光区 | 把机器人放在卫生间镜前 | 应自动跳过镜面区做 ICP，不挂 |
+| 黑暗 | 关灯只留小夜灯 | CLIP 降级，仍能由几何 ICP 兜住 |
+
+---
+
+## 5.11 性能预算（Jetson Orin AGX）
+
+| 阶段 | 耗时 | GPU 占用 |
+|------|------|----------|
+| Stage 1 CLIP | 80 ms | 1.2 GB |
+| Stage 1 DINO 重排（可选） | +60 ms | 2 GB |
+| Stage 2 RANSAC+FPFH | 300 ms | 0 (CPU) |
+| Stage 2 ICP refine | 100 ms | 0 |
+| 总冷启动 | < 1 s | < 3 GB |
+| 在线 anchor 校正 | 200 ms / 触发 | 1 GB |
+
+---
+
+## 5.12 本章小结
+
+| 关键点 | 一句话 |
+|--------|--------|
+| **目的** | 算一个 `T_zed→map`，让 ZED 的所有后续输出与 iPhone LTM 同框 |
+| **策略** | 两段式：CLIP 粗匹配 → ICP 精配准 |
+| **效率** | 冷启动 < 1 s，在线 anchor 校正 < 200 ms |
+| **鲁棒** | 多帧投票 + 多 anchor 轮询 + ArUco 兜底 |
+| **维护** | 巡逻中持续看到 anchor 就刷新一次，对抗 VIO 长漂 |
+
+读完本章你应能：
+- ✅ 实现一个 ROS 2 节点，3 s 内告诉机器人"我在 305 房间床前"
+- ✅ 设计鲁棒性测试用例
+- ✅ 在 fitness 低时知道如何回退
+
+下一章 [`06_pipeline_C_online_perception.md`](06_pipeline_C_online_perception.md) 讲机器人已经定位后，怎么把在线观测**写回**记忆。
+
+---
+
+**章节版本**：v1.0
+**估计阅读时间**：18 分钟
+**关键收获**：从"我在哪都不知道"到"我精确在 map 帧 (1.23, 4.56, 0.0)"的完整握手流程