mirror of
https://github.com/SikongJueluo/Mini-Nav.git
synced 2026-07-13 04:25:32 +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:
@@ -3,6 +3,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from math import radians, tan
|
||||
from typing import Any, Literal, Sequence
|
||||
|
||||
import numpy as np
|
||||
@@ -18,31 +19,43 @@ class SceneGraphBuildConfig:
|
||||
"""Configuration for the scene graph builder.
|
||||
|
||||
Attributes:
|
||||
position_strategy: How object positions are assigned. Only 'room_center'
|
||||
is supported in M0.
|
||||
position_strategy: How object positions are assigned.
|
||||
'room_center' uses the room's center point.
|
||||
'bbox_depth_center' uses depth at the bbox center to compute
|
||||
a 3D position via pinhole projection.
|
||||
inference_batch_size: Batch size passed to the pipeline.
|
||||
enable_fusion: Whether to fuse overlapping detections (not yet supported).
|
||||
fusion_hash_similarity_threshold: Hash similarity threshold for fusion.
|
||||
fusion_distance_threshold_m: Distance threshold in meters for fusion.
|
||||
camera_hfov_degrees: Horizontal field of view of the camera in degrees.
|
||||
Only used when position_strategy='bbox_depth_center'.
|
||||
"""
|
||||
|
||||
position_strategy: Literal["room_center"] = "room_center"
|
||||
position_strategy: Literal["room_center", "bbox_depth_center"] = "room_center"
|
||||
inference_batch_size: int = 4
|
||||
enable_fusion: bool = False
|
||||
fusion_hash_similarity_threshold: float = 0.95
|
||||
fusion_distance_threshold_m: float = 0.5
|
||||
camera_hfov_degrees: float = 90.0
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.position_strategy != "room_center":
|
||||
valid_strategies = {"room_center", "bbox_depth_center"}
|
||||
if self.position_strategy not in valid_strategies:
|
||||
raise ValueError(
|
||||
f"position_strategy must be 'room_center', got {self.position_strategy!r}"
|
||||
f"position_strategy must be one of {valid_strategies}, "
|
||||
f"got {self.position_strategy!r}"
|
||||
)
|
||||
if not 0 < self.camera_hfov_degrees < 180:
|
||||
raise ValueError(
|
||||
f"camera_hfov_degrees must be in (0, 180), "
|
||||
f"got {self.camera_hfov_degrees}"
|
||||
)
|
||||
if self.inference_batch_size <= 0:
|
||||
raise ValueError(
|
||||
f"inference_batch_size must be positive, got {self.inference_batch_size}"
|
||||
)
|
||||
if self.enable_fusion:
|
||||
raise ValueError("fusion is not yet supported in M0")
|
||||
raise ValueError("fusion is not yet supported in M0/M1")
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -247,10 +260,15 @@ class SceneGraphBuilder:
|
||||
meta=meta,
|
||||
selected_idx=selected_idx,
|
||||
)
|
||||
position, position_confidence = self._position_for_detection(
|
||||
view=view,
|
||||
room=room,
|
||||
bbox_xyxy=bbox_xyxy,
|
||||
)
|
||||
return ObjectNode(
|
||||
obj_id=obj_id,
|
||||
room_id=view.room_id,
|
||||
position=room.center.copy(),
|
||||
position=position,
|
||||
visual_hash=hash_bytes,
|
||||
semantic_hash=hash_bytes,
|
||||
hit_count=1,
|
||||
@@ -259,7 +277,7 @@ class SceneGraphBuilder:
|
||||
confidence=confidence,
|
||||
bbox_xyxy=bbox_xyxy,
|
||||
source_view_id=self._source_view_id(view),
|
||||
position_confidence=0.0,
|
||||
position_confidence=position_confidence,
|
||||
)
|
||||
|
||||
def _metadata_for_detection(
|
||||
@@ -287,7 +305,234 @@ class SceneGraphBuilder:
|
||||
)
|
||||
return values[selected_idx]
|
||||
|
||||
def _normalize_bbox(self, box: Any) -> tuple[float, float, float, float]:
|
||||
if len(box) != 4:
|
||||
raise ValueError(f"bbox entry has length {len(box)}, expected 4")
|
||||
return tuple(float(x) for x in box)
|
||||
def _normalize_bbox(self, box: Any) -> tuple[float, float, float, float] | None:
|
||||
try:
|
||||
if len(box) != 4:
|
||||
return None
|
||||
result = tuple(float(x) for x in box)
|
||||
if not all(np.isfinite(result)):
|
||||
return None
|
||||
return result
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
def _position_for_detection(
|
||||
self,
|
||||
*,
|
||||
view: Any,
|
||||
room: RoomNode,
|
||||
bbox_xyxy: tuple[float, float, float, float] | None,
|
||||
) -> tuple[np.ndarray, float]:
|
||||
"""Compute object position and confidence based on the configured strategy.
|
||||
|
||||
Returns:
|
||||
A tuple of (position: np.ndarray (3,), confidence: float).
|
||||
"""
|
||||
if self._config.position_strategy == "room_center":
|
||||
return room.center.copy(), 0.0
|
||||
|
||||
# bbox_depth_center strategy
|
||||
if bbox_xyxy is not None:
|
||||
position = self._compute_bbox_depth_center_position(view, bbox_xyxy)
|
||||
if position is not None:
|
||||
return position, 1.0
|
||||
|
||||
# Fallback: room center with 0.0 confidence
|
||||
return room.center.copy(), 0.0
|
||||
|
||||
def _compute_bbox_depth_center_position(
|
||||
self,
|
||||
view: Any,
|
||||
bbox_xyxy: tuple[float, float, float, float],
|
||||
) -> np.ndarray | None:
|
||||
"""Compute 3D world position from depth at the bbox centre.
|
||||
|
||||
Uses the configured camera_hfov_degrees for pinhole projection.
|
||||
Prefers camera pose (camera_position/camera_rotation) when available
|
||||
on the view; falls back to room center + 0.0 confidence when camera
|
||||
pose is absent (agent pose alone is insufficient because the Habitat
|
||||
depth sensor is offset from the agent body by sensor_height).
|
||||
|
||||
Returns:
|
||||
A (3,) float32 position array, or None if computation fails.
|
||||
"""
|
||||
depth = getattr(view, "depth", None)
|
||||
|
||||
if depth is None:
|
||||
return None
|
||||
|
||||
# Prefer camera pose when available. Agent pose alone is insufficient
|
||||
# because the Habitat depth sensor is offset [0.0, sensor_height, 0.0]
|
||||
# from the agent body, and we have no way to recover the offset.
|
||||
camera_position = getattr(view, "camera_position", None)
|
||||
camera_rotation = getattr(view, "camera_rotation", None)
|
||||
|
||||
if camera_position is not None and camera_rotation is not None:
|
||||
origin_position = camera_position
|
||||
origin_rotation = camera_rotation
|
||||
else:
|
||||
# No camera pose – cannot confidently place in world space.
|
||||
return None
|
||||
|
||||
# Coerce depth to ndarray; require 2D
|
||||
try:
|
||||
depth = np.asarray(depth)
|
||||
except (ValueError, TypeError):
|
||||
return None
|
||||
if depth.ndim != 2:
|
||||
return None
|
||||
|
||||
height, width = depth.shape
|
||||
|
||||
# Validate bbox values are finite before rounding
|
||||
try:
|
||||
x1, y1, x2, y2 = map(float, bbox_xyxy)
|
||||
except (ValueError, TypeError):
|
||||
return None
|
||||
if not all(np.isfinite(v) for v in (x1, y1, x2, y2)):
|
||||
return None
|
||||
|
||||
u = int(round((x1 + x2) / 2.0))
|
||||
v = int(round((y1 + y2) / 2.0))
|
||||
|
||||
# Out of bounds check
|
||||
if not (0 <= u < width and 0 <= v < height):
|
||||
return None
|
||||
|
||||
depth_value = depth[v, u]
|
||||
|
||||
# Depth must be finite and positive
|
||||
if not np.isfinite(depth_value) or depth_value <= 0:
|
||||
return None
|
||||
|
||||
# Convert rotation to matrix
|
||||
rotation_matrix = self._rotation_to_matrix(origin_rotation)
|
||||
if rotation_matrix is None:
|
||||
return None
|
||||
|
||||
# Coerce origin_position safely
|
||||
try:
|
||||
origin_pos = np.asarray(origin_position, dtype=np.float32).reshape(3)
|
||||
except (ValueError, TypeError, RuntimeError):
|
||||
return None
|
||||
if not np.all(np.isfinite(origin_pos)):
|
||||
return None
|
||||
|
||||
# Pinhole projection
|
||||
camera_point = self._pixel_depth_to_camera_point(
|
||||
u=u, v=v, depth=depth_value, width=width, height=height,
|
||||
)
|
||||
|
||||
# World position
|
||||
world_position = origin_pos + rotation_matrix @ camera_point
|
||||
|
||||
if not np.all(np.isfinite(world_position)):
|
||||
return None
|
||||
|
||||
return np.asarray(world_position, dtype=np.float32)
|
||||
|
||||
def _pixel_depth_to_camera_point(
|
||||
self,
|
||||
u: int,
|
||||
v: int,
|
||||
depth: float,
|
||||
width: int,
|
||||
height: int,
|
||||
) -> np.ndarray:
|
||||
"""Convert a pixel with depth to a camera-space 3D point via pinhole model.
|
||||
|
||||
M1 assumes Habitat camera forward is local -Z; this convention should be
|
||||
validated with a real Habitat sanity check.
|
||||
|
||||
Args:
|
||||
u: Pixel column (x) coordinate.
|
||||
v: Pixel row (y) coordinate.
|
||||
depth: Depth value at the pixel (positive, finite).
|
||||
width: Image width in pixels.
|
||||
height: Image height in pixels.
|
||||
|
||||
Returns:
|
||||
A (3,) float32 array [x, y, z] in camera space.
|
||||
"""
|
||||
hfov = radians(self._config.camera_hfov_degrees)
|
||||
fx = width / (2.0 * tan(hfov / 2.0))
|
||||
fy = fx
|
||||
cx = (width - 1) / 2.0
|
||||
cy = (height - 1) / 2.0
|
||||
|
||||
x = (u - cx) / fx * depth
|
||||
y = -(v - cy) / fy * depth
|
||||
z = -depth
|
||||
|
||||
return np.array([x, y, z], dtype=np.float32)
|
||||
|
||||
def _rotation_to_matrix(
|
||||
self,
|
||||
rotation: Any,
|
||||
) -> np.ndarray | None:
|
||||
"""Convert a rotation object to a (3, 3) float32 rotation matrix.
|
||||
|
||||
Supports:
|
||||
- Objects with a ``.rotation_matrix`` attribute (e.g. habitat_sim style).
|
||||
- ndarray or array-like objects directly convertible to (3, 3).
|
||||
- ``numpy-quaternion`` quaternion objects (optional dependency; uses
|
||||
``importlib`` to load ``quaternion.as_rotation_matrix``).
|
||||
- Objects with a ``.transform_vector()`` method (quaternion-like);
|
||||
constructs the matrix columns by transforming local basis vectors.
|
||||
|
||||
Returns:
|
||||
A (3, 3) float32 matrix, or None if conversion fails.
|
||||
"""
|
||||
try:
|
||||
# Path a: object with .rotation_matrix attribute
|
||||
if hasattr(rotation, "rotation_matrix"):
|
||||
mat = self._validate_matrix(rotation.rotation_matrix)
|
||||
if mat is not None:
|
||||
return mat
|
||||
return None
|
||||
|
||||
# Path b: direct ndarray or array-like (3, 3)
|
||||
mat = self._validate_matrix(rotation)
|
||||
if mat is not None:
|
||||
return mat
|
||||
|
||||
# Path c: numpy-quaternion quaternion object (optional dependency)
|
||||
try:
|
||||
import importlib as _il
|
||||
qmod = _il.import_module("quaternion")
|
||||
if hasattr(qmod, "as_rotation_matrix"):
|
||||
mat = self._validate_matrix(qmod.as_rotation_matrix(rotation))
|
||||
if mat is not None:
|
||||
return mat
|
||||
except (ImportError, AttributeError, TypeError, ValueError):
|
||||
pass
|
||||
|
||||
# Path d: quaternion-like with .transform_vector method
|
||||
if hasattr(rotation, "transform_vector"):
|
||||
eye = np.eye(3, dtype=np.float32)
|
||||
cols: list[np.ndarray] = []
|
||||
for i in range(3):
|
||||
col_vec = rotation.transform_vector(eye[:, i])
|
||||
col = np.asarray(col_vec, dtype=np.float32).ravel()
|
||||
if col.shape != (3,) or not np.all(np.isfinite(col)):
|
||||
return None
|
||||
cols.append(col)
|
||||
return np.column_stack(cols)
|
||||
|
||||
return None
|
||||
except (ValueError, TypeError, RuntimeError):
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _validate_matrix(mat: Any) -> np.ndarray | None:
|
||||
"""Validate and convert a candidate to a (3, 3) float32 matrix.
|
||||
|
||||
Returns the matrix if it is (3, 3) with all finite values, else None.
|
||||
"""
|
||||
try:
|
||||
mat = np.asarray(mat, dtype=np.float32)
|
||||
if mat.shape == (3, 3) and np.all(np.isfinite(mat)):
|
||||
return mat
|
||||
except (ValueError, TypeError, RuntimeError):
|
||||
pass
|
||||
return None
|
||||
|
||||
Reference in New Issue
Block a user