From d2481684ac007b8966e7de2a66f0f00cd875bd2d Mon Sep 17 00:00:00 2001
From: SikongJueluo <selfconfusion@gmail.com>
Date: Tue, 24 Mar 2026 17:39:00 +0800
Subject: [PATCH] feat(scenegraph): add basic scene graph data structures

---
 mini-nav/scenegraph/__init__.py   |  12 +++
 mini-nav/scenegraph/objectnode.py |  22 +++++
 mini-nav/scenegraph/roomnode.py   |  12 +++
 mini-nav/scenegraph/scenegraph.py |  10 ++
 notebooks/test.py                 | 159 ++++++++++++++++++++++++++++++
 notebooks/verification.py         |  23 +++++
 6 files changed, 238 insertions(+)
 create mode 100644 mini-nav/scenegraph/__init__.py
 create mode 100644 mini-nav/scenegraph/objectnode.py
 create mode 100644 mini-nav/scenegraph/roomnode.py
 create mode 100644 mini-nav/scenegraph/scenegraph.py
 create mode 100644 notebooks/test.py
 create mode 100644 notebooks/verification.py

diff --git a/mini-nav/scenegraph/__init__.py b/mini-nav/scenegraph/__init__.py
new file mode 100644
index 0000000..a94d20e
--- /dev/null
+++ b/mini-nav/scenegraph/__init__.py
@@ -0,0 +1,12 @@
+"""Scenegraph public API.
+
+This module exports the main scenegraph objects for easy import:
+
+from mini_nav.scenegraph import SimpleSceneGraph, RoomNode, ObjectNode
+"""
+
+from .objectnode import ObjectNode
+from .roomnode import RoomNode
+from .scenegraph import SimpleSceneGraph
+
+__all__ = ["ObjectNode", "RoomNode", "SimpleSceneGraph"]
diff --git a/mini-nav/scenegraph/objectnode.py b/mini-nav/scenegraph/objectnode.py
new file mode 100644
index 0000000..7e0a5a6
--- /dev/null
+++ b/mini-nav/scenegraph/objectnode.py
@@ -0,0 +1,22 @@
+from dataclasses import dataclass
+
+import numpy as np
+
+
+# --- 1. 物品节点：极其扁平，只存“当前最新/平均”状态 ---
+@dataclass
+class ObjectNode:
+    obj_id: str  # 唯一ID (例如: "obj_001")
+    obj_id: str  # 唯一ID (例如: "obj_001")
+    room_id: str  # 所属房间的直接外键，不搞复杂的层级关系
+
+    # 位置：在 debug 阶段，一切认准全局坐标，不要搞局部坐标系
+    position: np.ndarray  # [x, y, z] 世界坐标系下的中心点或锚点
+
+    # 特征：直接存你压缩后的 512bit 结果，不搞历史缓存
+    visual_hash: np.ndarray  # 512bit 视觉特征 (用于外观检索)
+    semantic_hash: np.ndarray  # 512bit 语义特征 (用于类别对齐)
+
+    # Debug 必备：极简的生命周期管理，防止满屏“幽灵节点”
+    hit_count: int = 1  # 被观测到的次数。太低的可以直接过滤掉
+    last_seen_frame: int = 0  # 最后一次看到的帧号或时间戳
diff --git a/mini-nav/scenegraph/roomnode.py b/mini-nav/scenegraph/roomnode.py
new file mode 100644
index 0000000..702dc7e
--- /dev/null
+++ b/mini-nav/scenegraph/roomnode.py
@@ -0,0 +1,12 @@
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass
+class RoomNode:
+    room_id: str  # 例如: "room_A"
+
+    # 范围：不用复杂的 Polygon，用一个中心点+半径，或者简单的 3D BBox 足够了
+    center: np.ndarray  # [x, y, z]
+    bbox_extent: np.ndarray  # [dx, dy, dz] 用于快速判断一个点在不在房间里
diff --git a/mini-nav/scenegraph/scenegraph.py b/mini-nav/scenegraph/scenegraph.py
new file mode 100644
index 0000000..9f5ac9d
--- /dev/null
+++ b/mini-nav/scenegraph/scenegraph.py
@@ -0,0 +1,10 @@
+from dataclasses import dataclass, field
+
+from .roomnode import RoomNode
+from .objectnode import ObjectNode
+
+
+@dataclass
+class SimpleSceneGraph:
+    rooms: dict[str, RoomNode] = field(default_factory=dict)
+    objects: dict[str, ObjectNode] = field(default_factory=dict)
diff --git a/notebooks/test.py b/notebooks/test.py
new file mode 100644
index 0000000..9259a29
--- /dev/null
+++ b/notebooks/test.py
@@ -0,0 +1,159 @@
+import marimo
+
+__generated_with = "0.21.1"
+app = marimo.App()
+
+
+@app.cell
+def _():
+    import habitat_sim
+    from habitat_sim import ShortestPath
+    import numpy as np
+    import plotly.express as px
+    from matplotlib import pyplot as plt
+
+    return ShortestPath, habitat_sim, np, plt
+
+
+@app.cell
+def _(habitat_sim):
+    # 配置场景
+    scene_path = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"
+
+    sim_cfg = habitat_sim.SimulatorConfiguration()
+    sim_cfg.scene_id = scene_path
+    sim_cfg.enable_physics = False
+    return (sim_cfg,)
+
+
+@app.cell
+def _(habitat_sim, sim_cfg):
+    # 配置 agent
+    agent_cfg = habitat_sim.agent.AgentConfiguration()
+    rgb_sensor_spec = habitat_sim.CameraSensorSpec()
+    rgb_sensor_spec.uuid = "color_sensor"
+    rgb_sensor_spec.sensor_type = habitat_sim.SensorType.COLOR
+    rgb_sensor_spec.resolution = [256, 256]
+    rgb_sensor_spec.position = [0.0, 1.5, 0.0]
+
+    agent_cfg.sensor_specifications = [rgb_sensor_spec]
+
+    # 创建 simulator 实例
+    cfg = habitat_sim.Configuration(sim_cfg, [agent_cfg])
+    sim = habitat_sim.Simulator(cfg)
+    return (sim,)
+
+
+@app.cell
+def _(habitat_sim, plt, sim):
+    # 初始化 agent
+    agent = sim.initialize_agent(0)
+
+    # 设置 agent 初始位置：使用可导航点，而不是写死 [0,0,0]
+    agent_state = habitat_sim.AgentState()
+    agent_state.position = sim.pathfinder.get_random_navigable_point()
+    agent.set_state(agent_state)
+
+    state = agent.get_state()
+    print("位置:", state.position)
+    print("旋转四元数:", state.rotation)
+
+    observations = sim.get_sensor_observations()
+    rgb_image = observations["color_sensor"]
+    print("RGB shape:", rgb_image.shape)
+    print("RGB min/max:", rgb_image[..., :3].min(), rgb_image[..., :3].max())
+
+
+    plt.imshow(rgb_image)
+    plt.axis("off")
+    plt.show()
+    return (agent,)
+
+
+@app.cell
+def _(agent, plt, sim):
+    action_names = list(agent.agent_config.action_space.keys())
+    print("可用动作:", action_names)
+
+    # 进行一步移动
+    sim.step("move_forward")
+
+    import random
+    6
+    for i in range(5):
+        action = random.choice(action_names)
+        obs = sim.step(action)
+        rgb = obs["color_sensor"]
+        plt.imshow(rgb)
+        plt.axis("off")
+        plt.show()
+    return
+
+
+@app.cell
+def _(sim):
+    pathfinder = sim.pathfinder
+    print("NavMesh是否加载:", pathfinder.is_loaded)
+    print("可导航面积:", pathfinder.navigable_area)
+    print("场景边界:", pathfinder.get_bounds())
+
+    # 获取随机可导航点
+    rand_point = pathfinder.get_random_navigable_point()
+    print("随机可导航点:", rand_point)
+    return (pathfinder,)
+
+
+@app.cell
+def _(ShortestPath, pathfinder):
+    start = pathfinder.get_random_navigable_point()
+    end = pathfinder.get_random_navigable_point()
+
+    path = ShortestPath()
+    path.requested_start = start
+    path.requested_end = end
+
+    found = pathfinder.find_path(path)
+    print("是否找到路径:", found)
+    print("路径点:", path.points)
+    print("路径长度:", path.geodesic_distance)
+    return path, start
+
+
+@app.cell
+def _(path, pathfinder, plt, sim, start):
+    from habitat.utils.visualizations import maps
+
+    top_down_map = maps.get_topdown_map(
+        pathfinder, height=start[1], meters_per_pixel=0.05
+    )
+
+    # 绘制路径
+    trajectory = [
+        maps.to_grid(p[2], p[0], top_down_map.shape, pathfinder=sim.pathfinder)
+        for p in path.points
+    ]
+    maps.draw_path(top_down_map, trajectory)
+
+    plt.imshow(top_down_map)
+    plt.axis("off")
+    plt.show()
+    return
+
+
+@app.cell
+def _(habitat_sim, np):
+    # 高级控制：连续动作和滑动碰撞
+
+    vel_control = habitat_sim.physics.VelocityControl()
+    vel_control.controlling_lin_vel = True
+    vel_control.lin_vel_is_local = True
+    vel_control.controlling_ang_vel = True
+
+    # 设置线速度和角速度
+    vel_control.linear_velocity = np.array([0, 0, -1.0])  # forward
+    vel_control.angular_velocity = np.array([0, 0.1, 0])  # rotation
+    return
+
+
+if __name__ == "__main__":
+    app.run()
diff --git a/notebooks/verification.py b/notebooks/verification.py
new file mode 100644
index 0000000..1e04b06
--- /dev/null
+++ b/notebooks/verification.py
@@ -0,0 +1,23 @@
+import marimo
+
+__generated_with = "0.21.1"
+app = marimo.App()
+
+
+@app.cell
+def _():
+    import marimo as mo
+    from scenegraph import ObjectNode, RoomNode
+
+
+
+    return
+
+
+@app.cell
+def _():
+    return
+
+
+if __name__ == "__main__":
+    app.run()