mirror of
https://github.com/SikongJueluo/Mini-Nav.git
synced 2026-07-12 20:15:31 +08:00
feat(scenegraph): add depth-based 3D positioning via pinhole projection
- Add bbox_depth_center position strategy in SceneGraphBuilder using depth at bbox centre and configurable camera_hfov_degrees for pinhole projection. - Add optional depth_sensor_uuid to HabitatSimulatorConfig; create depth sensor spec alongside RGB sensor. - Add camera_position/camera_rotation fields to RoomView; capture pose from sensor_states when depth sensor is available. - Update flatten_room_views for backward compatibility with legacy tuple format. - Wired in depth sensor and bbox_depth_center strategy in verification notebook. - Add tests for depth sensor support and new position strategies.
This commit is contained in:
@@ -15,6 +15,26 @@ class HabitatSimulatorConfig:
|
||||
enable_physics: bool = False
|
||||
sensor_uuid: str = "color_sensor"
|
||||
agent_id: int = 0
|
||||
depth_sensor_uuid: str | None = "depth_sensor"
|
||||
hfov_degrees: float = 90.0
|
||||
|
||||
|
||||
def _camera_sensor_spec(
|
||||
habitat_sim_module: Any,
|
||||
*,
|
||||
uuid: str,
|
||||
sensor_type: Any,
|
||||
image_size: int,
|
||||
sensor_height: float,
|
||||
hfov_degrees: float,
|
||||
) -> Any:
|
||||
spec = habitat_sim_module.CameraSensorSpec()
|
||||
spec.uuid = uuid
|
||||
spec.sensor_type = sensor_type
|
||||
spec.resolution = [image_size, image_size]
|
||||
spec.position = [0.0, sensor_height, 0.0]
|
||||
spec.hfov = float(hfov_degrees)
|
||||
return spec
|
||||
|
||||
|
||||
def create_habitat_simulator(
|
||||
@@ -30,6 +50,11 @@ def create_habitat_simulator(
|
||||
if config.move_forward_step <= 0:
|
||||
raise ValueError("move_forward_step must be greater than 0")
|
||||
|
||||
if not (0 < config.hfov_degrees < 180):
|
||||
raise ValueError(
|
||||
f"hfov_degrees must be in (0, 180), got {config.hfov_degrees}"
|
||||
)
|
||||
|
||||
if habitat_sim_module is None:
|
||||
habitat_sim_module = import_module("habitat_sim")
|
||||
|
||||
@@ -38,12 +63,31 @@ def create_habitat_simulator(
|
||||
sim_cfg.enable_physics = config.enable_physics
|
||||
|
||||
agent_cfg = habitat_sim_module.agent.AgentConfiguration()
|
||||
rgb_sensor_spec = habitat_sim_module.CameraSensorSpec()
|
||||
rgb_sensor_spec.uuid = config.sensor_uuid
|
||||
rgb_sensor_spec.sensor_type = habitat_sim_module.SensorType.COLOR
|
||||
rgb_sensor_spec.resolution = [config.image_size, config.image_size]
|
||||
rgb_sensor_spec.position = [0.0, config.sensor_height, 0.0]
|
||||
agent_cfg.sensor_specifications = [rgb_sensor_spec]
|
||||
|
||||
sensor_specs = [
|
||||
_camera_sensor_spec(
|
||||
habitat_sim_module,
|
||||
uuid=config.sensor_uuid,
|
||||
sensor_type=habitat_sim_module.SensorType.COLOR,
|
||||
image_size=config.image_size,
|
||||
sensor_height=config.sensor_height,
|
||||
hfov_degrees=config.hfov_degrees,
|
||||
),
|
||||
]
|
||||
|
||||
if config.depth_sensor_uuid is not None:
|
||||
sensor_specs.append(
|
||||
_camera_sensor_spec(
|
||||
habitat_sim_module,
|
||||
uuid=config.depth_sensor_uuid,
|
||||
sensor_type=habitat_sim_module.SensorType.DEPTH,
|
||||
image_size=config.image_size,
|
||||
sensor_height=config.sensor_height,
|
||||
hfov_degrees=config.hfov_degrees,
|
||||
),
|
||||
)
|
||||
|
||||
agent_cfg.sensor_specifications = sensor_specs
|
||||
|
||||
turn_angle = 360.0 / config.views_per_room
|
||||
agent_cfg.action_space = {
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
from dataclasses import dataclass, field
|
||||
from importlib import import_module
|
||||
from pathlib import Path
|
||||
@@ -20,16 +21,32 @@ class RoomView:
|
||||
depth: Any | None = field(default=None, compare=False)
|
||||
agent_position: np.ndarray | None = field(default=None, compare=False)
|
||||
agent_rotation: Any | None = field(default=None, compare=False)
|
||||
camera_position: Any | None = field(default=None, compare=False)
|
||||
camera_rotation: Any | None = field(default=None, compare=False)
|
||||
|
||||
|
||||
def flatten_room_views(room_views_by_room: RoomViewsByRoom) -> list[RoomView]:
|
||||
result: list[RoomView] = []
|
||||
for room_id, views in room_views_by_room.items():
|
||||
for idx, rgb in enumerate(views):
|
||||
result.append(RoomView(room_id=room_id, view_idx=idx, rgb=rgb))
|
||||
for idx, item in enumerate(views):
|
||||
if isinstance(item, RoomView):
|
||||
result.append(item)
|
||||
else:
|
||||
result.append(RoomView(room_id=room_id, view_idx=idx, rgb=item))
|
||||
return result
|
||||
|
||||
|
||||
def _snapshot_rotation(rotation: Any) -> Any:
|
||||
"""Create an independent copy of *rotation* to prevent mutation aliasing.
|
||||
|
||||
If the value exposes a ``.copy()`` method (e.g. numpy arrays) it is
|
||||
preferred; otherwise a stdlib shallow copy is used as a fallback.
|
||||
"""
|
||||
if hasattr(rotation, "copy") and callable(rotation.copy):
|
||||
return rotation.copy()
|
||||
return copy.copy(rotation)
|
||||
|
||||
|
||||
def collect_room_views_by_room(
|
||||
agent: Any,
|
||||
sim: Any,
|
||||
@@ -38,6 +55,7 @@ def collect_room_views_by_room(
|
||||
*,
|
||||
habitat_sim_module: Any | None = None,
|
||||
sensor_uuid: str = "color_sensor",
|
||||
depth_sensor_uuid: str | None = None,
|
||||
turn_action: str = "turn_left",
|
||||
progress_description: str = "Collecting room views",
|
||||
progress_track: ProgressTrack = track,
|
||||
@@ -55,9 +73,32 @@ def collect_room_views_by_room(
|
||||
agent.set_state(agent_state)
|
||||
|
||||
room_views = []
|
||||
for _ in range(views_per_room):
|
||||
for view_idx in range(views_per_room):
|
||||
observations = sim.get_sensor_observations()
|
||||
room_views.append(observations[sensor_uuid])
|
||||
current_state = agent.get_state()
|
||||
agent_position = np.asarray(current_state.position, dtype=np.float32).copy()
|
||||
|
||||
# Capture camera pose from sensor_states when depth sensor is available
|
||||
camera_position = None
|
||||
camera_rotation = None
|
||||
if depth_sensor_uuid is not None:
|
||||
sensor_states = getattr(current_state, "sensor_states", None)
|
||||
if sensor_states is not None and depth_sensor_uuid in sensor_states:
|
||||
sensor_state = sensor_states[depth_sensor_uuid]
|
||||
camera_position = np.asarray(sensor_state.position, dtype=np.float32).copy()
|
||||
camera_rotation = _snapshot_rotation(sensor_state.rotation)
|
||||
|
||||
room_view = RoomView(
|
||||
room_id=room_node.room_id,
|
||||
view_idx=view_idx,
|
||||
rgb=observations[sensor_uuid],
|
||||
depth=observations[depth_sensor_uuid] if depth_sensor_uuid is not None else None,
|
||||
agent_position=agent_position,
|
||||
agent_rotation=_snapshot_rotation(current_state.rotation),
|
||||
camera_position=camera_position,
|
||||
camera_rotation=camera_rotation,
|
||||
)
|
||||
room_views.append(room_view)
|
||||
sim.step(turn_action)
|
||||
|
||||
all_room_views[room_node.room_id] = room_views
|
||||
|
||||
Reference in New Issue
Block a user