worldmodel/plans/PRISM/05_pipeline_B_relocalization.md

title, date, draft, tags, categories

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

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 两段式重定位策略：粗 → 精

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 房间检索（默认方案）

# 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 在"语义相似但几何不同"（如所有酒店客房都长得差不多）时会混。增强做法：

# 用 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 投票（取连续多帧增强稳定性）

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 流程

# 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 候选轮询

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 级：

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 会慢慢漂，需要持续校正：

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 用：

# 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 树：

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 讲机器人已经定位后，怎么把在线观测写回记忆。

---

章节版本：v1.0 估计阅读时间：18 分钟 关键收获：从"我在哪都不知道"到"我精确在 map 帧 (1.23, 4.56, 0.0)"的完整握手流程