gaojie
21 KiB
21 KiB
Chapter 06 — 管线 C：在线感知与差异检测
本章目标：机器人已定位（Chapter 05 完成）后，把 ZED 2i 30 Hz 流式数据持续写入 L1/L2，并在与 LTM 不一致时记录到
delta/，但不直接修改 LTM（修改交给 Chapter 07 的巩固阶段）。
6.1 设计原则：四条铁律
- 不阻塞：感知主循环必须能跑满 30 Hz，慢操作（CLIP / VLM）放异步队列
- 不破坏：LTM 永远只读；ZED 的所有"修改意图"都写到
delta/ - 不耗内存：L1 是环形缓冲，老数据自动覆盖；L2 局部 TSDF 与全局合并是后台任务
- 可追溯：每条 delta 都带
evidence（关键帧 ID 列表），方便巩固时复核
6.2 主循环架构
flowchart TB
SDK["📷 ZED SDK<br/><i>30 Hz: pose, rgb_left, depth, imu</i>"]
FP["Frame Producer (30 Hz)<br/>写入 L1 环形缓冲"]
AV["Avoidance<br/>(30 Hz)"]
TSDF["TSDF Worker (5 Hz)<br/>depth → L2"]
DET["Detector Worker (2 Hz)<br/>rgb → YOLO / VLM"]
CD["Change Detector<br/>(TSDF vs LTM)"]
OA["Object Associator<br/>(det ↔ L4 node)"]
DELTA[("delta/pending.jsonl")]
SDK --> FP
FP --> AV
FP --> TSDF
FP --> DET
TSDF --> CD
DET --> OA
CD --> DELTA
OA --> DELTA
style SDK fill:#e3f2fd,stroke:#1565c0
style FP fill:#fff7d6,stroke:#c97a00
style DELTA fill:#f5e1ff,stroke:#7b1fa2
style AV fill:#fde2e2,stroke:#a33
style TSDF fill:#fff1c1,stroke:#a87a00
style DET fill:#d4f0d4,stroke:#2e7d32
三个 worker 独立频率、独立队列，通过 Python multiprocessing.Queue 或 ROS 2 topic 通信。
6.3 Frame Producer（L1 写入器）
# perception/frame_producer.py
import pyzed.sl as sl
from collections import deque
import numpy as np, time
class FrameProducer:
def __init__(self, capacity_sec=10.0, fps=30):
cam = sl.Camera()
init = sl.InitParameters()
init.camera_resolution = sl.RESOLUTION.HD720
init.camera_fps = fps
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())
self.cam = cam
self.buf = deque(maxlen=int(capacity_sec * fps))
self.keyframes = deque(maxlen=200)
self.last_kf_t = 0
self.kf_interval = 0.5 # 关键帧每 0.5 s 一张
def step(self) -> PerceptualFrame:
rt = sl.RuntimeParameters()
if self.cam.grab(rt) != sl.ERROR_CODE.SUCCESS:
return None
# 取数据
rgb = sl.Mat(); self.cam.retrieve_image(rgb, sl.VIEW.LEFT)
dpth = sl.Mat(); self.cam.retrieve_measure(dpth, sl.MEASURE.DEPTH)
pose = sl.Pose(); self.cam.get_position(pose, sl.REFERENCE_FRAME.WORLD)
frame = PerceptualFrame(
timestamp=time.time(),
pose=zed_pose_to_Pose(pose),
rgb_left=rgb.get_data()[:, :, :3].copy(),
depth=dpth.get_data().copy(),
imu_packet=self.cam.get_sensors_data(...))
self.buf.append(frame)
if frame.timestamp - self.last_kf_t > self.kf_interval:
self.keyframes.append(frame)
self.last_kf_t = frame.timestamp
return frame
注意：ZED VIO 输出的位姿是 T_camera→world(zed)；需要左乘 Chapter 05 的 T_zed→map 得到 T_camera→map。这一步在 zed_pose_to_Pose 里做。
6.4 TSDF Worker（L2 写入器）
6.4.1 局部 TSDF 持续融合
# perception/tsdf_worker.py
import open3d as o3d
import numpy as np
class TSDFWorker:
def __init__(self, l2_memory, voxel=0.02, hop=0.2):
self.l2 = l2_memory
self.voxel = voxel
# 实时维护的"局部 TSDF"：跟随机器人，半径 5 m 范围
self.local = o3d.t.geometry.VoxelBlockGrid(
attr_names=('tsdf','weight','color'),
attr_dtypes=(o3d.core.float32,)*3,
attr_channels=((1,),(1,),(3,)),
voxel_size=voxel, block_resolution=16, block_count=10000,
device=o3d.core.Device("CUDA:0"))
self.last_pos = None
def integrate(self, kf: PerceptualFrame, intrinsics):
depth_o3d = o3d.t.geometry.Image(kf.depth).to("CUDA:0")
T = kf.pose.to_matrix()
# 跳过镜面区与 mobile 家具区（参考 prior_mask / no_update_zone）
mask = self.l2.compute_skip_mask(kf.pose, kf.depth.shape)
depth_o3d = depth_o3d * (1.0 - mask) # 在 GPU 上掩膜
frustum_blocks = self.local.compute_unique_block_coordinates(
depth_o3d, intrinsics, T, depth_scale=1.0, depth_max=5.0)
self.local.integrate(frustum_blocks, depth_o3d, intrinsics, T,
depth_scale=1.0, depth_max=5.0)
def export_local_pc(self) -> o3d.t.geometry.PointCloud:
return self.local.extract_point_cloud()
6.4.2 局部 → 全局合并（后台 1 Hz）
def merge_local_to_global(self):
"""每秒一次把局部 TSDF 的稳定块合并到全局"""
stable_blocks = self.local.get_blocks_with_weight_above(threshold=8)
for blk in stable_blocks:
self.l2.tsdf.merge_block(blk, weight_prior=self.l2.prior_weight(blk.coord))
# 老的局部块过期淘汰（机器人已离开 > 5 m）
self.local.prune_blocks_far_from(self.current_pose, max_dist=6.0)
6.4.3 prior_mask 起作用的地方
def compute_skip_mask(self, pose, depth_shape) -> np.ndarray:
"""根据当前视锥决定哪些像素的写入应被降权或跳过"""
H, W = depth_shape
mask = np.zeros((H, W), dtype=np.float32)
# 1) 镜面区（投影 LTM 的 no_update_zone 到当前像素）
proj_mirror = project_voxels_to_image(self.l2.no_update_zone, pose, intrinsics)
mask[proj_mirror] = 1.0
# 2) 先验墙区（不是不写，而是低权重；让 integrate 收到 weight=0.1）
# 在 integrate 里另做
return mask
6.5 Detector Worker（L4 增量写入器）
6.5.1 检测 + 关联
# perception/detector_worker.py
from ultralytics import YOLO
import torch
class DetectorWorker:
def __init__(self, mem, vlm=None):
self.mem = mem
self.yolo = YOLO("yolov8x-worldv2.pt")
# 用 LTM 里出现过的标签作为开放词表
all_labels = set(n.label for n in mem.nodes.values()
if n.level == "L4")
# 额外加常见小物品
all_labels |= {"remote","cup","bottle","phone","book","towel",
"luggage","backpack","slipper"}
self.yolo.set_classes(list(all_labels))
self.vlm = vlm # optional GPT-4V / Qwen-VL
def step(self, frame: PerceptualFrame):
results = self.yolo.predict(frame.rgb_left, conf=0.3, verbose=False)
detections = parse_yolo(results)
events = []
for det in detections:
# 1) 把 2D bbox + depth → 3D bbox（map 帧）
bbox3d = lift_2d_to_3d(det.bbox_2d, frame.depth,
frame.intrinsics, frame.pose)
# 2) 与 LTM 中已有节点关联
match = associate(bbox3d, det.label, self.mem)
if match is not None:
events.append(self.update_existing(match, det, bbox3d, frame))
else:
events.append(self.propose_new(det, bbox3d, frame))
return events
6.5.2 关联算法（detection ↔ LTM node）
def associate(bbox3d_obs, label, mem, dist_thresh=0.5):
"""简单贪心：找同 label、距离最近的节点"""
cand = [n for n in mem.nodes.values()
if n.level == "L4" and n.label == label]
if not cand:
return None
center_obs = bbox3d_obs.mean(axis=0)
best, best_d = None, float("inf")
for n in cand:
center_n = n.pose.position
d = np.linalg.norm(center_obs - center_n)
if d < best_d:
best, best_d = n, d
return best if best_d < dist_thresh else None
更稳健的做法是用 CLIP embedding 相似度 + 几何距离的加权：
def associate_hybrid(rgb_crop, bbox3d, label, mem,
w_geo=0.5, w_clip=0.5):
geo_scores = []
clip_scores = []
crop_feat = clip_encode_image(rgb_crop)
for n in mem.nodes_of_label(label):
d = np.linalg.norm(bbox3d.mean(0) - n.pose.position)
s_g = np.exp(-d / 0.5)
s_c = cosine(crop_feat, n.clip_embedding) if n.clip_embedding is not None else 0
geo_scores.append(s_g); clip_scores.append(s_c)
total = w_geo*np.array(geo_scores) + w_clip*np.array(clip_scores)
idx = int(np.argmax(total))
return list(mem.nodes_of_label(label))[idx] if total[idx] > 0.5 else None
6.5.3 更新已有节点
def update_existing(self, node, det, bbox3d, frame) -> DeltaEvent:
node.last_seen = frame.timestamp
node.observation_count += 1
# 位姿用 EMA 平滑更新
alpha = 0.1 # iPhone 来的节点更"硬"
new_center = bbox3d.mean(axis=0)
old_center = node.pose.position
drift = np.linalg.norm(new_center - old_center)
if drift > 0.20: # 偏离 > 20 cm，写 delta
return DeltaEvent(
event_id=uuid(),
event_type="object_moved",
target_uid=node.uid,
new_pose=Pose(new_center, est_quat(bbox3d)),
new_bbox=bbox3d,
evidence=[frame.keyframe_id])
else:
node.pose.position = alpha*new_center + (1-alpha)*old_center
return None # 微调，不写 delta
6.5.4 新增节点提案
def propose_new(self, det, bbox3d, frame) -> DeltaEvent:
"""ZED 看到 LTM 没记录的物品 → 候选节点"""
new_node = SpatialNode(
uid=SpatialNode.new_uid("zed"),
label=det.label,
category="small_item" if det.label in SMALL_ITEMS else "furniture",
level=MemoryLevel.L4,
source=Source.ZED2I,
confidence=0.3, # 初始低置信
pose=Pose(bbox3d.mean(0), est_quat(bbox3d)),
bbox_3d=bbox3d,
parent_room=find_room_for_point(bbox3d.mean(0)[:2], self.mem),
attributes={"mobile": True},
observation_count=1)
return DeltaEvent(
event_id=uuid(),
event_type="object_added",
target_uid=None,
new_pose=new_node.pose,
new_bbox=bbox3d,
evidence=[frame.keyframe_id],
# 嵌入候选节点作为 payload（巩固时直接 add_node）
payload=asdict(new_node))
6.6 Change Detector（TSDF vs LTM）
⚠️ v1.5 起本节描述的写法已被 § 6.6b v1.5 新写法:keyframe-based 内容更新(代替 L2 几何反推) 升级。 本节中"从 TSDF voxel 直接重投影生成 L3/L4 节点 patch 与 CLIP 嵌入"的旧写法仍保留以备追溯,但实际数据流应改为:TSDF 只贡献"哪个节点被触发"的路由信号,节点内容(patch / CLIP / bbox)从 L1 keyframe 缓存读取。详见 §6.6b 与
02_architecture.md§2.7b。
家具被搬走的检测来源不是物体检测的"没看到"（视野限制太多），而是 TSDF 的几何变化：
# perception/change_detector.py
def detect_geometric_change(local_tsdf, ltm_tsdf, ltm_mesh,
voxel=0.02, threshold=0.05):
"""对比局部实时 TSDF 与 LTM TSDF；返回变化体素"""
overlap_blocks = local_tsdf.overlap_with(ltm_tsdf)
moved_voxels, added_voxels = [], []
for blk in overlap_blocks:
local_sdf = local_tsdf.get_sdf(blk)
prior_sdf = ltm_tsdf.get_sdf(blk)
weight = local_tsdf.get_weight(blk)
# 只在 weight 足够时下判断
diff = (prior_sdf - local_sdf) * (weight > 5.0)
moved_voxels.extend(blk.coords_where(diff > threshold))
added_voxels.extend(blk.coords_where(diff < -threshold))
return moved_voxels, added_voxels
def cluster_to_event(voxels, mem, kind: str):
"""把散乱的变化体素聚类成"对象级"事件"""
pts = np.array(voxels)
if len(pts) < 50: # 太小忽略
return []
labels = dbscan_cluster(pts, eps=0.15, min_samples=20)
events = []
for cid in set(labels):
if cid == -1: continue
cluster = pts[labels==cid]
bbox = aabb_from_points(cluster)
center = cluster.mean(axis=0)
# 找该位置对应的 LTM 节点
target = nearest_node_in_bbox(self.mem, bbox)
events.append(DeltaEvent(
event_id=uuid(),
event_type="object_removed" if kind=="moved"
else "object_added",
target_uid=target.uid if target else None,
new_bbox=bbox if kind=="added" else None,
evidence=current_keyframe_ids()))
return events
调用频率：1 Hz 即可。
6.6b v1.5 新写法:keyframe-based 内容更新(代替 L2 几何反推)
6.6b.1 动机
v1.4 之前,差异检测在发现 TSDF voxel 变化后,会直接从那些变化的 voxel 重投影回当前视图,把 voxel 颜色/法线聚合成一块 patch,再喂给 CLIP 生成 L3/L4 节点的视觉指纹。这种"从几何反推内容"的做法有两个固有缺陷:
- 量化损失:TSDF 默认 2 cm 体素,远小于 CLIP encoder 期望的 224×224 patch 细节;voxel 颜色已经过加权平均,CLIP 嵌入精度受 voxel 噪声放大。
- 角度损失:voxel 重投影出来的"虚拟视图"未必与机器人当初看到该物体的最佳视角一致,导致同一物体在不同时刻的嵌入漂移。
v1.5 起,我们采纳 02_architecture.md §2.7b 的新原则:TSDF 体素只用来"路由"——告诉系统"L3 图里哪个节点该被更新";真正的节点内容(RGB patch / CLIP / 文字描述)直接从 L1 缓存中那一张原始 keyframe 上取。这样 CLIP 嵌入面对的是未经量化的高分辨率像素与机器人当初真实采到的视角,语义保真度显著上升。
6.6b.2 旧 vs 新伪代码并列对比
v1.4 旧写法(从 TSDF voxel 重投影获取颜色):
# v1.4: 内容来自 L2 voxel 反推,受量化限制
def update_l3_node_v1_4(changed_voxels, l2_tsdf, l3_graph):
target_uid = nearest_l3_node(changed_voxels.mean(axis=0), l3_graph)
# ① 把 voxel 颜色聚合 → 虚拟 patch(几何反推,信息损失)
voxel_colors = l2_tsdf.get_colors(changed_voxels) # 2 cm 量化
virtual_patch = reproject_voxels_to_image(
changed_voxels, voxel_colors,
fake_camera=synth_view(changed_voxels)) # 视角是合成的
# ② 用合成 patch 算 CLIP(精度天然受限)
clip_emb = clip_encode_image(virtual_patch)
l3_graph[target_uid].clip_embedding = clip_emb
l3_graph[target_uid].bbox_3d = aabb_from_voxels(changed_voxels)
v1.5 新写法(从 L1 keyframe 原图取 patch):
# v1.5: 路由来自 L2,内容来自 L1 keyframe 原图
def update_l3_node_v1_5(changed_voxels, l2_tsdf, l1_buffer, l3_graph):
# ① L2 只做路由:决定写哪个 L3 节点
target_uid = nearest_l3_node(changed_voxels.mean(axis=0), l3_graph)
# ② 从 L1 keyframe 池里挑"看该区域最清楚的那一帧"
region_center = changed_voxels.mean(axis=0)
best_kf = l1_buffer.pick_best_keyframe(
target_point=region_center,
criteria=("nearest_pose", "max_pixel_coverage", "min_blur"))
# ③ 在原始 RGB 上裁出 patch(无量化,真实视角)
real_patch = crop_keyframe_to_region(best_kf, region_center)
clip_emb = clip_encode_image(real_patch) # 高保真
l3_graph[target_uid].clip_embedding = clip_emb
l3_graph[target_uid].bbox_3d = aabb_from_voxels(changed_voxels)
l3_graph[target_uid].evidence_keyframes.append(best_kf.id)
关键差异:旧版第 ① 步就把"路由"与"取内容"绑死在 L2 voxel 上;新版第 ① 步把路由留在 L2(nearest_l3_node 只看 voxel 位置,不看颜色),第 ② 步明确转身向 L1 索取原始观测,第 ③ 步在未经量化的 RGB 上做 CLIP 编码。
6.6b.3 验证策略与评测预期
本改动主要影响 13_evaluation.md 中"语义一致性"相关指标:同一物体在不同时段被重观测时,其 CLIP 嵌入的余弦相似度应当上升。基于 patch 分辨率从 ~2 cm voxel 重投影提升到 RGB 原图(≥ 720p)、视角从合成视角恢复为真实采集视角这两个因素,工程预估新写法相对 v1.4:
- 同物体跨时段 CLIP 嵌入余弦相似度:+0.05 ~ +0.10 个点(注:工程预估而非实测,需在 §13 评测里跑 A/B 验证)
- 对 L3 节点
clip_embedding做的"文本→物体"检索 top-1 命中率应同步上升
落地后请在 13_evaluation.md 的"语义一致性"小节补一组 v1.4 vs v1.5 的对照实验,把上述预估替换为真实数字。
6.7 Delta 写入与去重
delta/pending.jsonl 是 append-only 的事件日志，但需要去重 + 累计观测数：
class DeltaLog:
def __init__(self, path="robot_memory/delta/pending.jsonl"):
self.path = path
self.events_by_signature: Dict[str, DeltaEvent] = {}
self._load_existing()
def append(self, ev: DeltaEvent):
sig = self._signature(ev)
if sig in self.events_by_signature:
old = self.events_by_signature[sig]
old.observation_count += 1
old.last_observed = ev.last_observed
old.evidence.extend(ev.evidence)
self._rewrite()
else:
self.events_by_signature[sig] = ev
with open(self.path, "a") as f:
f.write(json.dumps(asdict(ev)) + "\n")
def _signature(self, ev):
# 同一 target + 同事件类型 + 中心点接近 → 视为同一事件
center = ev.new_pose.position if ev.new_pose else (0,0,0)
cell = tuple(np.round(np.array(center)/0.3).astype(int))
return f"{ev.event_type}|{ev.target_uid}|{cell}"
6.8 完整 ROS 2 节点编排
# launch/prism_online.launch.yaml
nodes:
- name: zed_node
package: zed_wrapper
type: zed_camera
- name: prism_relocalizer
package: prism
type: relocalizer_node # Chapter 05
parameters:
ltm_dir: /robot_memory/ltm
- name: prism_frame_producer
package: prism
type: frame_producer
parameters:
keyframe_interval: 0.5
- name: prism_tsdf_worker
package: prism
type: tsdf_worker
parameters:
voxel: 0.02
max_dist: 5.0
- name: prism_detector_worker
package: prism
type: detector_worker
parameters:
detection_rate_hz: 2.0
- name: prism_change_detector
package: prism
type: change_detector
parameters:
rate_hz: 1.0
diff_threshold_m: 0.05
- name: prism_delta_log
package: prism
type: delta_log
parameters:
path: /robot_memory/delta/pending.jsonl
6.9 性能预算（Jetson Orin AGX 64 GB）
| 模块 | 频率 | CPU | GPU | 内存 |
|---|---|---|---|---|
| Frame Producer | 30 Hz | 1 core | 2 GB（ZED 自带） | 500 MB |
| TSDF Worker | 5 Hz | 1 core | 1.5 GB | 1 GB |
| Detector Worker (YOLO-World) | 2 Hz | 1 core | 3 GB | 800 MB |
| Change Detector | 1 Hz | 1 core | 0 | 200 MB |
| Relocalizer (待命) | 按需 | <1 core | 1 GB peak | 500 MB |
| 合计 | 5 cores | < 8 GB | < 4 GB |
Orin AGX 12 核 + 64 GB，留 50% 余量给上层 Agent。
6.10 调试与可视化
提供两个 dashboard：
| 工具 | 用途 |
|---|---|
prism viz live |
RViz2 显示：当前位姿 + 局部 TSDF + 检测 bbox + delta 红框 |
prism viz delta |
Open3D 窗口：LTM mesh + delta 事件标点，鼠标点击可看 evidence keyframe |
6.11 异常处理
| 异常 | 表现 | 处理 |
|---|---|---|
| ZED 掉线 | grab 失败 | Frame Producer 重试 5 次，仍失败则降级模式（仅 IMU 推算） |
| TSDF OOM | GPU 内存爆 | 自动减小 block_count，丢弃最旧块 |
| YOLO 类别太多 | 推理慢 | 动态裁剪：只保留当前房间可能出现的类别 |
| delta 太多（异常情况） | 文件爆 | 触发紧急 Consolidation 或人工介入 |
6.12 本章小结
| 关键点 | 一句话 |
|---|---|
| L1 写入 | Frame Producer 30 Hz |
| L2 写入 | TSDF Worker 5 Hz，受 prior_mask / no_update_zone 保护 |
| L4 写入 | Detector Worker 2 Hz；存在节点 → EMA 更新；新物 → 候选 |
| 差异感知 | 几何变化由 TSDF 对比 + DBSCAN 聚类得出 |
| 铁律 | 永远不直接改 LTM，所有"想改"都先入 delta/pending.jsonl |
| 性能 | Jetson Orin AGX 上 < 50% 资源占用 |
读完本章你应能：
- ✅ 在 ROS 2 启动整套在线感知节点
- ✅ 解释为什么必须有 prior_mask 与 no_update_zone
- ✅ 实现一个最小的 DeltaLog 并跑通去重
下一章 07_pipeline_D_consolidation.md 讲机器人"睡觉"时如何把 delta/ 中的内容真正巩固进 LTM。
章节版本：v1.0 估计阅读时间：22 分钟 关键收获：可运行的实时感知三 worker 设计 + delta 写入机制