gaojie
369 lines
13 KiB
Markdown
369 lines
13 KiB
Markdown
# 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)"的完整握手流程
|