---
title: "Chapter 05 — 管线 B：ZED 2i 重定位握手"
date: 2026-05-20
draft: false
tags: ["PRISM", "世界模型", "空间记忆", "VIO", "点云", "机器人"]
categories: ["worldmodel"]
---

# 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)"的完整握手流程