DualMap 如何把开放词汇语义建图接到自然语言导航
0. 简介
DualMap 是一个面向在线机器人运行的开放词汇语义建图系统。它关心的问题不是“把一张图片分成若干固定类别”，而是让机器人在移动过程中持续维护一份对象级地图，并能用自然语言查询地图中的目标。DualMap 论文页面将系统概括为 online open-vocabulary mapping，强调开放词汇理解、在线高效建图和动态变化环境中的语言引导导航。放到工程实现里看，这三个目标分别对应视觉语言前端、局部具体图和全局抽象图、以及 ROS 路径发布与下游导航执行接口。
这张总览图如果需要重新绘制，可以直接把下面这段提示词交给 Gemini。图的重点不是画成论文插图，而是服务中文技术文章阅读，让读者一眼看清 DualMap 在机器人导航系统里的位置。
请为一篇中文技术文章绘制一张“DualMap 开放词汇语义导航总体架构图”。画面采用横向流程图，风格简洁、技术感强，适合放在 CSDN 或论文解读文章开头。
图中从左到右依次包含五个主要模块：
1. 机器人传感器输入：RGB 图像、Depth 深度图、Odometry/相机位姿、CameraInfo。可以画成相机、深度图、里程计和机器人小图标，并标注“同步后的 RGB-D + Pose 输入”。
2. 视觉语言前端：YOLO-World 负责开放词汇检测，SAM/FastSAM 负责分割 mask，MobileCLIP/CLIP 负责图文特征。请用三个小模块串联表示，并在旁边标注“从图像中生成对象语义与 mask”。
3. DualMap 双地图核心：上方画“局部具体图 Local Concrete Map”，包含当前区域对象、3D 点云、bbox、对象关系；下方画“全局抽象图 Global Abstract Map”，包含低移动性锚点、长期语义关系、可跨区域查询的目标候选。两张图之间画箭头，标注“稳定低移动性对象进入全局图，高移动性对象保留局部确认”。
4. 自然语言查询：画一个用户输入气泡，例如“帮我找桌子上的碗”，箭头指向全局图和局部图，标注“开放词汇检索 + 语义关系召回 + 局部确认”。
5. 导航执行接口：DualMap 输出候选目标、全局路径、局部路径或 action_path；右侧连接 ROS2/Nav2，Nav2 再连接机器人底盘执行。请明确画出 DualMap 是“语义大脑”，Nav2 是“运动执行层/运动小脑”，不要让读者误以为 DualMap 直接控制底盘速度。
图中需要体现两条关键闭环：
- 语义建图闭环：传感器输入 -> 视觉语言前端 -> 局部图 -> 全局图 -> 自然语言查询。
- 导航失败恢复闭环：Nav2 执行失败或目标未确认 -> 回到 DualMap 查询下一个候选锚点。
配色建议：传感器输入用蓝色，视觉语言模型用紫色，DualMap 双地图用绿色和橙色，Nav2 执行层用灰蓝色。所有文字使用中文标签，模块边界清晰，箭头方向明确，画面不要堆太多细碎公式，不要使用真实照片背景。
这篇文章按代码实现来解释 DualMap 的导航链路。读完后需要形成一个清楚判断：DualMap 不是传统 2D Navigation 的替代品，也不是单纯的语义分割后处理；它更像机器人导航系统中的“语义大脑”。它负责把“找碗”“去可以坐的地方”“去桌子旁边”这类语言目标变成候选锚点、局部目标和路径建议。真正让底盘稳定移动、避障、恢复失败的“运动小脑”，更适合交给 ROS2 Nav2 这类成熟导航栈。
1. 背景与整体定位
1.1 为什么自然语言导航不能只靠普通 2D 地图
传统 2D 建图和导航擅长回答“从当前位置到某个坐标怎么走”。它通常依赖 occupancy grid、costmap、planner、controller 和 recovery 行为，目标输入往往是一个 PoseStamped 或若干 waypoint。但用户说“帮我找桌子上的碗”时，问题的第一步并不是路径规划，而是目标解释：碗属于开放词汇对象，可能不在预定义类别表里；碗还可能被移动，历史坐标未必可靠；“桌子上的”又表达了对象之间的空间关系。普通 2D 地图没有对象语义，也没有语言查询能力，因此它很难独立完成这类任务。
DualMap 的切入点是把导航目标从坐标提升到对象。它通过 RGB-D 图像、相机位姿和视觉语言模型生成对象观测，再把观测组织成局部图和全局图。局部图保留当前区域的具体对象，适合做最后一段精确确认；全局图保留长期稳定的低移动性锚点，适合做跨房间或跨区域的候选选择。这种设计使机器人不必在全局范围内永久保存每个杯子、碗、靠垫的完整 3D 点云，而是把高动态目标和稳定锚点之间的语义关系保存下来。
1.2 从代码看主链路：输入、建图、查询、路径
仓库的入口集中在 applications/。runner_dataset.py 处理离线数据集，runner_record_3d.py 处理 iPhone Record3D 输入，runner_ros.py 处理 ROS1/ROS2 数据流。真正的系统调度在 dualmap/core.py 的 Dualmap 类中完成，它初始化 Detector、LocalMapManager、GlobalMapManager 和 ReRunVisualizer，并在每个关键帧上串起检测、局部建图、全局建图、路径规划和可视化发布。工具层集中在 utils/，其中 object_detector.py 负责视觉前端，object.py 负责对象生命周期，tracker.py 负责对象匹配，navigation_helper.py 负责占据图、Voronoi 图和 RRT 路径搜索。
# dualmap/core.py
self.visualizer = ReRunVisualizer(cfg)
self.detector = Detector(cfg)
self.local_map_manager = LocalMapManager(cfg)
self.global_map_manager = GlobalMapManager(cfg)
这四个对象对应系统的四个核心职责。Detector 把 RGB-D 帧变成带语义和点云的观测；LocalMapManager 管理短期对象、稳定性和对象关系；GlobalMapManager 维护可长期查询的低移动性锚点；ReRunVisualizer 和 ROS publisher 则把结果送到可视化或下游导航系统。代码结构上没有把“识别”和“导航”硬塞进一个函数，而是把语义建图与路径生成分层，这一点对理解后续的 Nav2 接入非常重要。
下面这段是按 dualmap/core.py 的源码逻辑整理出的主流程。它把一帧数据如何进入系统、如何变成对象观测、如何进入双地图，以及什么时候触发导航路径计算串起来。实际代码里串行和并行模式分开实现，但核心顺序是一致的：先用视觉前端生成对象，再由局部图判断对象生命周期，最后把稳定锚点提交给全局图。
# 按 dualmap/core.py 源码逻辑整理：DualMap 单帧处理主链路
def process_one_keyframe(self, data_input):
# 1. 保存当前关键帧编号和当前相机/机器人位姿
# 后续可视化、局部路径裁剪、全局路径起点都会读 curr_pose。
self.curr_frame_id = data_input.idx
self.curr_pose = data_input.pose
# 2. 视觉前端读取 RGB-D、内参和位姿，生成当前帧检测结果
self.detector.set_data_input(data_input)
if self.cfg.run_detection:
self.detector.process_detections()
else:
self.detector.load_detection_results()
# 3. 把检测框、mask、深度点云、CLIP 特征整理成 LocalObservation
# 这一步之后，系统处理的对象就不再只是 2D 框，而是 3D 对象观测。
self.detector.calculate_observations()
curr_obs_list = self.detector.get_curr_observations()
# 4. 局部图先吸收当前观测，完成对象匹配、稳定性判断和关系维护
self.local_map_manager.set_curr_idx(self.curr_frame_id)
self.local_map_manager.process_observations(curr_obs_list)
# 5. 局部图只把稳定低移动性对象输出给全局图
# 高移动性对象通常不会单独成为长期全局锚点。
global_obs_list = self.local_map_manager.get_global_observations()
self.local_map_manager.clear_global_observations()
# 6. 全局图融合稳定锚点，后续自然语言查询会在这里检索候选
self.global_map_manager.process_observations(global_obs_list)
1.3 整体融合链路：从单帧检测到双地图导航
DualMap 的融合链路不是简单地把多帧点云拼在一起，而是先把每一帧 RGB-D 输入转成对象级观测，再通过局部图和全局图分层融合。单帧进入系统后，检测前端先用 YOLO 给出类别和二维框，用 SAM 生成精确 mask；如果启用 FastSAM，还会补充类别表外的 unknown 候选。随后系统对检测结果进行过滤，去除背景、小 mask 和重复 mask，并把 mask、深度图、相机内参结合起来反投影为对象点云。同时，CLIP 会对每个图像 crop 编码语义特征，使对象不仅有几何形状，也有开放词汇语义表示。
这些结果会被打包成 LocalObservation，进入局部图。局部图会用 Tracker 将当前观测与已有 LocalObject 匹配：空间上看点云和 3D bbox 是否重叠，语义上看 CLIP 特征是否相似。匹配成功的观测会融合进已有对象，更新点云、bbox、类别概率、CLIP 特征和移动性判断；匹配失败则创建新对象。局部对象不会立即进入全局图，而是经过一套生命周期状态机，只有在多帧中表现稳定、类别收敛、几何可靠的对象才会被进一步处理。
局部图还会判断对象之间的相对关系，尤其是“高移动性物体依附在低移动性物体上”的情况。例如杯子、书本、遥控器这类物体位置经常变化，不适合作为长期全局锚点；但桌子、柜子、沙发等低移动性对象更稳定，可以作为长期地图中的参考物。系统会把高移动性物体的 CLIP 特征作为 related feature 挂到低移动性锚点上。这样既避免全局地图被移动物体的历史位置污染，又保留了“桌子上的杯子”这类语义查询能力。
当局部对象满足稳定条件后，低移动性对象会被转换成 GlobalObservation，再进入全局图。全局图的融合更偏向长期一致性，它使用对象的 2D 投影 bbox 和平面点云进行匹配，避免仅凭语义相似度把不同位置的同类物体错误合并。匹配成功时，新的全局观测会融合进已有 GlobalObject，更新 3D 点云、2D 导航点云、bbox 和 related features；匹配失败时则生成新的全局锚点。最终形成的是一种“大脑 + 小脑”的结构：全局图负责长期记忆、语义检索和粗导航，局部图负责当前环境、动态对象处理和末端精细导航。
2. 系统输入与对象观测
2.1 ROS 输入的第一关是时间同步和坐标统一
真实机器人里，RGB 图像、深度图和里程计并不会完全同时到达。DualMap 的 ROS2 runner 使用 message_filters.ApproximateTimeSynchronizer 同步 RGB、Depth 和 Odometry，配置中的 sync_threshold 对应 ROS2 官方文档里 slop 的概念，即允许不同消息时间戳之间存在一定误差。同步不是形式问题：如果 RGB 来自 10:00:00.100，深度来自 10:00:00.250，里程计又来自另一个时刻，那么反投影得到的对象点云会系统性偏移，后续全局图和路径规划都会被污染。
# applications/utils/runner_ros2.py
self.sync = ApproximateTimeSynchronizer(
[self.rgb_sub, self.depth_sub, self.odom_sub],
queue_size=10,
slop=self.cfg.sync_threshold,
)
self.sync.registerCallback(self.synced_callback)
同步之后还要处理坐标。RunnerROSBase.push_data() 会把传入 pose 先乘传感器外参，再乘世界坐标修正矩阵，生成后续统一使用的 DataInput.pose。这里的 world_roll、world_pitch、world_yaw 和 extrinsics 不是可有可无的参数。相机驱动、仿真器和底盘里程计经常采用不同坐标约定，如果这一步没对齐，Rerun 里看起来只是点云歪了一点，接到底盘执行时就会变成目标点落错方向。
# applications/utils/runner_ros_base.py
def push_data(self, rgb_img, depth_img, pose, timestamp):
transformed_pose = self.create_world_transform() @ (pose @ self.extrinsics)
data_input = DataInput(
idx=self.kf_idx,
time_stamp=timestamp,
color=rgb_img,
depth=depth_img,
intrinsics=self.intrinsics,
pose=transformed_pose,
)
self.synced_data_queue.append(data_input)
return data_input
如果把 ROS 输入再展开一层，关键帧判断也很重要。DualMap 不会对每一帧都做完整检测和建图，而是通过时间、平移和旋转阈值筛选关键帧。这样可以降低在线运行时的计算压力，同时避免机器人缓慢移动或原地旋转时漏掉新视角。下面是按 check_keyframe() 源码整理后的版本，重点在三个触发条件：移动足够远、转动足够大、距离上一关键帧时间足够久。
# 按 dualmap/core.py 源码逻辑整理：关键帧筛选
def check_keyframe(self, time_stamp, curr_pose):
is_keyframe = False
if self.last_keyframe_pose is not None:
# 条件一：机器人平移超过阈值，说明已经看到新的空间区域
translation_diff = np.linalg.norm(
curr_pose[:3, 3] - self.last_keyframe_pose[:3, 3]
)
if translation_diff >= self.pose_threshold:
is_keyframe = True
# 条件二：机器人旋转超过阈值
# 即使原地不动，视野方向变化也可能带来大量新物体观测。
curr_rotation = R.from_matrix(curr_pose[:3, :3])
last_rotation = R.from_matrix(self.last_keyframe_pose[:3, :3])
angle_diff = (curr_rotation.inv() * last_rotation).magnitude() * 180 / np.pi
if angle_diff >= self.rotation_threshold:
is_keyframe = True
# 条件三：时间兜底
# 防止机器人移动很慢时长期没有关键帧，导致局部图无法更新。
if self.last_keyframe_time is None or abs(time_stamp - self.last_keyframe_time) >= self.time_threshold:
is_keyframe = True
if is_keyframe:
self.last_keyframe_time = time_stamp
self.last_keyframe_pose = curr_pose
self.keyframe_counter += 1
return True
return False
2.2 视觉前端不是分类器，而是对象观测生成器
DualMap 的视觉前端组合了 YOLO-World、SAM、MobileCLIP、可选 FastSAM 和点云后处理。YOLO-World 论文把问题指向传统 YOLO 只能识别固定类别的局限，并通过视觉语言建模支持开放词汇检测；SAM 论文强调模型可以由提示驱动生成分割掩码，并迁移到新的图像分布；MobileCLIP 则面向低延迟图文特征对齐，在移动端或在线场景里更实用。DualMap 不是把这些模型堆在一起展示效果，而是把它们用于生成一个建图系统真正需要的最小对象单元：类别、CLIP 特征、mask、点云、bbox、距离和移动性判断。
这里适合放一张“对象观测生成流程图”。如果需要用 Gemini 画图，可以直接使用下面这段提示词，让图和本节代码解释对应起来。
请绘制一张“DualMap 视觉前端如何生成 LocalObservation”的中文流程图，适合放在技术文章 2.2 小节，画面要比普通模型堆叠图更工程化。
图中从左到右展示如下处理链路：
1. 输入：RGB 图像、Depth 深度图、相机内参、相机/机器人位姿 Pose。请画成四个输入卡片，并在 RGB 与 Depth 之间标注“时间同步”。
2. 开放词汇检测：YOLO-World 根据文本类别或开放词汇提示检测图像中的对象，输出 2D bbox、class_id、confidence。请在模块下方列出这三个输出字段。
3. 分割：SAM 或 FastSAM 根据 bbox 生成对象 mask。请画出 bbox 到 mask 的转换，并标注“从矩形框变成对象轮廓”。
4. 语义特征：MobileCLIP/CLIP 对对象裁剪图或 mask 区域提取 image feature，用于后续自然语言查询和对象匹配。请标注“clip_ft：开放词汇召回的关键特征”。
5. 几何反投影：根据 mask 内深度像素、相机内参和 Pose，把 2D 像素反投影成 3D object point cloud，再计算 3D bbox 和 distance。请把这条分支画成从 Depth + mask 指向点云的小流程。
6. 移动性判断：结合 CLIP 特征与 mobility_config.yaml 中的低移动/高移动示例，判断 is_low_mobility。请用分叉表示：低移动对象进入全局图候选，高移动对象主要留在局部图。
7. 输出：LocalObservation 对象。输出卡片中列出字段：class_id、mask、xyxy、conf、clip_ft、pcd、bbox、distance、is_low_mobility。
图中要强调两条并行信息流：
- 语义流：RGB -> YOLO-World/SAM -> CLIP feature -> class_id、mask、clip_ft。
- 几何流：Depth + mask + intrinsics + pose -> 3D point cloud -> bbox、distance。
最终两条流在 LocalObservation 合并。请使用清晰的箭头和中文字段名，避免过度卡通化；风格可以是白底、浅色模块、细线箭头，适合和代码片段放在一起阅读。
代码中的 Detector.process_detections() 和 calculate_observations() 体现了这种工程取向。检测和分割得到 mask 后，系统一边计算 CLIP 图像特征，一边把 mask 内深度像素反投影成对象点云，并通过采样和聚类降低噪声。最终进入地图管理器的不是图片框，而是 LocalObservation。这一步是自然语言导航和普通 2D 导航的分水岭：普通导航只需要障碍和自由空间，DualMap 还需要知道这个障碍或物体“可能是什么”以及“能否作为长期锚点”。
# utils/object_detector.py
curr_obs.class_id = self.curr_results["class_id"][i]
curr_obs.mask = self.curr_results["masks"][i]
curr_obs.clip_ft = self.curr_results["image_feats"][i]
curr_obs.pcd = pcd
curr_obs.bbox = bbox
curr_obs.distance = distance
curr_obs.is_low_mobility = self.is_low_mobility(curr_obs.clip_ft)
更细地看，对象观测生成可以拆成“语义”和“几何”两条线。语义线来自开放词汇检测和 CLIP 特征，决定这个对象能否被自然语言召回；几何线来自深度反投影和点云 bbox，决定这个对象在地图中的位置以及能否参与路径规划。下面的中文注释版代码按 Detector.calculate_observations() 的核心逻辑整理，省略了保存裁剪图、可视化和异常保护等周边代码。
# 按 utils/object_detector.py 源码逻辑整理：从检测结果生成 LocalObservation
def calculate_observations(self):
curr_observations = []
for i, pcd in enumerate(self.curr_results["object_pcds"]):
# 1. 空点云不能进入地图，否则 bbox、匹配和路径都会失真
if pcd is None or len(pcd.points) == 0:
continue
curr_obs = LocalObservation()
# 2. 语义信息：类别、mask、检测置信度、CLIP 特征
# 类别用于可视化和初步解释，CLIP 特征用于开放词汇查询。
curr_obs.idx = self.curr_data.idx
curr_obs.class_id = self.curr_results["class_id"][i]
curr_obs.mask = self.curr_results["masks"][i]
curr_obs.xyxy = self.curr_results["xyxy"][i]
curr_obs.conf = self.curr_results["confidence"][i]
curr_obs.clip_ft = self.curr_results["image_feats"][i]
# 3. 几何信息：对象点云、3D bbox、到当前相机/机器人距离
curr_obs.pcd = pcd
curr_obs.bbox = pcd.get_axis_aligned_bounding_box()
curr_obs.distance = self.get_distance(curr_obs.bbox, self.curr_data.pose)
# 4. 移动性判断：决定对象后续进入全局图还是只留在局部图
curr_obs.is_low_mobility = self.is_low_mobility(curr_obs.clip_ft)
# 5. 如果类别名本身在低移动示例中，直接加强低移动判断
class_name = self.obj_classes.get_classes_arr()[curr_obs.class_id]
if class_name in self.cfg.lm_examples:
curr_obs.is_low_mobility = True
curr_observations.append(curr_obs)
self.curr_observations = curr_observations
3. 双地图表示与动态目标处理
3.1 低移动性和高移动性是 DualMap 的核心分流
DualMap 把对象分成低移动性和高移动性，并不是为了给物体贴一个绝对标签，而是为了决定它在地图中的角色。低移动性对象包括桌子、柜子、沙发、床、椅子等，它们适合作为全局锚点；高移动性对象包括杯子、碗、盒子、靠垫、背包等，它们更适合在局部图里实时确认。配置文件 config/support_config/mobility_config.yaml 给出了一组示例，检测器再用 CLIP 特征与这些示例和描述做相似度判断。这个判断不需要完美，但必须足够稳定，因为它决定对象最终进入全局图还是只作为局部信息保留。
# config/support_config/mobility_config.yaml
lm_examples:
- "cabinet"
- "couch"
- "chair"
- "table"
- "bed"
hm_examples:
- "backpack"
- "box"
- "cup"
- "pillow"
- "bowl"
对象生命周期由 LocalObject.update_status() 管理。最近仍被观测到的对象保持 UPDATING；离开活动窗口后先进行稳定性检查，不稳定就进入 PENDING，超过等待次数后删除；稳定对象再根据移动性分成 LM_ELIMINATION 和 HM_ELIMINATION。这里的 ELIMINATION 容易误解，它不是“这个对象没用了”，而是“这个对象完成了局部生命周期，接下来要么输出到全局图，要么作为高动态对象被清理或附着到稳定锚点上”。
# utils/object.py
if self.is_low_mobility:
self.status = LocalObjStatus.LM_ELIMINATION
return
else:
self.status = LocalObjStatus.HM_ELIMINATION
return
对象状态机是 DualMap 能在线运行的关键。局部图不是无限增长的对象列表，而是一个带时间窗口的对象缓冲区。一个对象如果最近仍被看到，就保持更新；如果离开当前窗口，就检查它是否足够稳定；如果不稳定，先挂起等待；如果稳定，再根据移动性决定是否提交给全局图。下面的代码保留了源码里的主要判断顺序，并用中文解释每个状态为什么存在。
# 按 utils/object.py 源码逻辑整理：LocalObject 生命周期
def update_status(self):
last_obs = self.get_latest_observation()
# 1. 仍在活跃窗口内：继续等待后续帧更新
# 不能刚看到一个物体就提交到全局图，否则会把噪声检测长期保存。
in_active_window = (
last_obs.idx <= self._curr_idx
and last_obs.idx >= max(self._curr_idx - self._cfg.active_window_size, 0)
)
if in_active_window:
self.status = LocalObjStatus.UPDATING
self.pending_count = 0
self.waiting_count = 0
return
# 2. 离开活跃窗口后，先检查这个对象是否稳定
# 稳定性主要来自多帧观测数量和类别一致性。
self.stability_check()
if not self.is_stable:
# 3. 不稳定对象进入 PENDING，而不是立刻删除
# 这样可以给后续帧纠正误检或短暂遮挡的机会。
self.status = LocalObjStatus.PENDING
self.pending_count += 1
# 4. 如果长时间仍不稳定，才彻底清理
if self.pending_count > self._cfg.max_pending_count:
self.status = LocalObjStatus.ELIMINATION
return
# 5. 稳定对象也先等待几帧，避免刚稳定就被过早提交
self.status = LocalObjStatus.WAITING
self.waiting_count += 1
if self.waiting_count < self._cfg.max_pending_count:
return
# 6. 最终分流：低移动对象成为全局锚点，高移动对象等待被关系吸收或删除
if self.is_low_mobility:
self.status = LocalObjStatus.LM_ELIMINATION
else:
self.status = LocalObjStatus.HM_ELIMINATION
3.2 所谓“保留相对关系”，保留的不是旧坐标
很多人第一次看 DualMap 会问：如果碗被移动了，全局图里还保存“桌子上有碗”的信息，会不会导致机器人找到旧位置？代码给出的答案很明确：全局图并不把高动态物体的旧坐标当成最终目标。LocalMapManager.status_actions() 只在稳定对象之间检查 “on” 关系，并在低移动性对象生成 GlobalObservation 时，把相关高移动性对象的 CLIP 特征和可视化 bbox 附带进去。真正用于后续检索的是 related_objs 中的语义特征，而不是承诺碗仍然在原来的 3D 坐标。
# utils/local_map_manager.py
if related_objs:
for related_obj in related_objs:
curr_obs.related_objs.append(related_obj.clip_ft)
curr_obs.related_bbox.append(related_obj.bbox)
curr_obs.related_color.append(related_obj.class_id)
else:
curr_obs.related_objs = []
“on” 关系的判断也不是语言模型凭空推理，而是几何规则。源码会要求两个对象中只有一个具有主平面信息，例如桌面、柜面、椅面；然后比较高移动对象 bbox 在 XY 平面上是否与承载物有足够重叠，以及高移动对象底部是否接近承载平面高度。这个规则比较朴素，但好处是可解释、可调参，也能把“碗在桌子上”这类关系转换成全局图可保存的语义线索。
# 按 utils/local_map_manager.py 源码逻辑整理：on relation 几何判断
def on_relation_check(self, base_obj, test_obj):
# 1. 没有 bbox 就无法做几何关系判断
if base_obj.bbox is None or test_obj.bbox is None:
return False
# 2. 必须有且只有一个对象具有主平面信息
# 例如桌子有桌面平面，碗通常没有主平面。
if base_obj.major_plane_info is None and test_obj.major_plane_info is None:
return False
if base_obj.major_plane_info is not None and test_obj.major_plane_info is not None:
return False
# 3. 保证 base_obj 是有主平面的承载物
if base_obj.major_plane_info is None:
base_obj, test_obj = test_obj, base_obj
base_min = base_obj.bbox.get_min_bound()
base_max = base_obj.bbox.get_max_bound()
test_min = test_obj.bbox.get_min_bound()
test_max = test_obj.bbox.get_max_bound()
# 4. 计算 test_obj 在 XY 平面上落入 base_obj 的比例
overlap_x = max(0, min(base_max[0], test_max[0]) - max(base_min[0], test_min[0]))
overlap_y = max(0, min(base_max[1], test_max[1]) - max(base_min[1], test_min[1]))
overlap_area = overlap_x * overlap_y
test_area = (test_max[0] - test_min[0]) * (test_max[1] - test_min[1])
overlap_ratio = overlap_area / test_area
if overlap_ratio < self.cfg.object_matching.overlap_ratio:
return False
# 5. 检查高移动对象底部高度是否贴近承载物主平面
plane_distance = self.cfg.on_relation.plane_distance
near_plane = (
test_min[2] - plane_distance
<= base_obj.major_plane_info
<= test_min[2] + plane_distance * 2
)
return near_plane
当低移动对象进入 LM_ELIMINATION 状态时，局部图会把它转成 GlobalObservation。如果它周围有相关高移动对象，代码不会保存完整高移动对象点云，而是只把 related CLIP 特征、bbox 和类别颜色附着到锚点上。下面这段代码展示了全局观测的构造方式，也解释了为什么全局图更轻：它主要保存可导航的平面锚点和可检索的语义线索。
# 按 utils/local_map_manager.py 源码逻辑整理：从 LocalObject 生成 GlobalObservation
def create_global_observation(self, obj, related_objs=None):
related_objs = related_objs or []
curr_obs = GlobalObservation()
# 1. 锚点自身信息：低移动对象可以长期保存
curr_obs.uid = obj.uid
curr_obs.class_id = obj.class_id
curr_obs.pcd = obj.pcd
curr_obs.bbox = obj.pcd.get_axis_aligned_bounding_box()
curr_obs.clip_ft = obj.clip_ft
# 2. 导航只需要平面可通行信息，所以把 3D 点云投影/下采样成 2D 点云
curr_obs.pcd_2d = obj.voxel_downsample_2d(
obj.pcd,
self.cfg.downsample_voxel_size,
)
curr_obs.bbox_2d = curr_obs.pcd_2d.get_axis_aligned_bounding_box()
# 3. 高移动对象不直接成为全局锚点，只作为 related feature 附着保存
for related_obj in related_objs:
curr_obs.related_objs.append(related_obj.clip_ft)
curr_obs.related_bbox.append(related_obj.bbox)
curr_obs.related_color.append(related_obj.class_id)
return curr_obs
因此，更准确的说法是：DualMap 保存“稳定锚点附近曾经观测到与查询相关的高动态语义线索”。当用户查询 “bowl” 时，全局图可能召回某张桌子，因为这张桌子的 related feature 曾经和 bowl 很相似；机器人先走到桌子附近，再由当前局部图重新检测和确认碗是否真的还在那里。如果局部图没有找到当前碗，系统应该触发找下一个候选，而不是继续相信历史记录。这是动态场景导航里最重要的思想：全局图提供候选，局部图提供确认。
3.3 全局图如何用自然语言召回候选锚点
导航触发由 config/actions.yaml 控制，里面有 calculate_path、get_goal_mode、inquiry_sentence 和 trigger_find_next。Dualmap.monitor_config_file() 周期读取该文件，一旦 calculate_path 为 true，就把 inquiry_sentence 编码成 CLIP 文本特征，再交给全局图检索。CLIP 论文的核心价值在这里体现得很直接：自然语言可以引用视觉概念，模型不必只依赖固定类别 ID。因此，用户输入可以是 “bowl”，也可以是更接近任务语言的 “place to sit” 或 “something to drink from”。
# dualmap/core.py
def convert_inquiry_to_feat(self, inquiry_sentence: str):
text_query_tokenized = self.detector.clip_tokenizer(inquiry_sentence).to("cuda")
text_query_ft = self.detector.clip_model.encode_text(text_query_tokenized)
text_query_ft = text_query_ft / text_query_ft.norm(dim=-1, keepdim=True)
return text_query_ft.squeeze()
GlobalMapManager.find_best_candidate_with_inquiry() 会遍历全局对象，先比较查询文本和锚点自身 CLIP 特征，再比较查询文本和该锚点保存的 related_objs 特征，最终取最大相似度作为该锚点分数。这样，“碗”不一定要求全局图里有一个长期 bowl 对象；它可以通过“桌子锚点 + bowl 相关特征”被召回。代码里还有 ignore_global_obj_list，用于 lost-and-found 场景跳过已尝试候选，继续寻找下一个可能区域。
# utils/global_map_manager.py
max_sim = F.cosine_similarity(
text_query_ft.unsqueeze(0),
obj_feat.unsqueeze(0),
dim=-1,
).item()
if obj.related_objs:
related_sims = []
for related_obj_ft in obj.related_objs:
related_obj_ft_tensor = torch.from_numpy(related_obj_ft).to("cuda")
sim = F.cosine_similarity(
text_query_ft.unsqueeze(0),
related_obj_ft_tensor.unsqueeze(0),
dim=-1,
).item()
related_sims.append(sim)
max_sim = max(max_sim, max(related_sims))
全局查询完成后，还要把候选对象变成可执行的目标点。候选对象的 bbox 中心通常在物体内部，机器人不能直接导航到那里，所以代码会先把 3D 中心转换成 2D 栅格，再沿起点方向或最近自由空间进行吸附，最后把目标落到 Voronoi 图附近。下面的代码片段按 get_goal_position() 的 inquiry 分支整理，体现了“语义目标”和“可达目标”之间的转换。
# 按 utils/global_map_manager.py 源码逻辑整理：从语义候选变成全局路径终点
def get_goal_position_for_inquiry(self, nav_graph, start_position):
# 1. 在全局图中按 CLIP 相似度选择最相关锚点
global_goal_candidate, score = self.find_best_candidate_with_inquiry()
# 2. 保存候选 bbox 和分数，后续局部图会用它们缩小搜索范围
self.global_candidate_bbox = global_goal_candidate.bbox_2d
if self.global_candidate_score == 0.0:
self.global_candidate_score = score
# 3. 加入 ignore 列表，局部失败后可以跳过当前候选，找下一个候选
self.ignore_global_obj_list.append(global_goal_candidate.uid)
# 4. 语义候选的中心点不一定可达，需要从 3D 世界坐标转成 2D 栅格
goal_3d = global_goal_candidate.bbox_2d.get_center()
goal_2d = nav_graph.calculate_pos_2d(goal_3d)
# 5. 如果目标点落在障碍或物体内部，就吸附到自由空间
if not nav_graph.free_space_check(goal_2d):
snapped_goal = nav_graph.snap_to_free_space_directional(
goal_2d,
start_position,
nav_graph.free_space,
)
nearest_node = nav_graph.find_nearest_node(snapped_goal, start_position)
goal_2d = np.array(nearest_node)
return goal_2d
3.4 局部图如何避免“找到以前放过但已经移动了的东西”
全局候选只完成第一阶段，真正避免旧目标误导的是局部确认。全局图选出候选锚点后，会把该对象的 bbox_2d 和分数传给 LocalMapManager。局部路径规划进入 inquiry 模式时，先调用 filter_objects_in_global_bbox()，只在全局候选 bbox 扩展范围内筛选当前局部对象；再用同一个文本特征对这些当前对象做 CLIP 相似度比较；最后还会检查局部候选分数和全局候选分数的差距，差距过大就拒绝生成局部路径。这套流程不保证一定找到目标，但能避免把历史线索直接当成现实。
# utils/local_map_manager.py
candidate_objects = self.filter_objects_in_global_bbox(expand_ratio=0.1)
if len(candidate_objects) == 0:
return None
local_goal_candidate, candidate_score = (
self.find_best_candidate_with_inquiry(candidate_objects)
)
diff_score = abs(candidate_score - self.global_score)
if diff_score > self.cfg.object_matching.score_difference:
return None
局部确认可以理解成“在全局候选锚点附近重新做一次小范围目标搜索”。这一步不再遍历整个全局图，而是只看当前局部图中落入候选 bbox 的对象。下面的代码按源码逻辑整理了 bbox 过滤过程：它只比较 XY 平面范围，因为导航目标主要关心地面平面上的可达位置；Z 方向通常用于判断对象关系和点云高度，不适合作为局部候选过滤的主条件。
# 按 utils/local_map_manager.py 源码逻辑整理：在全局候选 bbox 附近筛选当前局部对象
def filter_objects_in_global_bbox(self, expand_ratio=0.1):
global_min = np.array(self.global_bbox.min_bound)
global_max = np.array(self.global_bbox.max_bound)
# 1. 给全局 bbox 在 XY 平面上扩展一点余量
# 这样可以容忍点云误差、bbox 估计误差和机器人定位误差。
expand_vector = np.array([
(global_max[0] - global_min[0]) * expand_ratio,
(global_max[1] - global_min[1]) * expand_ratio,
0,
])
expanded_min_xy = global_min[:2] - expand_vector[:2]
expanded_max_xy = global_max[:2] + expand_vector[:2]
candidate_objects = []
for obj in self.local_map:
# 2. 观测次数太少的对象不稳定，不参与末端目标确认
if obj.observed_num <= 2:
continue
obj_min_xy = np.array([obj.bbox.min_bound[0], obj.bbox.min_bound[1]])
obj_max_xy = np.array([obj.bbox.max_bound[0], obj.bbox.max_bound[1]])
# 3. 只要局部对象 bbox 与扩展后的全局 bbox 在 XY 上相交，就进入候选集
intersects = (
obj_min_xy[0] <= expanded_max_xy[0]
and obj_max_xy[0] >= expanded_min_xy[0]
and obj_min_xy[1] <= expanded_max_xy[1]
and obj_max_xy[1] >= expanded_min_xy[1]
)
if intersects:
candidate_objects.append(obj)
obj.nav_goal = True
return candidate_objects
如果局部图确认失败，系统需要进入“找下一个候选”的恢复逻辑。当前代码里 trigger_find_next 主要通过 YAML 触发，Dualmap.parallel_process() 在检测到该标志后停止当前局部规划，并让全局图进入 lost-and-found 状态。随后 monitor 线程会把 calculate_path 重新置为 true，触发下一次全局候选搜索。真实机器人接入时，建议把这个触发从手动 YAML 改成 Nav2 执行反馈驱动，例如目标超时、无法前进、没有有效局部路径时自动调用下一候选。
# dualmap/core.py
if self.trigger_find_next:
self.begin_local_planning = False
self.reset_trigger_find_next = True
self.global_map_manager.lost_and_found = True
4. 路径规划与执行接口
4.1 全局路径为什么用 Voronoi 图
DualMap 的全局路径不是直接在原始 3D 点云上搜索。GlobalMapManager.calculate_global_path() 会把全局对象的 pcd_2d、当前位置和 layout 墙体点云合成一个平面占据空间，再交给 NavigationGraph.get_graph() 生成自由空间图。NavigationGraph 先构建 occupancy map，膨胀障碍，取最大连通自由区域，再从自由空间边界生成 Voronoi 图。Voronoi 骨架天然偏向离障碍边界更远的位置，因此适合做全局粗路径：它不追求贴边最短，而是追求在对象级地图中走一条相对稳健的通道。
# utils/global_map_manager.py
total_pcd = o3d.geometry.PointCloud()
for obj in self.global_map:
total_pcd += obj.pcd_2d
curr_point.points = o3d.utility.Vector3dVector([curr_point_coords])
total_pcd += curr_point
total_pcd += self.layout_map.wall_pcd
nav_graph = NavigationGraph(self.cfg, total_pcd, resolution)
nav_graph.get_graph()
路径搜索本身使用 Dijkstra。find_shortest_path() 会先把任意起点和终点吸附到 Voronoi 图上的近邻节点，再用边权 dist 搜索最短拓扑路径，随后做急转弯过滤和平滑，并把栅格坐标转换回世界坐标。Open3D 文档中提到 voxel downsampling 会把点云按体素聚合，DualMap 在全局图中使用 2D 下采样点云也遵循同一工程取向：导航层并不需要保存物体表面的所有细节，它只需要足够表达障碍分布、锚点位置和可通行空间。
# utils/navigation_helper.py
path = nx.dijkstra_path(
self.graph,
source=nearest_start_node,
target=nearest_goal_node,
weight="dist",
)
full_path = [start] + path + [goal]
path = self.remove_sharp_turns(full_path)
path = self.smooth_path(path)
self.pos_path = [self.calculate_pos_3d(x, y) for x, y in path]
占据图构建是 Voronoi 路径的前置步骤。源码会先根据点云范围建立二维栅格，把对象点云和墙体点云投影到栅格中作为占据区域，再对障碍做膨胀，最后取最大连通自由区域。这一步决定了路径规划的几何基础：如果点云、坐标系或分辨率有问题，后续 Dijkstra 即使成功，也可能是在错误地图上成功。
# 按 utils/navigation_helper.py 源码逻辑整理：从点云生成自由空间栅格
def get_occupancy_map(self):
# 1. 初始化二维栅格，0 表示未占据，后续会被转换成自由空间
occupancy_grid_map = np.zeros(self.grid_size[::-1], dtype=int)
# 2. 把点云坐标平移到栅格局部坐标系
point_cloud = np.asarray(self.pcd.points)
point_cloud[:, 0] -= self.pcd_min[0]
point_cloud[:, 1] -= self.pcd_min[1]
# 3. 世界坐标转栅格坐标，并把有点云的位置标记为障碍
x_cells = np.floor(point_cloud[:, 0] / self.cell_size).astype(int)
y_cells = np.floor(point_cloud[:, 1] / self.cell_size).astype(int)
occupancy_grid_map[y_cells, x_cells] = 1
# 4. 膨胀障碍，相当于给机器人 footprint 和定位误差留安全边界
dilation_radius = 10
occupancy_grid_map = cv2.dilate(
occupancy_grid_map.astype(np.uint8),
np.ones((dilation_radius, dilation_radius)),
iterations=1,
)
# 5. 取最大连通自由区域，避免路径跳到孤立空洞中
free_space_map = (occupancy_grid_map == 0).astype(np.uint8)
num_labels, labels = cv2.connectedComponents(free_space_map)
largest_component = max(
range(1, num_labels),
key=lambda label: np.sum(labels == label),
)
self.free_space = (labels == largest_component).astype(np.uint8)
return self.free_space
4.2 局部路径为什么用 RRT-Sharp
全局路径把机器人带到语义锚点附近，但最后一段通常更难：目标可能在桌子边缘、柜子旁边、椅子附近，局部对象点云也会比全局锚点更碎、更密、更动态。DualMap 在局部阶段使用 RRT-Sharp，是因为采样式规划能在复杂局部占据空间里寻找可行路径，不要求先把空间整理成一个干净的全局拓扑图。RRT-Sharp 相比基础 RRT 增加了 rewiring 和代价优化，虽然这份实现仍然是较轻量的版本，但已经足够表达“局部末端接近”的工程意图。
# utils/navigation_helper.py
self.rrt = RRT(
algorithm="rrt_sharp",
max_iter=500,
steer_length=5,
search_radius=5,
goal_sample_rate=0.2,
)
RRT-Sharp 的核心不是“一次算出最短路径”，而是在自由空间中逐步采样、扩展树、接近目标，并通过 rewiring 改善局部代价。源码实现里，goal_sample_rate 会让采样有一定概率偏向目标附近，从而提高收敛速度；steer_length 控制每次扩展距离；search_radius 控制重连优化范围。下面这段整理版代码展示了局部规划为什么适合处理复杂末端空间。
# 按 utils/navigation_helper.py 源码逻辑整理：RRT-Sharp 主循环
def rrt_sharp_plan(self):
for _ in range(self.max_iter):
# 1. 随机采样；有一定概率在目标附近采样，加快向目标收敛
rand_point = self.heuristic_sampling()
# 2. 找到树上离采样点最近的节点
_, nearest_idx = self.kdtree.query(rand_point)
nearest_node = self.tree_nodes[nearest_idx]
# 3. 从最近节点朝采样点扩展一步
new_node = self.steer(nearest_node, rand_point)
# 4. 新节点必须落在自由空间，否则不能加入树
if not self.is_free(*new_node):
continue
# 5. 加入树并记录父节点和路径代价
cost = self.tree_costs[nearest_node] + np.linalg.norm(
np.array(nearest_node) - np.array(new_node)
)
self.tree_nodes.append(new_node)
self.tree_parents[new_node] = nearest_node
self.tree_costs[new_node] = cost
self.kdtree = KDTree(self.tree_nodes)
# 6. 在附近节点之间重连，尝试降低路径代价
self.rewire(new_node)
# 7. 如果已经接近目标，就把目标接到树上并结束
if np.linalg.norm(np.array(new_node) - np.array(self.goal)) <= self.steer_length:
self.tree_nodes.append(self.goal)
self.tree_parents[self.goal] = new_node
break
return self._reconstruct_path()
LocalMapManager.calculate_local_path() 构造局部占据图时，会把当前局部对象点云和全局图的 pcd_2d 一起加入。这样做的含义是：局部路径既考虑当前看到的具体物体，也保留全局锚点和墙体边界对自由空间的约束。进入 inquiry 模式后，局部图先确认目标对象，再把目标 bbox 中心转换成 2D 栅格，并吸附到自由空间。机器人不会直接被规划到碗、桌子或柜子的中心，而是被引导到附近可达点。
# utils/local_map_manager.py
goal_3d = local_goal_candidate.bbox.get_center()
goal_2d = nav_graph.calculate_pos_2d(goal_3d)
snapped_goal = self.nav_graph.snap_to_free_space(
goal_2d,
self.nav_graph.free_space,
)
goal_2d = np.array(snapped_goal)
4.3 最终发布的是 action_path，而不是机器人速度
DualMap 的最终路径由全局路径和局部路径合成。Dualmap.get_action_path() 会在局部路径生成后，把 curr_global_path 与 curr_local_path 拼接，并用 remaining_path() 裁掉机器人已经走过的前缀，再可选移除急转弯。这个 action_path 是一组世界坐标路径点，适合给下游导航系统做参考，但它还不是底盘速度命令。也就是说，DualMap 解决的是“应该去哪、候选目标是什么、路径建议是什么”，不是“下一时刻轮子转多快”。
# dualmap/core.py
if self.start_action_path:
self.action_path = self.curr_global_path + self.curr_local_path[1:]
self.action_path = remaining_path(self.action_path, self.curr_pose)
if self.cfg.use_remove_sharp_turns:
self.action_path = remove_sharp_turns_3d(self.action_path)
self.global_map_manager.action_path = self.action_path
self.global_map_manager.has_action_path = True
ROS2 发布端也体现了这个边界。ROSPublisher 会发布 /global_path、/local_path 和 /action_path，消息类型是 nav_msgs/Path，frame 固定为 "map"。同时，它还能发布局部和全局地图点云到 RViz。这个接口已经足够让外部 adapter 订阅路径，但如果要让机器人真正动起来，还需要把 Path 转成 Nav2 action、路径跟踪请求或自研控制器输入。直接把 Path 当成控制命令是不完整的。
# applications/utils/ros_publisher.py
self.global_path_publisher = node.create_publisher(Path, "/global_path", 10)
self.local_path_publisher = node.create_publisher(Path, "/local_path", 10)
self.action_path_publisher = node.create_publisher(Path, "/action_path", 10)
path_msg.header.frame_id = "map"
for pos in path:
pose_stamped = PoseStamped()
pose_stamped.pose.position.x = pos[0]
pose_stamped.pose.position.y = pos[1]
pose_stamped.pose.position.z = pos[2]
pose_stamped.pose.orientation.w = 1.0
path_msg.poses.append(pose_stamped)
从发布代码也能看出，DualMap 当前没有直接输出 cmd_vel。它把内部世界坐标路径点包装成 nav_msgs/Path，每个点变成一个 PoseStamped，姿态只给了单位四元数。这对可视化和 adapter 足够，但对真实导航还不够，因为 Nav2 controller 需要结合 robot footprint、局部 costmap、目标朝向和速度约束来输出速度。因此，/action_path 更适合作为“路径建议”而不是“控制结果”。
# 按 applications/utils/ros_publisher.py 源码逻辑整理：发布 Path 消息
def _publish_path(self, path, path_type):
if path is None:
return
path_msg = Path()
path_msg.header.stamp = self.node.get_clock().now().to_msg()
path_msg.header.frame_id = "map"
for pos in path:
# DualMap 内部路径点是 [x, y, z] 世界坐标
pose_stamped = PoseStamped()
pose_stamped.header = path_msg.header
pose_stamped.pose.position.x = float(pos[0])
pose_stamped.pose.position.y = float(pos[1])
pose_stamped.pose.position.z = float(pos[2])
# 当前实现没有计算路径朝向，只给默认朝向
# 如果接 Nav2，adapter 可以根据相邻路径点补 yaw。
pose_stamped.pose.orientation.w = 1.0
path_msg.poses.append(pose_stamped)
publisher = {
"global": self.global_path_publisher,
"local": self.local_path_publisher,
"action": self.action_path_publisher,
}.get(path_type)
if publisher is not None:
publisher.publish(path_msg)
5. 接入 2D 建图与 Nav2
5.1 为什么不直接用 Navigation2 取代 Voronoi 和 RRT
这里要区分两件事：语义路径建议和运动执行。Nav2 官方概念文档说明，它围绕 NavigateToPose action、Behavior Tree Navigator、Planner Server、Controller Server、Smoother Server、Behavior Server 等组件组织导航任务；Nav2 Simple Commander 也提供 goToPose()、goThroughPoses()、followPath()、costmap 查询和清理等 API。这些能力非常适合做底盘运动、局部避障、失败恢复和生命周期管理，但它们并不天然知道“碗在哪里”“哪个桌子可能和碗有关”“当前局部图是否重新看到了碗”。
因此，DualMap 里的 Voronoi 和 RRT 更像语义地图内部的规划工具。Voronoi 用于从对象级全局图中快速找一条靠近锚点的粗路径；RRT 用于在局部具体对象图里找末端路径。Nav2 则应该接在后面，负责把目标 pose 或 path 执行成真实运动。如果完全抛弃 DualMap 的内部规划，只把自然语言查询结果直接变成某个坐标交给 Nav2，会丢掉“先到锚点附近，再局部确认高动态目标”的核心机制。
5.2 和 2D 建图、SLAM、Nav2 的推荐接口
…详情请参照古月居
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
3
0- 0
已为社区贡献131条内容
所有评论(0)