From 6a5f8b924160fb328cf70786b2856f5ed3a426ee Mon Sep 17 00:00:00 2001 From: lighthqg Date: Fri, 7 Aug 2026 12:23:32 +0800 Subject: [PATCH 1/6] feat: add deterministic VLN diagnostics --- .../evaluator/utils/diagnostic_logger.py | 373 ++++++++++++++++ .../vln/habitat_vln_evaluator.py | 415 +++++++++++++++++- 2 files changed, 775 insertions(+), 13 deletions(-) create mode 100644 internnav/evaluator/utils/diagnostic_logger.py diff --git a/internnav/evaluator/utils/diagnostic_logger.py b/internnav/evaluator/utils/diagnostic_logger.py new file mode 100644 index 00000000..faf647ed --- /dev/null +++ b/internnav/evaluator/utils/diagnostic_logger.py @@ -0,0 +1,373 @@ +"""Deterministic evaluation helpers and crash-safe VLN diagnostic logging.""" + +from __future__ import annotations + +import hashlib +import json +import os +import random +import time +from collections import deque +from pathlib import Path +from typing import Any, Dict, Iterable, Optional + +import numpy as np +import torch + + +DEFAULT_CRITERIA = { + "collision_streak_min": 2, + "stuck_forward_window": 3, + "stuck_displacement_m": 0.05, + "oscillation_turn_count": 6, + "oscillation_max_step_span": 8, + "no_progress_window": 8, + "no_progress_distance_delta_m": 0.05, +} + + +def seed_everything(seed: int, deterministic: bool = True) -> None: + """Seed every random source used by the evaluator and diffusion policy.""" + seed = int(seed) + random.seed(seed) + np.random.seed(seed) + torch.manual_seed(seed) + if torch.cuda.is_available(): + torch.cuda.manual_seed(seed) + torch.cuda.manual_seed_all(seed) + + if deterministic: + torch.backends.cudnn.benchmark = False + torch.backends.cudnn.deterministic = True + torch.use_deterministic_algorithms(True, warn_only=True) + + +def stable_episode_seed(base_seed: int, scene_id: str, episode_id: Any) -> int: + """Derive a stable per-episode seed without relying on randomized Python hashes.""" + identity = f"{scene_id}:{episode_id}".encode("utf-8") + offset = int.from_bytes(hashlib.sha256(identity).digest()[:4], "big") + return (int(base_seed) + offset) % (2**31 - 1) + + +def _jsonable(value: Any) -> Any: + if isinstance(value, Path): + return str(value) + if isinstance(value, np.ndarray): + return value.tolist() + if isinstance(value, np.generic): + return value.item() + if torch.is_tensor(value): + if value.numel() == 1: + return value.detach().cpu().item() + return value.detach().cpu().tolist() + if isinstance(value, dict): + return {str(key): _jsonable(item) for key, item in value.items()} + if isinstance(value, (list, tuple, deque)): + return [_jsonable(item) for item in value] + if isinstance(value, float) and not np.isfinite(value): + return None + return value + + +def depth_statistics(depth_m: Any) -> Dict[str, Optional[float]]: + """Return compact full-frame and center-crop depth statistics in metres.""" + array = np.asarray(depth_m, dtype=np.float32).squeeze() + if array.ndim != 2: + return {} + + def summarize(values: np.ndarray, prefix: str) -> Dict[str, Optional[float]]: + values = values[np.isfinite(values) & (values > 0)] + if values.size == 0: + return { + f"{prefix}_min_m": None, + f"{prefix}_p10_m": None, + f"{prefix}_median_m": None, + f"{prefix}_near_0_5_ratio": None, + f"{prefix}_near_1_0_ratio": None, + } + return { + f"{prefix}_min_m": float(np.min(values)), + f"{prefix}_p10_m": float(np.percentile(values, 10)), + f"{prefix}_median_m": float(np.median(values)), + f"{prefix}_near_0_5_ratio": float(np.mean(values < 0.5)), + f"{prefix}_near_1_0_ratio": float(np.mean(values < 1.0)), + } + + height, width = array.shape + y0, y1 = int(height * 0.35), max(int(height * 0.65), int(height * 0.35) + 1) + x0, x1 = int(width * 0.35), max(int(width * 0.65), int(width * 0.35) + 1) + stats = summarize(array.reshape(-1), "depth") + stats.update(summarize(array[y0:y1, x0:x1].reshape(-1), "depth_center")) + return stats + + +class DiagnosticLogger: + """Write an episode timeline and derive conservative navigation failure signals.""" + + def __init__( + self, + root_dir: str, + run_metadata: Dict[str, Any], + criteria: Optional[Dict[str, Any]] = None, + ) -> None: + self.root_dir = Path(root_dir) + self.root_dir.mkdir(parents=True, exist_ok=True) + self.criteria = dict(DEFAULT_CRITERIA) + if criteria: + self.criteria.update(criteria) + + metadata = { + **run_metadata, + "criteria": self.criteria, + "created_unix_s": time.time(), + } + self._write_json(self.root_dir / "run.json", metadata) + self.episode_dir: Optional[Path] = None + self.timeline_path: Optional[Path] = None + self.reset_episode_state() + + def reset_episode_state(self) -> None: + self.sequence_id = 0 + self.collision_steps = 0 + self.collision_streak_events = 0 + self.current_collision_streak = 0 + self.max_collision_streak = 0 + self.stuck_events = 0 + self.oscillation_events = 0 + self.no_progress_events = 0 + self.s2_calls = 0 + self.s1_plans = 0 + self.pixel_goal_calls = 0 + self._recent_actions: deque = deque(maxlen=max(16, int(self.criteria["no_progress_window"]))) + self._recent_turns: deque = deque(maxlen=int(self.criteria["oscillation_turn_count"])) + self._last_stuck_step = -1 + self._last_oscillation_step = -1 + self._last_no_progress_step = -1 + + def start_episode(self, episode_metadata: Dict[str, Any]) -> Path: + self.reset_episode_state() + scene = str(episode_metadata["scene_id"]) + episode = str(episode_metadata["episode_id"]) + safe_scene = scene.replace("/", "_").replace("\\", "_") + safe_episode = episode.replace("/", "_").replace("\\", "_") + self.episode_dir = self.root_dir / "episodes" / f"{safe_scene}_{safe_episode}" + self.episode_dir.mkdir(parents=True, exist_ok=True) + (self.episode_dir / "depth").mkdir(exist_ok=True) + (self.episode_dir / "plans").mkdir(exist_ok=True) + self.timeline_path = self.episode_dir / "timeline.jsonl" + self.timeline_path.write_text("", encoding="utf-8") + self._write_json(self.episode_dir / "episode.json", episode_metadata) + self.log("episode_start", **episode_metadata) + return self.episode_dir + + def log(self, event_type: str, **fields: Any) -> Dict[str, Any]: + if self.timeline_path is None: + raise RuntimeError("start_episode must be called before logging") + event = { + "sequence_id": self.sequence_id, + "event_type": event_type, + "time_unix_s": time.time(), + **fields, + } + self.sequence_id += 1 + event = _jsonable(event) + with self.timeline_path.open("a", encoding="utf-8") as handle: + handle.write(json.dumps(event, ensure_ascii=False) + "\n") + handle.flush() + if event_type == "s2_inference": + self.s2_calls += 1 + if fields.get("output_type") == "pixel_goal": + self.pixel_goal_calls += 1 + elif event_type == "s1_plan": + self.s1_plans += 1 + return event + + def save_depth(self, label: str, depth_m: Any, **metadata: Any) -> Dict[str, Any]: + if self.episode_dir is None: + raise RuntimeError("start_episode must be called before saving depth") + array = np.asarray(depth_m, dtype=np.float32).squeeze() + file_path = self.episode_dir / "depth" / f"{label}.npz" + np.savez_compressed(file_path, depth_m=array) + result = { + "depth_file": str(file_path.relative_to(self.root_dir)), + **depth_statistics(array), + **metadata, + } + self.log("depth_snapshot", label=label, **result) + return result + + def save_s1_plan(self, plan_id: int, dp_actions: Any, **metadata: Any) -> Dict[str, Any]: + if self.episode_dir is None: + raise RuntimeError("start_episode must be called before saving an S1 plan") + if torch.is_tensor(dp_actions): + raw = dp_actions.detach().float().cpu().numpy().copy() + else: + raw = np.asarray(dp_actions, dtype=np.float32).copy() + if raw.ndim != 3 or raw.shape[-1] < 2: + raise ValueError(f"Expected S1 trajectories shaped [N, T, >=2], got {raw.shape}") + + scaled = raw.copy() + scaled[:, :, :2] /= 4.0 + cumulative_xy = np.concatenate( + [np.zeros((scaled.shape[0], 1, 2), dtype=np.float32), np.cumsum(scaled[:, :, :2], axis=1)], + axis=1, + ) + mean_xy = np.mean(cumulative_xy, axis=0) + endpoint_spread_m = float(np.mean(np.linalg.norm(cumulative_xy[:, -1] - mean_xy[-1], axis=1))) + file_path = self.episode_dir / "plans" / f"plan_{int(plan_id):04d}.npz" + np.savez_compressed( + file_path, + raw_delta_xyt=raw, + scaled_delta_xyt=scaled, + candidate_xy=cumulative_xy, + mean_xy=mean_xy, + ) + summary = { + "plan_id": int(plan_id), + "plan_file": str(file_path.relative_to(self.root_dir)), + "candidate_count": int(raw.shape[0]), + "trajectory_steps": int(raw.shape[1]), + "mean_endpoint_xy_m": mean_xy[-1].tolist(), + "endpoint_spread_m": endpoint_spread_m, + **metadata, + } + self.log("s1_plan", **summary) + return summary + + def record_env_step( + self, + *, + env_step: int, + decision_step: int, + action: int, + action_name: str, + gps_before: Iterable[float], + gps_after: Iterable[float], + distance_before: Optional[float], + distance_after: Optional[float], + collision: bool, + collision_count: Optional[int], + **fields: Any, + ) -> Dict[str, Any]: + gps_before_array = np.asarray(gps_before, dtype=np.float32) + gps_after_array = np.asarray(gps_after, dtype=np.float32) + displacement = float(np.linalg.norm(gps_after_array - gps_before_array)) + + record = { + "env_step": int(env_step), + "decision_step": int(decision_step), + "action": int(action), + "action_name": action_name, + "gps_before": gps_before_array.tolist(), + "gps_after": gps_after_array.tolist(), + "displacement_m": displacement, + "distance_before_m": distance_before, + "distance_after_m": distance_after, + "collision": bool(collision), + "collision_count": collision_count, + } + + flags = [] + is_navigation_action = action_name in {"FORWARD", "LEFT", "RIGHT"} + if is_navigation_action: + if collision: + self.collision_steps += 1 + self.current_collision_streak += 1 + self.max_collision_streak = max(self.max_collision_streak, self.current_collision_streak) + if self.current_collision_streak == int(self.criteria["collision_streak_min"]): + self.collision_streak_events += 1 + else: + self.current_collision_streak = 0 + self._recent_actions.append(record) + + forward_window = int(self.criteria["stuck_forward_window"]) + recent_forward = list(self._recent_actions)[-forward_window:] + if ( + len(recent_forward) == forward_window + and all(item["action_name"] == "FORWARD" for item in recent_forward) + and decision_step - self._last_stuck_step >= forward_window + ): + total_displacement = float( + np.linalg.norm( + np.asarray(recent_forward[-1]["gps_after"], dtype=np.float32) + - np.asarray(recent_forward[0]["gps_before"], dtype=np.float32) + ) + ) + if total_displacement < float(self.criteria["stuck_displacement_m"]): + self.stuck_events += 1 + self._last_stuck_step = decision_step + flags.append("stuck") + + if action_name in {"LEFT", "RIGHT"}: + self._recent_turns.append((decision_step, action_name)) + required_turns = int(self.criteria["oscillation_turn_count"]) + turns = list(self._recent_turns) + alternating = len(turns) == required_turns and all( + turns[index][1] != turns[index - 1][1] for index in range(1, len(turns)) + ) + if ( + alternating + and turns[-1][0] - turns[0][0] <= int(self.criteria["oscillation_max_step_span"]) + and decision_step - self._last_oscillation_step >= required_turns + ): + self.oscillation_events += 1 + self._last_oscillation_step = decision_step + flags.append("oscillation") + + no_progress_window = int(self.criteria["no_progress_window"]) + recent_progress = list(self._recent_actions)[-no_progress_window:] + if ( + len(recent_progress) == no_progress_window + and recent_progress[0]["distance_before_m"] is not None + and recent_progress[-1]["distance_after_m"] is not None + and decision_step - self._last_no_progress_step >= no_progress_window + ): + improvement = float( + recent_progress[0]["distance_before_m"] - recent_progress[-1]["distance_after_m"] + ) + if improvement < float(self.criteria["no_progress_distance_delta_m"]): + self.no_progress_events += 1 + self._last_no_progress_step = decision_step + flags.append("no_progress") + + return self.log("env_step", **record, flags=flags, **fields) + + def finish_episode(self, metrics: Dict[str, Any], stop_reason: str, **fields: Any) -> Dict[str, Any]: + success = float(metrics.get("success", 0.0)) + low_level_failure_signal = any( + [ + self.collision_streak_events > 0, + self.stuck_events > 0, + self.oscillation_events > 0, + self.no_progress_events > 0, + ] + ) + automatic_candidate = success == 0.0 and self.pixel_goal_calls > 0 and low_level_failure_signal + summary = { + "metrics": metrics, + "stop_reason": stop_reason, + "s2_calls": self.s2_calls, + "s1_plans": self.s1_plans, + "pixel_goal_calls": self.pixel_goal_calls, + "collision_steps": self.collision_steps, + "collision_streak_events": self.collision_streak_events, + "max_collision_streak": self.max_collision_streak, + "stuck_events": self.stuck_events, + "oscillation_events": self.oscillation_events, + "no_progress_events": self.no_progress_events, + "automatic_s1_failure_candidate": automatic_candidate, + "pure_s1_failure": None, + "manual_review_required": automatic_candidate, + **fields, + } + self.log("episode_end", **summary) + if self.episode_dir is None: + raise RuntimeError("start_episode must be called before finishing an episode") + self._write_json(self.episode_dir / "summary.json", summary) + return _jsonable(summary) + + @staticmethod + def _write_json(path: Path, data: Dict[str, Any]) -> None: + temporary_path = path.with_suffix(path.suffix + ".tmp") + temporary_path.write_text(json.dumps(_jsonable(data), ensure_ascii=False, indent=2), encoding="utf-8") + os.replace(temporary_path, path) diff --git a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py index 01378df3..11f453d0 100644 --- a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py +++ b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py @@ -9,6 +9,7 @@ import itertools import random import re +import time from collections import OrderedDict import cv2 @@ -33,6 +34,12 @@ from internnav.configs.evaluator import EvalCfg from internnav.evaluator import DistributedEvaluator, Evaluator +from internnav.evaluator.utils.diagnostic_logger import ( + DiagnosticLogger, + depth_statistics, + seed_everything, + stable_episode_seed, +) from internnav.habitat_extensions.vln.utils import ( get_axis_align_matrix, get_intrinsic_matrix, @@ -70,6 +77,14 @@ def __init__(self, cfg: EvalCfg): self.epoch = args.epoch self.max_steps_per_episode = args.max_steps_per_episode self.output_path = args.output_path + self.base_seed = int(getattr(args, 'seed', 42)) + self.diagnostic_enabled = bool(getattr(args, 'diagnostic_log', False)) + self.diagnostic_dir = getattr(args, 'diagnostic_dir', os.path.join(self.output_path, 'diagnostics')) + self.diagnostic_criteria = getattr(args, 'diagnostic_criteria', None) + + # This happens before model construction. The launch script additionally + # sets PYTHONHASHSEED and CUBLAS_WORKSPACE_CONFIG before Python starts. + seed_everything(self.base_seed) # create habitat config self.config_path = cfg.env.env_settings['config_path'] @@ -78,6 +93,8 @@ def __init__(self, cfg: EvalCfg): self.sim_sensors_config = self.config.habitat.simulator.agents.main_agent.sim_sensors with habitat.config.read_write(self.config): + if hasattr(self.config.habitat, 'seed'): + self.config.habitat.seed = self.base_seed self.config.habitat.task.measurements.update( { "top_down_map": TopDownMapMeasurementConfig( @@ -104,9 +121,13 @@ def __init__(self, cfg: EvalCfg): # init agent and env super().__init__(cfg, init_agent=False) + # DistributedEvaluator seeds NumPy by rank, so restore the requested + # experiment seed before constructing or invoking either model. + seed_everything(self.base_seed) + # ------------------------------------- model ------------------------------------------ self.model_args = argparse.Namespace(**cfg.agent.model_settings) - self.vis_debug = bool(getattr(self.model_args, "vis_debug", False)) + self.vis_debug = bool(getattr(self.model_args, "vis_debug", False) or self.save_video) self.vis_debug_path = getattr(self.model_args, "vis_debug_path", os.path.join(self.output_path, "vis_debug")) processor = AutoProcessor.from_pretrained(self.model_args.model_path) @@ -136,6 +157,26 @@ def __init__(self, cfg: EvalCfg): self.model = model self.processor = processor + self.diagnostic_logger = None + if self.diagnostic_enabled: + self.diagnostic_logger = DiagnosticLogger( + os.path.join(self.diagnostic_dir, f'rank_{self.rank}'), + run_metadata={ + 'run_id': getattr(args, 'run_id', os.environ.get('DUALVLN_RUN_ID', 'diagnostic')), + 'base_seed': self.base_seed, + 'rank': self.rank, + 'local_rank': self.local_rank, + 'mode': self.model_args.mode, + 'model_path': self.model_args.model_path, + 'habitat_config': self.config_path, + 'code_commit': os.environ.get('DUALVLN_CODE_COMMIT'), + 'dataset_id': getattr(args, 'dataset_id', None), + 'deterministic_algorithms': True, + 'cublas_workspace_config': os.environ.get('CUBLAS_WORKSPACE_CONFIG'), + }, + criteria=self.diagnostic_criteria, + ) + # refactor: this part used in three places prompt = "You are an autonomous navigation assistant. Your task is to . Where should you go next to stay on track? Please output the next waypoint\'s coordinates in the image. Please output STOP when you have successfully completed the task." answer = "" @@ -259,6 +300,67 @@ def resume_from_output_path(self) -> None: ndtw.append(res['ndtw']) return sucs, spls, oss, nes, ndtw + @staticmethod + def _collision_values(metrics): + collisions = metrics.get('collisions', {}) if metrics else {} + if isinstance(collisions, dict): + return bool(collisions.get('is_collision', False)), collisions.get('count') + return bool(collisions), None + + def _depth_observation_to_metres(self, depth): + depth = np.asarray(depth).squeeze().astype(np.float32) + return depth * (self._max_depth - self._min_depth) + self._min_depth + + def _diagnostic_env_step(self, action, observations_before, decision_step, phase, **fields): + """Execute one Habitat action and log both model and camera-only steps.""" + metrics_before = self.env.get_metrics() + result = self.env.step(action) + observations_after, _, _, _ = result + + if self.diagnostic_logger is None or observations_after is None: + return result + + metrics_after = self.env.get_metrics() + collision, collision_count = self._collision_values(metrics_after) + gps_before = np.asarray(observations_before.get('gps', [np.nan, np.nan])).reshape(-1) + gps_after = np.asarray(observations_after.get('gps', [np.nan, np.nan])).reshape(-1) + compass_before = np.asarray(observations_before.get('compass', [np.nan])).reshape(-1).tolist() + compass_after = np.asarray(observations_after.get('compass', [np.nan])).reshape(-1).tolist() + depth_m = self._depth_observation_to_metres(observations_after['depth']) + + try: + action_name = action_code(int(action)).name + except ValueError: + action_name = f'UNKNOWN_{int(action)}' + + self.diagnostic_logger.record_env_step( + env_step=self._diagnostic_env_step_id, + decision_step=decision_step, + action=int(action), + action_name=action_name, + gps_before=gps_before, + gps_after=gps_after, + distance_before=metrics_before.get('distance_to_goal'), + distance_after=metrics_after.get('distance_to_goal'), + collision=collision, + collision_count=collision_count, + compass_before=compass_before, + compass_after=compass_after, + phase=phase, + depth=depth_statistics(depth_m), + **fields, + ) + if collision: + self.diagnostic_logger.save_depth( + f'collision_env_{self._diagnostic_env_step_id:04d}', + depth_m, + env_step=self._diagnostic_env_step_id, + decision_step=decision_step, + action=action_name, + ) + self._diagnostic_env_step_id += 1 + return result + def _run_eval_dual_system(self) -> tuple: # noqa: C901 self.model.eval() @@ -281,8 +383,32 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 scene_id = episode.scene_id.split('/')[-2] episode_id = int(episode.episode_id) episode_instruction = episode.instruction.instruction_text + episode_seed = stable_episode_seed(self.base_seed, scene_id, episode_id) + seed_everything(episode_seed) print("episode start", episode_instruction) + self._diagnostic_env_step_id = 0 + s2_call_id = 0 + s1_plan_id = 0 + current_s2_call_id = None + current_s1_plan_id = None + latest_s2_output = None + stop_reason = 'unknown' + if self.diagnostic_logger is not None: + goal_positions = [getattr(goal, 'position', None) for goal in getattr(episode, 'goals', [])] + self.diagnostic_logger.start_episode( + { + 'scene_id': scene_id, + 'episode_id': episode_id, + 'instruction': episode_instruction, + 'base_seed': self.base_seed, + 'episode_seed': episode_seed, + 'start_position': getattr(episode, 'start_position', None), + 'start_rotation': getattr(episode, 'start_rotation', None), + 'goal_positions': goal_positions, + } + ) + # save first frame per rank to validate sim quality os.makedirs(os.path.join(self.output_path, f'check_sim_{self.epoch}'), exist_ok=True) Image.fromarray(observations['rgb']).save( @@ -291,6 +417,8 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 vis_frames = [] step_id = 0 + first_person_frame_id = 0 + top_down_frame_id = 0 vis_writer = None if self.save_video: @@ -346,8 +474,20 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 image = image.resize((self.model_args.resize_w, self.model_args.resize_h)) rgb_list.append(image) - down_observations, _, _, _ = self.env.step(action_code.LOOKDOWN) - down_observations, _, _, _ = self.env.step(action_code.LOOKDOWN) + down_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKDOWN, + observations, + step_id, + 'camera_adjustment', + reason='prepare_s1_depth', + ) + down_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKDOWN, + down_observations, + step_id, + 'camera_adjustment', + reason='prepare_s1_depth', + ) look_down_image = Image.fromarray(down_observations["rgb"]).convert('RGB') depth = down_observations["depth"] @@ -364,8 +504,20 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 look_down_depth = torch.as_tensor(np.ascontiguousarray(look_down_depth)).float() look_down_depth[look_down_depth > 5.0] = 5.0 - self.env.step(action_code.LOOKUP) - self.env.step(action_code.LOOKUP) + up_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKUP, + down_observations, + step_id, + 'camera_adjustment', + reason='restore_horizontal_view', + ) + self._diagnostic_env_step( + action_code.LOOKUP, + up_observations, + step_id, + 'camera_adjustment', + reason='restore_horizontal_view', + ) if len(action_seq) == 0 and pixel_goal is None: if action == action_code.LOOKDOWN: @@ -414,6 +566,9 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 inputs = self.processor(text=[text], images=input_images, return_tensors="pt").to(self.model.device) + current_s2_call_id = s2_call_id + s2_call_id += 1 + s2_started = time.perf_counter() with torch.no_grad(): output_ids = self.model.generate( **inputs, @@ -423,10 +578,12 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 past_key_values=None, return_dict_in_generate=True, ).sequences + s2_generate_ms = (time.perf_counter() - s2_started) * 1000 llm_outputs = self.processor.tokenizer.decode( output_ids[0][inputs.input_ids.shape[1] :], skip_special_tokens=True ) + latest_s2_output = llm_outputs print('step_id:', step_id, 'output text:', llm_outputs) if bool(re.search(r'\d', llm_outputs)): # output pixel goal @@ -437,15 +594,46 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 draw_pixel_goal = True # look down --> horizontal - self.env.step(action_code.LOOKUP) - self.env.step(action_code.LOOKUP) + up_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKUP, + observations, + step_id, + 'camera_adjustment', + reason='restore_from_s2_lookdown', + ) + self._diagnostic_env_step( + action_code.LOOKUP, + up_observations, + step_id, + 'camera_adjustment', + reason='restore_from_s2_lookdown', + ) local_actions = [] pixel_values = inputs.pixel_values image_grid_thw = torch.cat([thw.unsqueeze(0) for thw in inputs.image_grid_thw], dim=0) with torch.no_grad(): + latent_started = time.perf_counter() traj_latents = self.model.generate_latents(output_ids, pixel_values, image_grid_thw) + latent_generate_ms = (time.perf_counter() - latent_started) * 1000 + + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 's2_inference', + s2_call_id=current_s2_call_id, + decision_step=step_id, + raw_output=llm_outputs, + output_type='pixel_goal', + pixel_goal=pixel_goal, + history_frame_ids=history_id if action != action_code.LOOKDOWN else [], + input_image_count=len(input_images), + generated_token_count=int(output_ids.shape[-1] - inputs.input_ids.shape[1]), + generate_ms=s2_generate_ms, + latent_generate_ms=latent_generate_ms, + latent_shape=list(traj_latents.shape), + latent_l2_norm=float(traj_latents.detach().float().norm().item()), + ) # prepocess align with navdp image_dp = torch.tensor(np.array(look_down_image.resize((224, 224)))).to(torch.bfloat16) / 255 @@ -455,10 +643,41 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 pix_goal_depth = copy.copy(depth_dp) depths_dp = torch.stack([pix_goal_depth, depth_dp]).unsqueeze(0).to(self.device) + current_s1_plan_id = s1_plan_id + s1_plan_id += 1 + s1_started = time.perf_counter() with torch.no_grad(): dp_actions = self.model.generate_traj(traj_latents, images_dp, depths_dp) + s1_generate_ms = (time.perf_counter() - s1_started) * 1000 + + if self.diagnostic_logger is not None: + self.diagnostic_logger.save_s1_plan( + current_s1_plan_id, + dp_actions, + s2_call_id=current_s2_call_id, + decision_step=step_id, + pixel_goal=pixel_goal, + replan_reason='new_s2_pixel_goal', + generate_ms=s1_generate_ms, + ) + self.diagnostic_logger.save_depth( + f'plan_{current_s1_plan_id:04d}', + look_down_depth.numpy(), + plan_id=current_s1_plan_id, + s2_call_id=current_s2_call_id, + decision_step=step_id, + ) action_list = traj_to_actions(dp_actions) + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 's1_discretization', + plan_id=current_s1_plan_id, + s2_call_id=current_s2_call_id, + decision_step=step_id, + discrete_actions=[int(item) for item in action_list], + executable_chunk=[int(item) for item in action_list[:MAX_LOCAL_STEPS]], + ) if len(action_list) < MAX_STEPS: action_list += [0] * (MAX_STEPS - len(action_list)) @@ -471,7 +690,26 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 pixel_goal = None output_ids = None action = action_code.LEFT - observations, _, done, _ = self.env.step(action) + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 'controller_fallback', + decision_step=step_id, + s2_call_id=current_s2_call_id, + plan_id=current_s1_plan_id, + reason='initial_s1_plan_started_with_stop', + replacement_action='LEFT', + ) + observations, _, done, _ = self._diagnostic_env_step( + action, + observations, + step_id, + 'model_action', + action_source='s1_stop_fallback', + s2_call_id=current_s2_call_id, + plan_id=current_s1_plan_id, + first_person_frame_id=None, + top_down_frame_id=None, + ) step_id += 1 messages = [] continue @@ -479,12 +717,27 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 else: action_seq = self.parse_actions(llm_outputs) + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 's2_inference', + s2_call_id=current_s2_call_id, + decision_step=step_id, + raw_output=llm_outputs, + output_type='stop' if action_seq == [action_code.STOP] else 'direct_actions', + parsed_actions=[int(item) for item in action_seq], + history_frame_ids=history_id if action != action_code.LOOKDOWN else [], + input_image_count=len(input_images), + generated_token_count=int(output_ids.shape[-1] - inputs.input_ids.shape[1]), + generate_ms=s2_generate_ms, + ) print('actions', action_seq, flush=True) if len(action_seq) != 0: action = action_seq[0] action_seq.pop(0) + action_source = 's2_direct' elif pixel_goal is not None: + action_source = 's1' if len(local_actions) == 0: # navdp local_actions = [] @@ -494,10 +747,41 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 depth_dp = look_down_depth.unsqueeze(-1).to(torch.bfloat16) depths_dp = torch.stack([pix_goal_depth, depth_dp]).unsqueeze(0).to(self.device) + current_s1_plan_id = s1_plan_id + s1_plan_id += 1 + s1_started = time.perf_counter() with torch.no_grad(): dp_actions = self.model.generate_traj(traj_latents, images_dp, depths_dp) + s1_generate_ms = (time.perf_counter() - s1_started) * 1000 + + if self.diagnostic_logger is not None: + self.diagnostic_logger.save_s1_plan( + current_s1_plan_id, + dp_actions, + s2_call_id=current_s2_call_id, + decision_step=step_id, + pixel_goal=pixel_goal, + replan_reason='local_chunk_exhausted', + generate_ms=s1_generate_ms, + ) + self.diagnostic_logger.save_depth( + f'plan_{current_s1_plan_id:04d}', + look_down_depth.numpy(), + plan_id=current_s1_plan_id, + s2_call_id=current_s2_call_id, + decision_step=step_id, + ) action_list = traj_to_actions(dp_actions) + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 's1_discretization', + plan_id=current_s1_plan_id, + s2_call_id=current_s2_call_id, + decision_step=step_id, + discrete_actions=[int(item) for item in action_list], + executable_chunk=[int(item) for item in action_list[:MAX_LOCAL_STEPS]], + ) if len(action_list) < MAX_STEPS: action_list += [0] * (MAX_STEPS - len(action_list)) @@ -528,6 +812,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 continue else: action = 0 + action_source = 'default_stop' info = self.env.get_metrics() @@ -536,6 +821,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 if pixel_goal is not None and flag: cv2.circle(frame, (pixel_goal[0], pixel_goal[1]), radius=8, color=(255, 0, 0), thickness=-1) vis_frames.append(frame) + top_down_frame_id += 1 print("step_id", step_id, "action", action) @@ -550,17 +836,71 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 (0, 255, 0), 2, ) + vis = cv2.putText( + vis, + f"source {action_source} s2 {current_s2_call_id} s1 {current_s1_plan_id}", + (20, 75), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + (0, 255, 0), + 2, + ) + if latest_s2_output: + vis = cv2.putText( + vis, + f"S2: {latest_s2_output[:70]}", + (20, 108), + cv2.FONT_HERSHEY_SIMPLEX, + 0.55, + (0, 255, 255), + 1, + ) if pixel_goal is not None: if draw_pixel_goal: cv2.circle(vis, (pixel_goal[0], pixel_goal[1]), radius=8, color=(255, 0, 0), thickness=-1) vis_writer.append_data(vis) + first_person_frame_id += 1 if action == action_code.LOOKDOWN: - self.env.step(action) - observations, _, done, _ = self.env.step(action) + down_observations, _, _, _ = self._diagnostic_env_step( + action, + observations, + step_id, + 'model_action', + action_source=action_source, + s2_call_id=current_s2_call_id, + plan_id=current_s1_plan_id, + first_person_frame_id=first_person_frame_id - 1 if vis_writer is not None else None, + top_down_frame_id=top_down_frame_id - 1 if self.save_video else None, + ) + observations, _, done, _ = self._diagnostic_env_step( + action, + down_observations, + step_id, + 'model_action', + action_source=action_source, + s2_call_id=current_s2_call_id, + plan_id=current_s1_plan_id, + first_person_frame_id=first_person_frame_id - 1 if vis_writer is not None else None, + top_down_frame_id=top_down_frame_id - 1 if self.save_video else None, + ) flag = True else: - observations, _, done, _ = self.env.step(action) + observations, _, done, _ = self._diagnostic_env_step( + action, + observations, + step_id, + 'model_action', + action_source=action_source, + s2_call_id=current_s2_call_id, + plan_id=current_s1_plan_id, + pixel_goal=pixel_goal, + s2_raw_output=latest_s2_output, + remaining_s2_actions=[int(item) for item in action_seq], + remaining_s1_actions=[int(item) for item in local_actions], + first_person_frame_id=first_person_frame_id - 1 if vis_writer is not None else None, + top_down_frame_id=top_down_frame_id - 1 if self.save_video else None, + ) step_id += 1 messages = [] flag = False @@ -568,6 +908,13 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 # ---------- 3. End of episode ----------- # collect the metric result of this episode and write progress to the output_path/progress.json + if done: + stop_reason = 'habitat_episode_done' + elif step_id > self.max_steps_per_episode: + stop_reason = 'model_decision_limit' + else: + stop_reason = 'evaluation_loop_exit' + process_bar.update(1) # After the episode finishes, collect metrics: @@ -586,6 +933,34 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 f"ne: {metrics['distance_to_goal']}" ) + diagnostic_summary = None + if self.diagnostic_logger is not None: + metric_summary = { + 'success': metrics['success'], + 'spl': metrics['spl'], + 'oracle_success': metrics['oracle_success'], + 'distance_to_goal': metrics['distance_to_goal'], + 'ndtw': metrics.get('ndtw'), + 'collisions': metrics.get('collisions'), + } + diagnostic_summary = self.diagnostic_logger.finish_episode( + metric_summary, + stop_reason, + model_decision_steps=step_id, + habitat_action_steps=self._diagnostic_env_step_id, + first_person_video=os.path.join( + self.vis_debug_path, + f'epoch_{self.epoch}', + f'{scene_id}_{episode_id:04d}.mp4', + ), + top_down_video=os.path.join( + self.output_path, + f'vis_{self.epoch}', + f'{scene_id}', + f'{episode_id:04d}.mp4', + ), + ) + # Write per-episode progress.json entry (still per-rank) result = { "scene_id": scene_id, @@ -599,6 +974,20 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 } if 'ndtw' in metrics: result['ndtw'] = metrics['ndtw'] + if diagnostic_summary is not None: + result.update( + { + 'base_seed': self.base_seed, + 'episode_seed': episode_seed, + 'stop_reason': stop_reason, + 'collision_steps': diagnostic_summary['collision_steps'], + 'collision_streak_events': diagnostic_summary['collision_streak_events'], + 'stuck_events': diagnostic_summary['stuck_events'], + 'oscillation_events': diagnostic_summary['oscillation_events'], + 'no_progress_events': diagnostic_summary['no_progress_events'], + 'automatic_s1_failure_candidate': diagnostic_summary['automatic_s1_failure_candidate'], + } + ) # save current progress os.makedirs(self.output_path, exist_ok=True) @@ -606,7 +995,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 f.write(json.dumps(result) + "\n") # save video - if self.save_video and metrics['success'] == 1.0: + if self.save_video and vis_frames: images_to_video( vis_frames, os.path.join(self.output_path, f'vis_{self.epoch}', f'{scene_id}'), @@ -922,7 +1311,7 @@ def _run_eval_system2(self) -> tuple: os.makedirs(self.output_path, exist_ok=True) with open(os.path.join(self.output_path, 'progress.json'), 'a') as f: f.write(json.dumps(result) + "\n") - if self.save_video and metrics['success'] == 1.0: + if self.save_video and vis_frames: images_to_video( vis_frames, os.path.join(self.output_path, f'vis_{self.epoch}', f'{scene_id}'), From ffceb0a0134660cdd8ac0b517f5566206314897b Mon Sep 17 00:00:00 2001 From: lighthqg Date: Fri, 7 Aug 2026 14:05:20 +0800 Subject: [PATCH 2/6] fix: refine diagnostic signals and overlays --- .../evaluator/utils/diagnostic_logger.py | 18 +++++++++++++-- .../vln/habitat_vln_evaluator.py | 23 ++++++++++++++++++- 2 files changed, 38 insertions(+), 3 deletions(-) diff --git a/internnav/evaluator/utils/diagnostic_logger.py b/internnav/evaluator/utils/diagnostic_logger.py index faf647ed..7e0ee763 100644 --- a/internnav/evaluator/utils/diagnostic_logger.py +++ b/internnav/evaluator/utils/diagnostic_logger.py @@ -21,7 +21,9 @@ "stuck_displacement_m": 0.05, "oscillation_turn_count": 6, "oscillation_max_step_span": 8, - "no_progress_window": 8, + "no_progress_window": 12, + "no_progress_min_forward_actions": 4, + "no_progress_min_path_m": 0.5, "no_progress_distance_delta_m": 0.05, } @@ -322,13 +324,25 @@ def record_env_step( and recent_progress[-1]["distance_after_m"] is not None and decision_step - self._last_no_progress_step >= no_progress_window ): + forward_actions = sum(item["action_name"] == "FORWARD" for item in recent_progress) + path_length = float(sum(item["displacement_m"] for item in recent_progress)) improvement = float( recent_progress[0]["distance_before_m"] - recent_progress[-1]["distance_after_m"] ) - if improvement < float(self.criteria["no_progress_distance_delta_m"]): + if ( + forward_actions >= int(self.criteria["no_progress_min_forward_actions"]) + and path_length >= float(self.criteria["no_progress_min_path_m"]) + and improvement < float(self.criteria["no_progress_distance_delta_m"]) + ): self.no_progress_events += 1 self._last_no_progress_step = decision_step flags.append("no_progress") + record["no_progress_evidence"] = { + "window": no_progress_window, + "forward_actions": forward_actions, + "path_length_m": path_length, + "ne_improvement_m": improvement, + } return self.log("env_step", **record, flags=flags, **fields) diff --git a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py index 11f453d0..dd95ebe3 100644 --- a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py +++ b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py @@ -60,6 +60,27 @@ MAX_LOCAL_STEPS = 4 +def format_s2_output_for_overlay(output): + """Render symbolic S2 actions with OpenCV-safe ASCII labels.""" + output = str(output).strip() + arrow_names = { + '\u2191': 'FORWARD', + '\u2190': 'LEFT', + '\u2192': 'RIGHT', + '\u2193': 'LOOKDOWN', + } + if output and all(character in arrow_names for character in output): + groups = [] + for character, items in itertools.groupby(output): + count = sum(1 for _ in items) + label = arrow_names[character] + groups.append(f'{label} x{count}' if count > 1 else label) + return ' | '.join(groups) + + normalized = ''.join(arrow_names.get(character, character) for character in output) + return normalized.encode('ascii', errors='replace').decode('ascii') + + class action_code(IntEnum): STOP = 0 FORWARD = 1 @@ -848,7 +869,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 if latest_s2_output: vis = cv2.putText( vis, - f"S2: {latest_s2_output[:70]}", + f"S2: {format_s2_output_for_overlay(latest_s2_output)[:70]}", (20, 108), cv2.FONT_HERSHEY_SIMPLEX, 0.55, From d30ee37734998841c36700160449b28f67ead96a Mon Sep 17 00:00:00 2001 From: lighthqg Date: Mon, 10 Aug 2026 15:51:48 +0800 Subject: [PATCH 3/6] feat: add lightweight S1 safety controls --- .../vln/habitat_vln_evaluator.py | 190 +++++++++++++++--- .../vln/recovery_controller.py | 101 ++++++++++ .../vln/trajectory_selector.py | 169 ++++++++++++++++ tests/unit_test/test_minimal_s1_safety.py | 124 ++++++++++++ 4 files changed, 559 insertions(+), 25 deletions(-) create mode 100644 internnav/habitat_extensions/vln/recovery_controller.py create mode 100644 internnav/habitat_extensions/vln/trajectory_selector.py create mode 100644 tests/unit_test/test_minimal_s1_safety.py diff --git a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py index dd95ebe3..19a0044c 100644 --- a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py +++ b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py @@ -47,6 +47,8 @@ preprocess_depth_image_v2, xyz_yaw_pitch_to_tf_matrix, ) +from internnav.habitat_extensions.vln.recovery_controller import RecoveryController +from internnav.habitat_extensions.vln.trajectory_selector import TrajectorySelector from internnav.model.basemodel.internvla_n1.internvla_n1 import InternVLAN1ForCausalLM from internnav.model.utils.vln_utils import split_and_clean, traj_to_actions @@ -102,6 +104,22 @@ def __init__(self, cfg: EvalCfg): self.diagnostic_enabled = bool(getattr(args, 'diagnostic_log', False)) self.diagnostic_dir = getattr(args, 'diagnostic_dir', os.path.join(self.output_path, 'diagnostics')) self.diagnostic_criteria = getattr(args, 'diagnostic_criteria', None) + self.minimal_s1_safety = bool(getattr(args, 'minimal_s1_safety', False)) + self.trajectory_selector = TrajectorySelector( + cluster_min_fraction=float(getattr(args, 's1_cluster_min_fraction', 0.25)), + cluster_lateral_gap=float(getattr(args, 's1_cluster_lateral_gap', 0.25)), + depth_lookahead=float(getattr(args, 's1_depth_lookahead', 1.25)), + unsafe_clearance=float(getattr(args, 's1_unsafe_clearance', 0.12)), + near_clearance=float(getattr(args, 's1_near_clearance', 0.35)), + horizontal_fov_deg=float(getattr(args, 's1_horizontal_fov_deg', 79.0)), + ) + self.recovery_controller = RecoveryController( + min_forward_displacement=float(getattr(args, 's1_min_forward_displacement', 0.03)), + system2_retry_limit=int(getattr(args, 's1_system2_retry_limit', 3)), + max_consecutive_turns=( + int(getattr(args, 's1_max_consecutive_turns', 24)) if self.minimal_s1_safety else 0 + ), + ) # This happens before model construction. The launch script additionally # sets PYTHONHASHSEED and CUBLAS_WORKSPACE_CONFIG before Python starts. @@ -328,6 +346,23 @@ def _collision_values(metrics): return bool(collisions.get('is_collision', False)), collisions.get('count') return bool(collisions), None + def _prepare_s1_actions(self, dp_actions, depth=None): + """Choose one candidate and determine how many actions may run open-loop.""" + if not self.minimal_s1_safety: + return traj_to_actions(dp_actions), None, MAX_LOCAL_STEPS + + selection = self.trajectory_selector.select( + dp_actions, + depth=depth, + recent_failure=self.recovery_controller.goal_retry_count > 0, + ) + selected = dp_actions[selection.selected_index : selection.selected_index + 1] + if hasattr(selected, 'clone'): + selected = selected.clone() + else: + selected = np.array(selected, copy=True) + return traj_to_actions(selected), selection, selection.chunk_size + def _depth_observation_to_metres(self, depth): depth = np.asarray(depth).squeeze().astype(np.float32) return depth * (self._max_depth - self._min_depth) + self._min_depth @@ -409,11 +444,13 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 print("episode start", episode_instruction) self._diagnostic_env_step_id = 0 + self.recovery_controller.reset() s2_call_id = 0 s1_plan_id = 0 current_s2_call_id = None current_s1_plan_id = None latest_s2_output = None + recovery_context = None stop_reason = 'unknown' if self.diagnostic_logger is not None: goal_positions = [getattr(goal, 'position', None) for goal in getattr(episode, 'goals', [])] @@ -475,6 +512,9 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 depth = filter_depth(depth.reshape(depth.shape[:2]), blur_type=None) depth = depth * (self._max_depth - self._min_depth) + self._min_depth depth = depth * 1000 + front_depth_m = None + if action != action_code.LOOKDOWN: + front_depth_m = depth / 1000.0 image = Image.fromarray(rgb).convert('RGB') save_raw_image = image.copy() @@ -532,15 +572,18 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 'camera_adjustment', reason='restore_horizontal_view', ) - self._diagnostic_env_step( + horizontal_observations, _, _, _ = self._diagnostic_env_step( action_code.LOOKUP, up_observations, step_id, 'camera_adjustment', reason='restore_horizontal_view', ) + front_depth_m = self._depth_observation_to_metres(horizontal_observations['depth']) + observations = horizontal_observations if len(action_seq) == 0 and pixel_goal is None: + s2_recovery_context = None if action == action_code.LOOKDOWN: # last action is look down sources = [{"from": "human", "value": ""}, {"from": "gpt", "value": ""}] @@ -568,6 +611,11 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 input_images = [rgb_list[i] for i in history_id] + cur_images input_img_id = 0 + if recovery_context: + s2_recovery_context = recovery_context + sources[0]["value"] += f" {recovery_context}" + recovery_context = None + prompt = random.choice(self.conjunctions) + DEFAULT_IMAGE_TOKEN sources[0]["value"] += f" {prompt}." prompt_instruction = copy.deepcopy(sources[0]["value"]) @@ -622,13 +670,15 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 'camera_adjustment', reason='restore_from_s2_lookdown', ) - self._diagnostic_env_step( + horizontal_observations, _, _, _ = self._diagnostic_env_step( action_code.LOOKUP, up_observations, step_id, 'camera_adjustment', reason='restore_from_s2_lookdown', ) + front_depth_m = self._depth_observation_to_metres(horizontal_observations['depth']) + observations = horizontal_observations local_actions = [] pixel_values = inputs.pixel_values @@ -654,6 +704,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 latent_generate_ms=latent_generate_ms, latent_shape=list(traj_latents.shape), latent_l2_norm=float(traj_latents.detach().float().norm().item()), + recovery_context=s2_recovery_context, ) # prepocess align with navdp @@ -689,7 +740,10 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 decision_step=step_id, ) - action_list = traj_to_actions(dp_actions) + self.recovery_controller.start_new_goal() + action_list, trajectory_selection, chunk_size = self._prepare_s1_actions( + dp_actions, depth=front_depth_m + ) if self.diagnostic_logger is not None: self.diagnostic_logger.log( 's1_discretization', @@ -697,40 +751,51 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 s2_call_id=current_s2_call_id, decision_step=step_id, discrete_actions=[int(item) for item in action_list], - executable_chunk=[int(item) for item in action_list[:MAX_LOCAL_STEPS]], + executable_chunk=[int(item) for item in action_list[:chunk_size]], + selected_index=( + trajectory_selection.selected_index if trajectory_selection is not None else None + ), + trajectory_dispersion=( + trajectory_selection.dispersion if trajectory_selection is not None else None + ), + adaptive_chunk_size=chunk_size, + trajectory_bimodal=( + trajectory_selection.bimodal if trajectory_selection is not None else None + ), + trajectory_cluster_sizes=( + trajectory_selection.cluster_sizes if trajectory_selection is not None else None + ), + selected_clearance=( + trajectory_selection.clearance if trajectory_selection is not None else None + ), + selected_smoothness=( + trajectory_selection.smoothness if trajectory_selection is not None else None + ), + depth_risk=( + trajectory_selection.depth_risk if trajectory_selection is not None else None + ), ) if len(action_list) < MAX_STEPS: action_list += [0] * (MAX_STEPS - len(action_list)) local_actions = action_list - if len(local_actions) >= MAX_LOCAL_STEPS: - local_actions = local_actions[:MAX_LOCAL_STEPS] + if len(local_actions) >= chunk_size: + local_actions = local_actions[:chunk_size] - action = local_actions[0] + action = local_actions.pop(0) if action == action_code.STOP: pixel_goal = None output_ids = None - action = action_code.LEFT + local_actions = [] if self.diagnostic_logger is not None: self.diagnostic_logger.log( - 'controller_fallback', + 'controller_replan', decision_step=step_id, s2_call_id=current_s2_call_id, plan_id=current_s1_plan_id, reason='initial_s1_plan_started_with_stop', - replacement_action='LEFT', + next_system='s2', ) - observations, _, done, _ = self._diagnostic_env_step( - action, - observations, - step_id, - 'model_action', - action_source='s1_stop_fallback', - s2_call_id=current_s2_call_id, - plan_id=current_s1_plan_id, - first_person_frame_id=None, - top_down_frame_id=None, - ) step_id += 1 messages = [] continue @@ -750,6 +815,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 input_image_count=len(input_images), generated_token_count=int(output_ids.shape[-1] - inputs.input_ids.shape[1]), generate_ms=s2_generate_ms, + recovery_context=s2_recovery_context, ) print('actions', action_seq, flush=True) @@ -793,7 +859,9 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 decision_step=step_id, ) - action_list = traj_to_actions(dp_actions) + action_list, trajectory_selection, chunk_size = self._prepare_s1_actions( + dp_actions, depth=front_depth_m + ) if self.diagnostic_logger is not None: self.diagnostic_logger.log( 's1_discretization', @@ -801,14 +869,36 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 s2_call_id=current_s2_call_id, decision_step=step_id, discrete_actions=[int(item) for item in action_list], - executable_chunk=[int(item) for item in action_list[:MAX_LOCAL_STEPS]], + executable_chunk=[int(item) for item in action_list[:chunk_size]], + selected_index=( + trajectory_selection.selected_index if trajectory_selection is not None else None + ), + trajectory_dispersion=( + trajectory_selection.dispersion if trajectory_selection is not None else None + ), + adaptive_chunk_size=chunk_size, + trajectory_bimodal=( + trajectory_selection.bimodal if trajectory_selection is not None else None + ), + trajectory_cluster_sizes=( + trajectory_selection.cluster_sizes if trajectory_selection is not None else None + ), + selected_clearance=( + trajectory_selection.clearance if trajectory_selection is not None else None + ), + selected_smoothness=( + trajectory_selection.smoothness if trajectory_selection is not None else None + ), + depth_risk=( + trajectory_selection.depth_risk if trajectory_selection is not None else None + ), ) if len(action_list) < MAX_STEPS: action_list += [0] * (MAX_STEPS - len(action_list)) local_actions = action_list - if len(local_actions) >= MAX_LOCAL_STEPS: - local_actions = local_actions[:MAX_LOCAL_STEPS] + if len(local_actions) >= chunk_size: + local_actions = local_actions[:chunk_size] print("local_actions", local_actions) action = local_actions.pop(0) else: @@ -907,6 +997,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 ) flag = True else: + gps_before_action = np.asarray(observations.get('gps', [np.nan, np.nan])).copy() observations, _, done, _ = self._diagnostic_env_step( action, observations, @@ -922,6 +1013,55 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 first_person_frame_id=first_person_frame_id - 1 if vis_writer is not None else None, top_down_frame_id=top_down_frame_id - 1 if self.save_video else None, ) + collision, _ = self._collision_values(self.env.get_metrics()) + recovery = self.recovery_controller.update( + action, + gps_before_action, + observations.get('gps', [np.nan, np.nan]), + collision=collision, + is_s1=self.minimal_s1_safety and action_source == 's1', + ) + if recovery.cancel_remaining: + cancelled_s1_actions = [int(item) for item in local_actions] + cancelled_s2_actions = [int(item) for item in action_seq] + local_actions = [] + action_seq = [] + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 's1_recovery' if action_source == 's1' else 'controller_recovery', + decision_step=step_id, + plan_id=current_s1_plan_id, + action_source=action_source, + reason=recovery.reason, + gps_displacement=recovery.gps_displacement, + cancelled_s1_actions=cancelled_s1_actions, + cancelled_s2_actions=cancelled_s2_actions, + retry_count=recovery.retry_count, + goal_retry_count=recovery.goal_retry_count, + turn_count=recovery.turn_count, + consecutive_failures=self.recovery_controller.consecutive_failures, + failed_directions=dict(self.recovery_controller.failed_directions), + replan_system2=recovery.replan_system2, + ) + if recovery.replan_system2: + pixel_goal = None + output_ids = None + forward_action = 0 + messages = [] + if recovery.reason == 'repeated_turns': + recovery_context = ( + 'Recovery notice: you just rotated a full circle without translating. ' + 'Reassess which parts of the original instruction remain unfinished, ' + 'then choose a genuinely different next action or STOP only if the task is complete. ' + 'Returning along the previous route is allowed when it is necessary.' + ) + else: + recovery_context = ( + f'Recovery notice: the previous waypoint failed repeatedly because of ' + f'{recovery.reason}. Reassess which parts of the original instruction remain ' + 'unfinished and select a reachable next waypoint. Returning along the previous ' + 'route is allowed when it is necessary.' + ) step_id += 1 messages = [] flag = False diff --git a/internnav/habitat_extensions/vln/recovery_controller.py b/internnav/habitat_extensions/vln/recovery_controller.py new file mode 100644 index 00000000..521d5131 --- /dev/null +++ b/internnav/habitat_extensions/vln/recovery_controller.py @@ -0,0 +1,101 @@ +"""Minimal stateful recovery policy for queued S1 actions.""" + +from collections import Counter +from dataclasses import dataclass +from typing import Optional + +import numpy as np + + +@dataclass(frozen=True) +class RecoveryDecision: + cancel_remaining: bool + replan_system2: bool + reason: Optional[str] + gps_displacement: Optional[float] + retry_count: int + goal_retry_count: int + turn_count: int + + +class RecoveryController: + def __init__( + self, + min_forward_displacement=0.03, + system2_retry_limit=3, + max_consecutive_turns=24, + ): + self.min_forward_displacement = float(min_forward_displacement) + self.system2_retry_limit = int(system2_retry_limit) + self.max_consecutive_turns = int(max_consecutive_turns) + self.failed_directions = Counter() + self.retry_count = 0 + self.goal_retry_count = 0 + self.consecutive_failures = 0 + self.turn_direction = None + self.consecutive_turns = 0 + + def reset(self): + self.failed_directions.clear() + self.retry_count = 0 + self.turn_direction = None + self.consecutive_turns = 0 + self.start_new_goal() + + def start_new_goal(self): + self.goal_retry_count = 0 + self.consecutive_failures = 0 + self.turn_direction = None + self.consecutive_turns = 0 + + def update(self, action, gps_before, gps_after, collision=False, is_s1=True): + action = int(action) + before = np.asarray(gps_before, dtype=np.float32).reshape(-1) + after = np.asarray(gps_after, dtype=np.float32).reshape(-1) + displacement = None + if before.size >= 2 and after.size >= 2 and np.all(np.isfinite(before[:2])) and np.all(np.isfinite(after[:2])): + displacement = float(np.linalg.norm(after[:2] - before[:2])) + + if action in (2, 3) and self.max_consecutive_turns > 0: + if action == self.turn_direction: + self.consecutive_turns += 1 + else: + self.turn_direction = action + self.consecutive_turns = 1 + else: + self.turn_direction = None + self.consecutive_turns = 0 + observed_turn_count = self.consecutive_turns + + reason = None + if is_s1 and collision: + reason = "collision" + elif is_s1 and action == 1 and displacement is not None and displacement < self.min_forward_displacement: + reason = "low_gps_displacement" + elif self.max_consecutive_turns > 0 and observed_turn_count >= self.max_consecutive_turns: + reason = "repeated_turns" + + if reason is not None: + self.retry_count += 1 + self.goal_retry_count += 1 + self.consecutive_failures += 1 + self.failed_directions[action] += 1 + elif is_s1 and action == 1: + self.goal_retry_count = 0 + self.consecutive_failures = 0 + + replan_system2 = reason == "repeated_turns" or ( + reason is not None and self.goal_retry_count >= self.system2_retry_limit + ) + if reason == "repeated_turns": + self.turn_direction = None + self.consecutive_turns = 0 + return RecoveryDecision( + cancel_remaining=reason is not None, + replan_system2=replan_system2, + reason=reason, + gps_displacement=displacement, + retry_count=self.retry_count, + goal_retry_count=self.goal_retry_count, + turn_count=observed_turn_count, + ) diff --git a/internnav/habitat_extensions/vln/trajectory_selector.py b/internnav/habitat_extensions/vln/trajectory_selector.py new file mode 100644 index 00000000..653e9809 --- /dev/null +++ b/internnav/habitat_extensions/vln/trajectory_selector.py @@ -0,0 +1,169 @@ +"""Lightweight candidate selection and risk estimates for S1 trajectories.""" + +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class TrajectorySelection: + selected_index: int + trajectory: np.ndarray + dispersion: float + chunk_size: int + bimodal: bool + cluster_sizes: tuple + clearance: float + smoothness: float + depth_risk: bool + + +class TrajectorySelector: + """Select a real candidate using consensus, clustering, and cheap safety scores.""" + + def __init__( + self, + cluster_min_fraction=0.25, + cluster_lateral_gap=0.25, + depth_lookahead=1.25, + unsafe_clearance=0.12, + near_clearance=0.35, + horizontal_fov_deg=79.0, + ): + self.cluster_min_fraction = float(cluster_min_fraction) + self.cluster_lateral_gap = float(cluster_lateral_gap) + self.depth_lookahead = float(depth_lookahead) + self.unsafe_clearance = float(unsafe_clearance) + self.near_clearance = float(near_clearance) + self.horizontal_fov_rad = np.deg2rad(float(horizontal_fov_deg)) + + @staticmethod + def _as_numpy(value): + value = value.detach() if hasattr(value, "detach") else value + value = value.float().cpu().numpy() if hasattr(value, "cpu") else np.asarray(value) + return np.asarray(value, dtype=np.float32) + + @classmethod + def reconstruct(cls, dp_actions): + deltas = cls._as_numpy(dp_actions).copy() + if deltas.ndim != 3 or deltas.shape[-1] < 2 or deltas.shape[0] == 0: + raise ValueError("dp_actions must have shape [candidates, steps, >=2]") + deltas[:, :, :2] /= 4.0 + positions = np.cumsum(deltas[:, :, :2], axis=1) + origin = np.zeros((positions.shape[0], 1, 2), dtype=positions.dtype) + return np.concatenate((origin, positions), axis=1) + + def _two_medoid_clusters(self, pairwise, trajectories): + count = pairwise.shape[0] + if count < 4: + return False, (count, 0) + + medoids = np.unravel_index(np.argmax(pairwise), pairwise.shape) + assignments = None + for _ in range(3): + assignments = np.argmin(pairwise[:, medoids], axis=1) + updated = [] + for cluster in range(2): + members = np.flatnonzero(assignments == cluster) + if members.size == 0: + updated.append(medoids[cluster]) + else: + costs = pairwise[np.ix_(members, members)].sum(axis=1) + updated.append(int(members[np.argmin(costs)])) + medoids = tuple(updated) + + assignments = np.argmin(pairwise[:, medoids], axis=1) + sizes = tuple(int(np.sum(assignments == cluster)) for cluster in range(2)) + minimum_size = max(2, int(np.ceil(count * self.cluster_min_fraction))) + endpoints = trajectories[list(medoids), -1, 1] + lateral_gap = float(abs(endpoints[0] - endpoints[1])) + opposite_sides = bool(endpoints[0] * endpoints[1] < 0) + within = [] + for cluster in range(2): + members = np.flatnonzero(assignments == cluster) + within.extend(pairwise[members, medoids[cluster]].tolist()) + within_distance = float(np.mean(within)) if within else 0.0 + separation = float(pairwise[medoids]) + bimodal = ( + min(sizes) >= minimum_size + and opposite_sides + and lateral_gap >= self.cluster_lateral_gap + and separation >= max(0.12, 1.5 * within_distance) + ) + return bimodal, sizes + + @staticmethod + def _smoothness(trajectories): + segments = np.diff(trajectories, axis=1) + headings = np.unwrap(np.arctan2(segments[:, :, 1], segments[:, :, 0]), axis=1) + turns = np.diff(headings, axis=1) + return np.mean(np.abs(turns), axis=1) + + def _depth_clearance(self, trajectories, depth): + if depth is None: + return np.full(trajectories.shape[0], np.inf, dtype=np.float32) + depth = self._as_numpy(depth).squeeze() + if depth.ndim != 2: + raise ValueError("depth must be a 2-D image in metres") + + height, width = depth.shape + row_start, row_stop = int(height * 0.30), max(int(height * 0.82), 1) + clearance = np.full(trajectories.shape[0], np.inf, dtype=np.float32) + for candidate_index, trajectory in enumerate(trajectories): + candidate_clearance = [] + for forward, lateral in trajectory[1:]: + radial = float(np.hypot(forward, lateral)) + if forward <= 0.05 or radial > self.depth_lookahead: + continue + bearing = float(np.arctan2(lateral, forward)) + if abs(bearing) >= self.horizontal_fov_rad / 2: + continue + column = int(round((0.5 - bearing / self.horizontal_fov_rad) * (width - 1))) + left, right = max(0, column - 3), min(width, column + 4) + values = depth[row_start:row_stop, left:right] + valid = values[np.isfinite(values) & (values > 0.05)] + if valid.size: + ray_depth = float(np.quantile(valid, 0.15)) + candidate_clearance.append(ray_depth - float(forward)) + if candidate_clearance: + clearance[candidate_index] = min(candidate_clearance) + return clearance + + def select(self, dp_actions, depth=None, recent_failure=False): + trajectories = self.reconstruct(dp_actions) + flattened = trajectories.reshape(trajectories.shape[0], -1) + pairwise = np.sqrt(np.mean((flattened[:, None] - flattened[None, :]) ** 2, axis=2)) + centrality = pairwise.mean(axis=1) + medoid_index = int(np.argmin(centrality)) + dispersion = float(np.sqrt(np.mean((trajectories - trajectories[medoid_index]) ** 2))) + bimodal, cluster_sizes = self._two_medoid_clusters(pairwise, trajectories) + smoothness = self._smoothness(trajectories) + clearance = self._depth_clearance(trajectories, depth) + + selected_index = medoid_index + if depth is not None: + centrality_scale = max(float(np.median(centrality)), 1e-6) + smoothness_scale = max(float(np.median(smoothness)), 1e-6) + clearance_penalty = np.maximum(self.near_clearance - clearance, 0.0) / self.near_clearance + scores = centrality / centrality_scale + 0.20 * smoothness / smoothness_scale + 4.0 * clearance_penalty + selected_index = int(np.argmin(scores)) + + selected_clearance = float(clearance[selected_index]) + depth_risk = selected_clearance < self.unsafe_clearance + if recent_failure or bimodal or depth_risk: + chunk_size = 1 + elif selected_clearance < self.near_clearance: + chunk_size = 2 + else: + chunk_size = 4 + return TrajectorySelection( + selected_index=selected_index, + trajectory=trajectories[selected_index], + dispersion=dispersion, + chunk_size=chunk_size, + bimodal=bimodal, + cluster_sizes=cluster_sizes, + clearance=selected_clearance, + smoothness=float(smoothness[selected_index]), + depth_risk=depth_risk, + ) diff --git a/tests/unit_test/test_minimal_s1_safety.py b/tests/unit_test/test_minimal_s1_safety.py new file mode 100644 index 00000000..138d2bbd --- /dev/null +++ b/tests/unit_test/test_minimal_s1_safety.py @@ -0,0 +1,124 @@ +import importlib.util +import sys +from pathlib import Path + +import numpy as np + + +def _load_module(name): + path = Path(__file__).parents[2] / "internnav" / "habitat_extensions" / "vln" / f"{name}.py" + spec = importlib.util.spec_from_file_location(name, path) + module = importlib.util.module_from_spec(spec) + sys.modules[name] = module + spec.loader.exec_module(module) + return module + + +RecoveryController = _load_module("recovery_controller").RecoveryController +TrajectorySelector = _load_module("trajectory_selector").TrajectorySelector + + +def _constant_candidates(offsets, steps=4): + candidates = np.zeros((len(offsets), steps, 3), dtype=np.float32) + for index, lateral_delta in enumerate(offsets): + candidates[index, :, 0] = 1.0 + candidates[index, :, 1] = lateral_delta + return candidates + + +def test_medoid_is_an_actual_candidate_not_the_mean_between_modes(): + actions = _constant_candidates([-1.0, -1.0, 1.0, 1.0]) + selection = TrajectorySelector().select(actions) + reconstructed = TrajectorySelector.reconstruct(actions) + assert any(np.array_equal(selection.trajectory, candidate) for candidate in reconstructed) + assert not np.allclose(selection.trajectory[:, 1], 0.0) + + +def test_adaptive_chunks_only_shorten_for_risk_or_recent_failure(): + selector = TrajectorySelector() + consistent = _constant_candidates([0.0] * 32) + bimodal = _constant_candidates([-1.0] * 16 + [1.0] * 16) + assert selector.select(consistent).chunk_size == 4 + assert selector.select(consistent, recent_failure=True).chunk_size == 1 + selection = selector.select(bimodal) + assert selection.bimodal + assert selection.cluster_sizes == (16, 16) + assert selection.chunk_size == 1 + + +def test_depth_short_range_check_marks_forward_path_unsafe(): + actions = _constant_candidates([0.0] * 8) + depth = np.full((120, 160), 0.60, dtype=np.float32) + selection = TrajectorySelector().select(actions, depth=depth) + assert selection.depth_risk + assert selection.clearance < 0.12 + assert selection.chunk_size == 1 + + +def test_safe_depth_keeps_four_step_chunk(): + actions = _constant_candidates([0.0] * 8) + depth = np.full((120, 160), 5.0, dtype=np.float32) + selection = TrajectorySelector().select(actions, depth=depth) + assert not selection.depth_risk + assert selection.chunk_size == 4 + + +def test_collision_and_low_forward_displacement_cancel_s1_queue(): + controller = RecoveryController(min_forward_displacement=0.03, system2_retry_limit=3) + collision = controller.update(1, [0, 0], [0.2, 0], collision=True) + blocked = controller.update(1, [0, 0], [0.01, 0], collision=False) + assert collision.cancel_remaining and collision.reason == "collision" + assert blocked.cancel_remaining and blocked.reason == "low_gps_displacement" + assert controller.retry_count == 2 + assert controller.failed_directions[1] == 2 + assert not blocked.replan_system2 + + +def test_third_goal_failure_escalates_to_system2_and_turn_does_not_reset_it(): + controller = RecoveryController(system2_retry_limit=3) + first = controller.update(1, [0, 0], [0, 0], collision=True) + controller.update(2, [0, 0], [0, 0]) + second = controller.update(1, [0, 0], [0, 0], collision=True) + third = controller.update(1, [0, 0], [0, 0], collision=True) + assert not first.replan_system2 + assert not second.replan_system2 + assert third.replan_system2 + assert third.goal_retry_count == 3 + + +def test_turns_and_non_s1_actions_do_not_trigger_low_displacement(): + controller = RecoveryController() + assert not controller.update(2, [0, 0], [0, 0], is_s1=True).cancel_remaining + assert not controller.update(1, [0, 0], [0, 0], collision=True, is_s1=False).cancel_remaining + + +def test_success_resets_goal_failures_but_preserves_episode_retry_count(): + controller = RecoveryController() + controller.update(1, [0, 0], [0, 0]) + controller.update(1, [0, 0], [0.2, 0]) + assert controller.consecutive_failures == 0 + assert controller.goal_retry_count == 0 + assert controller.retry_count == 1 + + +def test_full_same_direction_rotation_replans_without_blocking_normal_turns(): + controller = RecoveryController(max_consecutive_turns=24) + for _ in range(23): + decision = controller.update(3, [0, 0], [0, 0], is_s1=False) + assert not decision.cancel_remaining + decision = controller.update(3, [0, 0], [0, 0], is_s1=False) + assert decision.cancel_remaining + assert decision.replan_system2 + assert decision.reason == "repeated_turns" + assert decision.turn_count == 24 + + +def test_direction_change_and_forward_motion_reset_turn_watchdog(): + controller = RecoveryController(max_consecutive_turns=24) + for _ in range(16): + assert not controller.update(3, [0, 0], [0, 0], is_s1=False).cancel_remaining + assert not controller.update(2, [0, 0], [0, 0], is_s1=False).cancel_remaining + for _ in range(16): + assert not controller.update(2, [0, 0], [0, 0], is_s1=False).cancel_remaining + assert not controller.update(1, [0, 0], [0.25, 0], is_s1=False).cancel_remaining + assert controller.consecutive_turns == 0 From bb1d0f5a8f39abbf1e8dadbf219736c4a96e5f86 Mon Sep 17 00:00:00 2001 From: lighthqg Date: Tue, 11 Aug 2026 15:20:48 +0800 Subject: [PATCH 4/6] Harden DualVLN navigation recovery --- .../vln/habitat_vln_evaluator.py | 600 ++++++++++++++---- .../vln/navigation_state.py | 553 ++++++++++++++++ .../vln/recovery_controller.py | 85 ++- .../vln/trajectory_selector.py | 86 ++- tests/unit_test/test_navigation_state.py | 208 ++++++ 5 files changed, 1390 insertions(+), 142 deletions(-) create mode 100644 internnav/habitat_extensions/vln/navigation_state.py create mode 100644 tests/unit_test/test_navigation_state.py diff --git a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py index 19a0044c..7bb4695a 100644 --- a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py +++ b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py @@ -48,6 +48,17 @@ xyz_yaw_pitch_to_tf_matrix, ) from internnav.habitat_extensions.vln.recovery_controller import RecoveryController +from internnav.habitat_extensions.vln.navigation_state import ( + CameraPitchState, + DepthObservationSummarizer, + InstructionStateTracker, + PixelGoalMemory, + bound_actions_at_lookdown, + load_semantic_labels, + select_history_indices, + select_uniform_history_indices, + semantic_label_for_episode, +) from internnav.habitat_extensions.vln.trajectory_selector import TrajectorySelector from internnav.model.basemodel.internvla_n1.internvla_n1 import InternVLAN1ForCausalLM from internnav.model.utils.vln_utils import split_and_clean, traj_to_actions @@ -105,6 +116,25 @@ def __init__(self, cfg: EvalCfg): self.diagnostic_dir = getattr(args, 'diagnostic_dir', os.path.join(self.output_path, 'diagnostics')) self.diagnostic_criteria = getattr(args, 'diagnostic_criteria', None) self.minimal_s1_safety = bool(getattr(args, 'minimal_s1_safety', False)) + self.structured_s2_context = bool(getattr(args, 'structured_s2_context', False)) + self.s2_stair_depth_context = bool(getattr(args, 's2_stair_depth_context', False)) + self.s2_reject_unverified_stair_stop = bool( + getattr(args, 's2_reject_unverified_stair_stop', False) + ) + self.s2_force_verified_stair_stop = bool( + getattr(args, 's2_force_verified_stair_stop', False) + ) + self.semantic_labels = load_semantic_labels(getattr(args, 'semantic_labels_path', None)) + self.depth_summarizer = DepthObservationSummarizer( + near_distance=float(getattr(args, 's2_depth_near_distance', 0.55)), + open_margin=float(getattr(args, 's2_depth_open_margin', 0.25)), + ) + self.pixel_goal_memory = PixelGoalMemory( + pixel_tolerance=float(getattr(args, 's2_duplicate_goal_pixel_tolerance', 32.0)), + pose_tolerance=float(getattr(args, 's2_duplicate_goal_pose_tolerance', 0.30)), + heading_tolerance_deg=float(getattr(args, 's2_duplicate_goal_heading_tolerance_deg', 20.0)), + retry_limit=int(getattr(args, 's2_duplicate_goal_retry_limit', 2)), + ) self.trajectory_selector = TrajectorySelector( cluster_min_fraction=float(getattr(args, 's1_cluster_min_fraction', 0.25)), cluster_lateral_gap=float(getattr(args, 's1_cluster_lateral_gap', 0.25)), @@ -119,6 +149,9 @@ def __init__(self, cfg: EvalCfg): max_consecutive_turns=( int(getattr(args, 's1_max_consecutive_turns', 24)) if self.minimal_s1_safety else 0 ), + direct_turn_escape_clearance=float( + getattr(args, 's2_direct_turn_escape_clearance', 0.55) + ), ) # This happens before model construction. The launch script additionally @@ -355,7 +388,13 @@ def _prepare_s1_actions(self, dp_actions, depth=None): dp_actions, depth=depth, recent_failure=self.recovery_controller.goal_retry_count > 0, + failed_route_directions=( + self.recovery_controller.failed_route_directions + if self.structured_s2_context + else None + ), ) + self.recovery_controller.set_route_direction(selection.route_direction) selected = dp_actions[selection.selected_index : selection.selected_index + 1] if hasattr(selected, 'clone'): selected = selected.clone() @@ -367,6 +406,79 @@ def _depth_observation_to_metres(self, depth): depth = np.asarray(depth).squeeze().astype(np.float32) return depth * (self._max_depth - self._min_depth) + self._min_depth + def _low_head_inputs(self, observations): + """Build S1 RGB/depth inputs from an already lowered camera view.""" + image = Image.fromarray(observations['rgb']).convert('RGB') + depth = np.asarray(observations['depth']).squeeze() + depth = filter_depth(depth, blur_type=None) + depth_m = depth * (self._max_depth - self._min_depth) + self._min_depth + depth_mm = depth_m * 1000.0 + depth_tensor, _ = preprocess_depth_image_v2( + Image.fromarray(depth_mm.astype(np.uint16), mode='I;16'), + do_depth_scale=True, + depth_scale=1000, + target_height=224, + target_width=224, + ) + depth_tensor = torch.as_tensor(np.ascontiguousarray(depth_tensor)).float() + depth_tensor[depth_tensor > 5.0] = 5.0 + return image, depth_tensor, depth_m + + def _capture_low_head_inputs(self, observations, decision_step, reason): + """Capture low-head S1 inputs and restore the horizontal view immediately.""" + down_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKDOWN, + observations, + decision_step, + 'camera_adjustment', + reason=reason, + ) + down_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKDOWN, + down_observations, + decision_step, + 'camera_adjustment', + reason=reason, + ) + look_down_image, look_down_depth, low_head_depth_m = self._low_head_inputs( + down_observations + ) + up_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKUP, + down_observations, + decision_step, + 'camera_adjustment', + reason='restore_horizontal_view', + ) + horizontal_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKUP, + up_observations, + decision_step, + 'camera_adjustment', + reason='restore_horizontal_view', + ) + front_depth_m = self._depth_observation_to_metres(horizontal_observations['depth']) + return ( + horizontal_observations, + look_down_image, + look_down_depth, + low_head_depth_m, + front_depth_m, + ) + + def _agent_height(self): + return float(self.env._env.sim.get_agent_state().position[1]) + + @staticmethod + def _parse_pixel_goal(output): + """Prefer an explicit coordinate pair and ignore unrelated prose numbers.""" + output = str(output) + pair = re.search(r"[\[(]\s*(-?\d+)\s*[, ]\s*(-?\d+)\s*[\])]", output) + if pair: + return [int(pair.group(2)), int(pair.group(1))] + values = [int(value) for value in re.findall(r"-?\d+", output)] + return [values[1], values[0]] if len(values) >= 2 else None + def _diagnostic_env_step(self, action, observations_before, decision_step, phase, **fields): """Execute one Habitat action and log both model and camera-only steps.""" metrics_before = self.env.get_metrics() @@ -445,12 +557,25 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 self._diagnostic_env_step_id = 0 self.recovery_controller.reset() + self.pixel_goal_memory.reset() + initial_compass = np.asarray(observations.get('compass', [0.0])).reshape(-1) + instruction_state = InstructionStateTracker( + episode_instruction, + initial_height=self._agent_height(), + initial_compass=float(initial_compass[0]) if initial_compass.size else 0.0, + initial_gps=observations.get('gps'), + ) + manual_semantic_label = semantic_label_for_episode( + self.semantic_labels, scene_id, episode_id + ) s2_call_id = 0 s1_plan_id = 0 current_s2_call_id = None current_s1_plan_id = None latest_s2_output = None recovery_context = None + latest_depth_summary = None + camera_pitch = CameraPitchState() stop_reason = 'unknown' if self.diagnostic_logger is not None: goal_positions = [getattr(goal, 'position', None) for goal in getattr(episode, 'goals', [])] @@ -464,6 +589,8 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 'start_position': getattr(episode, 'start_position', None), 'start_rotation': getattr(episode, 'start_rotation', None), 'goal_positions': goal_positions, + 'instruction_state': instruction_state.as_dict(), + 'manual_semantic_label': manual_semantic_label, } ) @@ -518,72 +645,50 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 image = Image.fromarray(rgb).convert('RGB') save_raw_image = image.copy() + look_down_image = None + look_down_depth = None + low_head_depth_m = None if action == action_code.LOOKDOWN: - look_down_image = image - save_raw_image = look_down_image.copy() - look_down_depth, resize_shape = preprocess_depth_image_v2( - Image.fromarray(depth.astype(np.uint16), mode='I;16'), - do_depth_scale=True, - depth_scale=1000, - target_height=224, - target_width=224, + look_down_image, look_down_depth, low_head_depth_m = self._low_head_inputs( + observations ) - look_down_depth = torch.as_tensor(np.ascontiguousarray(look_down_depth)).float() - look_down_depth[look_down_depth > 5.0] = 5.0 + save_raw_image = look_down_image.copy() else: image = image.resize((self.model_args.resize_w, self.model_args.resize_h)) rgb_list.append(image) - down_observations, _, _, _ = self._diagnostic_env_step( - action_code.LOOKDOWN, + needs_s2 = len(action_seq) == 0 and pixel_goal is None + if ( + needs_s2 + and action != action_code.LOOKDOWN + and (self.structured_s2_context or self.s2_stair_depth_context) + ): + ( + observations, + look_down_image, + look_down_depth, + low_head_depth_m, + front_depth_m, + ) = self._capture_low_head_inputs( observations, step_id, - 'camera_adjustment', - reason='prepare_s1_depth', - ) - down_observations, _, _, _ = self._diagnostic_env_step( - action_code.LOOKDOWN, - down_observations, - step_id, - 'camera_adjustment', - reason='prepare_s1_depth', + reason='prepare_s2_depth_context', ) - look_down_image = Image.fromarray(down_observations["rgb"]).convert('RGB') - depth = down_observations["depth"] - depth = filter_depth(depth.reshape(depth.shape[:2]), blur_type=None) - depth = depth * (self._max_depth - self._min_depth) + self._min_depth - depth = depth * 1000 - look_down_depth, resize_shape = preprocess_depth_image_v2( - Image.fromarray(depth.astype(np.uint16), mode='I;16'), - do_depth_scale=True, - depth_scale=1000, - target_height=224, - target_width=224, + latest_depth_summary = None + if ( + low_head_depth_m is not None + and (self.structured_s2_context or self.s2_stair_depth_context) + ): + latest_depth_summary = self.depth_summarizer.summarize( + low_head_depth_m, + stair_mode=instruction_state.stair_mode, ) - look_down_depth = torch.as_tensor(np.ascontiguousarray(look_down_depth)).float() - look_down_depth[look_down_depth > 5.0] = 5.0 - up_observations, _, _, _ = self._diagnostic_env_step( - action_code.LOOKUP, - down_observations, - step_id, - 'camera_adjustment', - reason='restore_horizontal_view', - ) - horizontal_observations, _, _, _ = self._diagnostic_env_step( - action_code.LOOKUP, - up_observations, - step_id, - 'camera_adjustment', - reason='restore_horizontal_view', - ) - front_depth_m = self._depth_observation_to_metres(horizontal_observations['depth']) - observations = horizontal_observations - - if len(action_seq) == 0 and pixel_goal is None: + if needs_s2: s2_recovery_context = None + history_id = [] if action == action_code.LOOKDOWN: # last action is look down sources = [{"from": "human", "value": ""}, {"from": "gpt", "value": ""}] @@ -598,24 +703,62 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 '.', episode.instruction.instruction_text[:-1] ) cur_images = rgb_list[-1:] - if step_id == 0: - history_id = [] + if self.structured_s2_context: + history_id = select_history_indices( + len(rgb_list), + self.num_history, + prioritize_recent=( + instruction_state.stair_mode + or self.recovery_controller.goal_retry_count > 0 + ), + ) else: - history_id = np.unique( - np.linspace(0, step_id - 1, self.num_history, dtype=np.int32) - ).tolist() + history_id = select_uniform_history_indices( + len(rgb_list), self.num_history + ) + if history_id: placeholder = (DEFAULT_IMAGE_TOKEN + '\n') * len(history_id) sources[0]["value"] += f' These are your historical observations: {placeholder}.' - history_id = sorted(history_id) input_images = [rgb_list[i] for i in history_id] + cur_images input_img_id = 0 - if recovery_context: - s2_recovery_context = recovery_context - sources[0]["value"] += f" {recovery_context}" - recovery_context = None - + context_parts = [] + if self.structured_s2_context: + context_parts.append(instruction_state.prompt_context()) + failed_route_context = ( + self.recovery_controller.failed_route_context() + if self.structured_s2_context + else None + ) + if failed_route_context: + context_parts.append(failed_route_context) + if recovery_context and self.structured_s2_context: + s2_recovery_context = recovery_context + context_parts.append(recovery_context) + if recovery_context: + recovery_context = None + if context_parts: + sources[0]["value"] += " " + " ".join(context_parts) + + if ( + self.s2_stair_depth_context + and instruction_state.stair_mode + and latest_depth_summary is not None + ): + if action != action_code.LOOKDOWN: + sources[0]["value"] += ( + f" This is a low-head RGB inspection image: {DEFAULT_IMAGE_TOKEN}." + ) + input_images.insert( + len(input_images) - 1, + look_down_image.resize((224, 224)), + ) + sources[0]["value"] += f" {latest_depth_summary.text}" + sources[0]["value"] += ( + " Use the low-head image only to understand nearby geometry; place the waypoint " + "in the main current RGB image shown next." + ) prompt = random.choice(self.conjunctions) + DEFAULT_IMAGE_TOKEN sources[0]["value"] += f" {prompt}." prompt_instruction = copy.deepcopy(sources[0]["value"]) @@ -654,39 +797,59 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 ) latest_s2_output = llm_outputs print('step_id:', step_id, 'output text:', llm_outputs) + if action == action_code.LOOKDOWN and camera_pitch.restore_steps: + horizontal_observations = observations + for _ in range(camera_pitch.restore_steps): + horizontal_observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKUP, + horizontal_observations, + step_id, + 'camera_adjustment', + reason='restore_after_s2_low_head_observation', + ) + camera_pitch.mark_horizontal() + front_depth_m = self._depth_observation_to_metres(horizontal_observations['depth']) + observations = horizontal_observations + model_completed_subtask = False + if self.structured_s2_context: + model_completed_subtask = instruction_state.mark_model_completion(llm_outputs) + terminal_turn_stall = self.recovery_controller.consume_turn_stall() + forced_stop = bool( + self.s2_force_verified_stair_stop + and instruction_state.should_force_stop( + stall_confirmed=terminal_turn_stall + ) + ) - if bool(re.search(r'\d', llm_outputs)): # output pixel goal + parsed_pixel_goal = None if forced_stop else self._parse_pixel_goal(llm_outputs) + if parsed_pixel_goal is not None: # output pixel goal forward_action = 0 - coord = [int(c) for c in re.findall(r'\d+', llm_outputs)] - - pixel_goal = [int(coord[1]), int(coord[0])] + pixel_goal = parsed_pixel_goal draw_pixel_goal = True - - # look down --> horizontal - up_observations, _, _, _ = self._diagnostic_env_step( - action_code.LOOKUP, - observations, - step_id, - 'camera_adjustment', - reason='restore_from_s2_lookdown', + previous_goal_failed = ( + self.structured_s2_context + and self.recovery_controller.goal_retry_count > 0 ) - horizontal_observations, _, _, _ = self._diagnostic_env_step( - action_code.LOOKUP, - up_observations, - step_id, - 'camera_adjustment', - reason='restore_from_s2_lookdown', + goal_decision = self.pixel_goal_memory.evaluate( + pixel_goal, + observations.get('gps', [np.nan, np.nan]), + observations.get('compass', [np.nan]), + previous_goal_failed=previous_goal_failed, ) - front_depth_m = self._depth_observation_to_metres(horizontal_observations['depth']) - observations = horizontal_observations local_actions = [] - pixel_values = inputs.pixel_values - image_grid_thw = torch.cat([thw.unsqueeze(0) for thw in inputs.image_grid_thw], dim=0) - - with torch.no_grad(): - latent_started = time.perf_counter() - traj_latents = self.model.generate_latents(output_ids, pixel_values, image_grid_thw) + traj_latents = None + latent_generate_ms = 0.0 + if not goal_decision.reject: + pixel_values = inputs.pixel_values + image_grid_thw = torch.cat( + [thw.unsqueeze(0) for thw in inputs.image_grid_thw], dim=0 + ) + with torch.no_grad(): + latent_started = time.perf_counter() + traj_latents = self.model.generate_latents( + output_ids, pixel_values, image_grid_thw + ) latent_generate_ms = (time.perf_counter() - latent_started) * 1000 if self.diagnostic_logger is not None: @@ -702,27 +865,57 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 generated_token_count=int(output_ids.shape[-1] - inputs.input_ids.shape[1]), generate_ms=s2_generate_ms, latent_generate_ms=latent_generate_ms, - latent_shape=list(traj_latents.shape), - latent_l2_norm=float(traj_latents.detach().float().norm().item()), + latent_shape=(list(traj_latents.shape) if traj_latents is not None else None), + latent_l2_norm=( + float(traj_latents.detach().float().norm().item()) + if traj_latents is not None + else None + ), recovery_context=s2_recovery_context, + instruction_state=instruction_state.as_dict(), + depth_summary=( + latest_depth_summary.__dict__ if latest_depth_summary is not None else None + ), + duplicate_goal=goal_decision.duplicate, + duplicate_goal_failed_count=goal_decision.failed_duplicate_count, + duplicate_goal_rejected=goal_decision.reject, ) - # prepocess align with navdp - image_dp = torch.tensor(np.array(look_down_image.resize((224, 224)))).to(torch.bfloat16) / 255 - pix_goal_image = copy.copy(image_dp) - images_dp = torch.stack([pix_goal_image, image_dp]).unsqueeze(0).to(self.device) - depth_dp = look_down_depth.unsqueeze(-1).to(torch.bfloat16) - pix_goal_depth = copy.copy(depth_dp) - depths_dp = torch.stack([pix_goal_depth, depth_dp]).unsqueeze(0).to(self.device) - - current_s1_plan_id = s1_plan_id - s1_plan_id += 1 - s1_started = time.perf_counter() - with torch.no_grad(): - dp_actions = self.model.generate_traj(traj_latents, images_dp, depths_dp) - s1_generate_ms = (time.perf_counter() - s1_started) * 1000 + current_s1_plan_id = None + s1_generate_ms = 0.0 + if goal_decision.reject: + dp_actions = torch.zeros((1, 4, 3), dtype=torch.float32, device=self.device) + else: + if look_down_image is None: + ( + observations, + look_down_image, + look_down_depth, + low_head_depth_m, + front_depth_m, + ) = self._capture_low_head_inputs( + observations, + step_id, + reason='prepare_new_s1_goal', + ) - if self.diagnostic_logger is not None: + # Preprocess inputs aligned with NavDP only when S1 will run. + image_dp = torch.tensor( + np.array(look_down_image.resize((224, 224))) + ).to(torch.bfloat16) / 255 + pix_goal_image = copy.copy(image_dp) + images_dp = torch.stack([pix_goal_image, image_dp]).unsqueeze(0).to(self.device) + depth_dp = look_down_depth.unsqueeze(-1).to(torch.bfloat16) + pix_goal_depth = copy.copy(depth_dp) + depths_dp = torch.stack([pix_goal_depth, depth_dp]).unsqueeze(0).to(self.device) + current_s1_plan_id = s1_plan_id + s1_plan_id += 1 + s1_started = time.perf_counter() + with torch.no_grad(): + dp_actions = self.model.generate_traj(traj_latents, images_dp, depths_dp) + s1_generate_ms = (time.perf_counter() - s1_started) * 1000 + + if self.diagnostic_logger is not None and not goal_decision.reject: self.diagnostic_logger.save_s1_plan( current_s1_plan_id, dp_actions, @@ -740,11 +933,16 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 decision_step=step_id, ) - self.recovery_controller.start_new_goal() - action_list, trajectory_selection, chunk_size = self._prepare_s1_actions( - dp_actions, depth=front_depth_m - ) - if self.diagnostic_logger is not None: + if goal_decision.reject: + action_list, trajectory_selection, chunk_size = [], None, 1 + else: + self.recovery_controller.start_new_goal( + preserve_failures=goal_decision.duplicate and previous_goal_failed + ) + action_list, trajectory_selection, chunk_size = self._prepare_s1_actions( + dp_actions, depth=front_depth_m + ) + if self.diagnostic_logger is not None and not goal_decision.reject: self.diagnostic_logger.log( 's1_discretization', plan_id=current_s1_plan_id, @@ -774,6 +972,14 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 depth_risk=( trajectory_selection.depth_risk if trajectory_selection is not None else None ), + route_direction=( + trajectory_selection.route_direction if trajectory_selection is not None else None + ), + failed_route_penalty=( + trajectory_selection.failed_route_penalty + if trajectory_selection is not None + else None + ), ) if len(action_list) < MAX_STEPS: action_list += [0] * (MAX_STEPS - len(action_list)) @@ -782,8 +988,32 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 if len(local_actions) >= chunk_size: local_actions = local_actions[:chunk_size] - action = local_actions.pop(0) - if action == action_code.STOP: + if goal_decision.reject: + rejected_goal = list(pixel_goal) + pixel_goal = None + output_ids = None + local_actions = [] + action_seq = [self.recovery_controller.exploratory_turn()] + recovery_context = ( + f"The near-duplicate waypoint {rejected_goal} was rejected after repeated failure. " + "Use the new observation after this small exploratory turn and choose a visibly " + "different reachable waypoint; do not repeat the rejected pixel goal." + ) + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 'pixel_goal_rejected', + s2_call_id=current_s2_call_id, + decision_step=step_id, + pixel_goal=rejected_goal, + failed_duplicate_count=goal_decision.failed_duplicate_count, + exploratory_action=int(action_seq[0]), + failed_route_directions=dict( + self.recovery_controller.failed_route_directions + ), + ) + else: + action = local_actions.pop(0) + if not goal_decision.reject and action == action_code.STOP: pixel_goal = None output_ids = None local_actions = [] @@ -802,20 +1032,99 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 print('predicted goal', pixel_goal, flush=True) else: - action_seq = self.parse_actions(llm_outputs) + action_seq = ( + [action_code.STOP] + if forced_stop + else self.parse_actions(llm_outputs) + ) + action_seq = bound_actions_at_lookdown( + action_seq, lookdown_action=action_code.LOOKDOWN + ) + original_direct_actions = [int(item) for item in action_seq] + action_seq, direct_action_override = ( + self.recovery_controller.filter_direct_actions( + action_seq, + centre_clearance=( + latest_depth_summary.centre_clearance + if latest_depth_summary is not None + else None + ), + centre_blocked=( + latest_depth_summary.centre_blocked + if latest_depth_summary is not None + else False + ), + ) + ) + if direct_action_override is not None: + recovery_context = ( + "The previous same-direction rotation completed a full circle without " + "translation, so the controller replaced the repeated turn sequence with " + "one bounded escape action. Reassess the new observation instead of " + "repeating the failed full-circle direction." + ) + if self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 'direct_action_override', + s2_call_id=current_s2_call_id, + decision_step=step_id, + reason=direct_action_override, + original_actions=original_direct_actions, + replacement_actions=[int(item) for item in action_seq], + centre_clearance=( + latest_depth_summary.centre_clearance + if latest_depth_summary is not None + else None + ), + centre_blocked=( + latest_depth_summary.centre_blocked + if latest_depth_summary is not None + else None + ), + ) + stop_rejected = False + if ( + action_seq == [action_code.STOP] + and self.s2_reject_unverified_stair_stop + and instruction_state.should_reject_stop() + ): + action_seq = [action_code.LOOKDOWN] + stop_rejected = True + recovery_context = ( + "STOP was rejected because a stair subtask still lacks measured vertical " + "completion. Inspect the low-head view, continue through the stair corridor, " + "and stop only after the required height change or landing is confirmed." + ) + elif not action_seq and model_completed_subtask: + action_seq = [ + action_code.LOOKDOWN + if instruction_state.stair_mode + else self.recovery_controller.exploratory_turn() + ] if self.diagnostic_logger is not None: self.diagnostic_logger.log( 's2_inference', s2_call_id=current_s2_call_id, decision_step=step_id, raw_output=llm_outputs, - output_type='stop' if action_seq == [action_code.STOP] else 'direct_actions', + output_type=( + 'stop_forced' + if forced_stop + else 'stop_rejected' + if stop_rejected + else ('stop' if action_seq == [action_code.STOP] else 'direct_actions') + ), parsed_actions=[int(item) for item in action_seq], history_frame_ids=history_id if action != action_code.LOOKDOWN else [], input_image_count=len(input_images), generated_token_count=int(output_ids.shape[-1] - inputs.input_ids.shape[1]), generate_ms=s2_generate_ms, recovery_context=s2_recovery_context, + forced_stop=forced_stop, + instruction_state=instruction_state.as_dict(), + depth_summary=( + latest_depth_summary.__dict__ if latest_depth_summary is not None else None + ), ) print('actions', action_seq, flush=True) @@ -828,6 +1137,18 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 if len(local_actions) == 0: # navdp local_actions = [] + if look_down_image is None: + ( + observations, + look_down_image, + look_down_depth, + low_head_depth_m, + front_depth_m, + ) = self._capture_low_head_inputs( + observations, + step_id, + reason='prepare_s1_replan', + ) image_dp = torch.tensor(np.array(look_down_image.resize((224, 224)))).to(torch.bfloat16) / 255 images_dp = torch.stack([pix_goal_image, image_dp]).unsqueeze(0).to(self.device) @@ -892,6 +1213,14 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 depth_risk=( trajectory_selection.depth_risk if trajectory_selection is not None else None ), + route_direction=( + trajectory_selection.route_direction if trajectory_selection is not None else None + ), + failed_route_penalty=( + trajectory_selection.failed_route_penalty + if trajectory_selection is not None + else None + ), ) if len(action_list) < MAX_STEPS: action_list += [0] * (MAX_STEPS - len(action_list)) @@ -995,8 +1324,19 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 first_person_frame_id=first_person_frame_id - 1 if vis_writer is not None else None, top_down_frame_id=top_down_frame_id - 1 if self.save_video else None, ) + camera_pitch.record_look_down(count=2) flag = True else: + if camera_pitch.restore_steps: + for _ in range(camera_pitch.restore_steps): + observations, _, _, _ = self._diagnostic_env_step( + action_code.LOOKUP, + observations, + step_id, + 'camera_adjustment', + reason='restore_before_non_lookdown_action', + ) + camera_pitch.mark_horizontal() gps_before_action = np.asarray(observations.get('gps', [np.nan, np.nan])).copy() observations, _, done, _ = self._diagnostic_env_step( action, @@ -1013,6 +1353,20 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 first_person_frame_id=first_person_frame_id - 1 if vis_writer is not None else None, top_down_frame_id=top_down_frame_id - 1 if self.save_video else None, ) + if action == action_code.LOOKUP: + camera_pitch.record_look_up() + instruction_event = instruction_state.observe( + self._agent_height(), + observations.get('compass', [np.nan]), + observations.get('gps', [np.nan, np.nan]), + ) + if instruction_event and self.diagnostic_logger is not None: + self.diagnostic_logger.log( + 'instruction_state_update', + decision_step=step_id, + event=instruction_event, + instruction_state=instruction_state.as_dict(), + ) collision, _ = self._collision_values(self.env.get_metrics()) recovery = self.recovery_controller.update( action, @@ -1041,6 +1395,9 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 turn_count=recovery.turn_count, consecutive_failures=self.recovery_controller.consecutive_failures, failed_directions=dict(self.recovery_controller.failed_directions), + failed_route_directions=dict( + self.recovery_controller.failed_route_directions + ), replan_system2=recovery.replan_system2, ) if recovery.replan_system2: @@ -1080,6 +1437,11 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 # After the episode finishes, collect metrics: metrics = self.env.get_metrics() + final_instruction_state = instruction_state.as_dict() + semantic_status = instruction_state.semantic_status( + metrics['success'], manual_label=manual_semantic_label + ) + instruction_ambiguous = bool(manual_semantic_label.get('ambiguous', False)) sucs.append(metrics['success']) spls.append(metrics['spl']) @@ -1103,6 +1465,10 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 'distance_to_goal': metrics['distance_to_goal'], 'ndtw': metrics.get('ndtw'), 'collisions': metrics.get('collisions'), + 'semantic_status': semantic_status, + 'instruction_ambiguous': instruction_ambiguous, + 'instruction_state': final_instruction_state, + 'manual_semantic_label': manual_semantic_label, } diagnostic_summary = self.diagnostic_logger.finish_episode( metric_summary, @@ -1132,6 +1498,10 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 "ne": metrics["distance_to_goal"], "steps": step_id, "episode_instruction": episode_instruction, + "semantic_status": semantic_status, + "instruction_ambiguous": instruction_ambiguous, + "semantic_note": manual_semantic_label.get('note'), + "instruction_state": final_instruction_state, } if 'ndtw' in metrics: result['ndtw'] = metrics['ndtw'] diff --git a/internnav/habitat_extensions/vln/navigation_state.py b/internnav/habitat_extensions/vln/navigation_state.py new file mode 100644 index 00000000..483aa2ac --- /dev/null +++ b/internnav/habitat_extensions/vln/navigation_state.py @@ -0,0 +1,553 @@ +"""Lightweight structured context for S2 navigation decisions. + +The helpers in this module deliberately avoid learned components. They turn +signals already produced by Habitat (instruction text, depth, pose, and recent +goals) into compact state that can be logged, unit-tested, and optionally added +to the S2 prompt. +""" + +from collections import deque +from dataclasses import dataclass +import json +import re +from pathlib import Path +from typing import Optional + +import numpy as np + + +_ACTION_START = ( + "turn|walk|go|head|move|exit|enter|stop|wait|step|continue|take|follow|" + "face|look|proceed|pass|cross|climb|descend" +) +_NUMBER_WORDS = { + "one": 1, + "two": 2, + "three": 3, + "four": 4, + "five": 5, + "six": 6, + "seven": 7, + "eight": 8, + "nine": 9, + "ten": 10, + "tenth": 10, +} + + +@dataclass +class CameraPitchState: + """Track temporary low-head camera actions in 15-degree Habitat steps.""" + + down_steps: int = 0 + + def record_look_down(self, count=1): + self.down_steps += max(0, int(count)) + + def record_look_up(self, count=1): + self.down_steps = max(0, self.down_steps - max(0, int(count))) + + @property + def restore_steps(self): + return self.down_steps + + def mark_horizontal(self): + self.down_steps = 0 + + +def bound_actions_at_lookdown(actions, lookdown_action=5): + """Make LOOKDOWN an observation boundary, not a queued persistent tilt.""" + sequence = [int(action) for action in actions] + try: + index = sequence.index(int(lookdown_action)) + except ValueError: + return sequence + return sequence[: index + 1] + + +def split_instruction(instruction: str) -> tuple[str, ...]: + """Split an R2R instruction into conservative action-sized clauses.""" + text = re.sub(r"\s+", " ", str(instruction).strip()).strip(" .") + if not text: + return () + text = re.sub(r"\b(?:and then|then|after that|afterwards|next)\b", "|", text, flags=re.I) + text = re.sub(r"\b(?:once you|when you)\b", "|", text, flags=re.I) + text = re.sub(r"[.;]+", "|", text) + text = re.sub(r",\s*(?:and\s+)?(?=(?:" + _ACTION_START + r")\b)", "|", text, flags=re.I) + text = re.sub(r"\s+and\s+(?=(?:" + _ACTION_START + r")\b)", "|", text, flags=re.I) + parts = [part.strip(" ,.") for part in text.split("|") if part.strip(" ,.")] + merged = [] + for part in parts: + if ( + merged + and re.match( + r"^stop\s+(?:at|on)\s+the\s+(?:very\s+)?(?:top|bottom|landing)\b", + part, + re.I, + ) + and re.search( + r"\b(stair|stairs|stairway|step|steps|upstairs|downstairs|climb|descend)\b", + merged[-1], + re.I, + ) + ): + merged[-1] = f"{merged[-1]} and {part}" + else: + merged.append(part) + return tuple(merged) + + +def select_history_indices(frame_count: int, num_history: int, prioritize_recent: bool = False) -> list[int]: + """Select prior RGB frames while excluding the current final frame.""" + previous_count = max(0, int(frame_count) - 1) + count = min(max(0, int(num_history)), previous_count) + if count == 0: + return [] + if count == previous_count: + return list(range(previous_count)) + + recent_fraction = 0.75 if prioritize_recent else 0.5 + recent_count = min(count, max(1, int(np.ceil(count * recent_fraction)))) + recent = list(range(previous_count - recent_count, previous_count)) + older_count = count - len(recent) + older_stop = previous_count - recent_count + older = [] + if older_count > 0 and older_stop > 0: + older = np.unique(np.linspace(0, older_stop - 1, older_count, dtype=np.int32)).tolist() + selected = sorted(set(int(index) for index in older + recent)) + if len(selected) < count: + for index in range(previous_count - 1, -1, -1): + if index not in selected: + selected.append(index) + if len(selected) == count: + break + return sorted(selected[-count:] if len(selected) > count else selected) + + +def select_uniform_history_indices(frame_count: int, num_history: int) -> list[int]: + """Restore the original evenly spaced history without semantic recency bias.""" + previous_count = max(0, int(frame_count) - 1) + count = min(max(0, int(num_history)), previous_count) + if count == 0: + return [] + if count == previous_count: + return list(range(previous_count)) + return np.unique( + np.linspace(0, previous_count - 1, count, dtype=np.int32) + ).tolist() + + +@dataclass(frozen=True) +class DepthSummary: + text: str + left_clearance: Optional[float] + centre_clearance: Optional[float] + right_clearance: Optional[float] + invalid_fraction: float + centre_open: bool + centre_blocked: bool + step_edge_count: int + + +class DepthObservationSummarizer: + """Summarize a low-head depth frame without projecting or building a map.""" + + def __init__(self, near_distance=0.55, open_margin=0.25): + self.near_distance = float(near_distance) + self.open_margin = float(open_margin) + + @staticmethod + def _clearance(region): + values = region[np.isfinite(region) & (region > 0.05)] + if values.size == 0: + return None + return float(np.quantile(values, 0.15)) + + def summarize(self, depth, stair_mode=False) -> DepthSummary: + array = np.asarray(depth, dtype=np.float32).squeeze() + if array.ndim != 2: + raise ValueError("depth must be a 2-D image in metres") + height, width = array.shape + crop = array[int(height * 0.18) : max(int(height * 0.95), 1), int(width * 0.05) : int(width * 0.95)] + valid = np.isfinite(crop) & (crop > 0.05) + invalid_fraction = float(1.0 - np.mean(valid)) if crop.size else 1.0 + thirds = np.array_split(crop, 3, axis=1) + clearances = [self._clearance(region) for region in thirds] + left, centre, right = clearances + + centre_open = bool( + centre is not None + and left is not None + and right is not None + and centre >= max(left, right) + self.open_margin + ) + centre_values = thirds[1][np.isfinite(thirds[1]) & (thirds[1] > 0.05)] + centre_near_fraction = ( + float(np.mean(centre_values < self.near_distance)) if centre_values.size else 1.0 + ) + centre_blocked = bool(centre is not None and centre < self.near_distance and centre_near_fraction >= 0.15) + + centre_strip = crop[:, crop.shape[1] // 3 : (2 * crop.shape[1]) // 3] + row_medians = [] + for band in np.array_split(centre_strip, 8, axis=0): + values = band[np.isfinite(band) & (band > 0.05)] + row_medians.append(float(np.median(values)) if values.size else np.nan) + row_medians = np.asarray(row_medians, dtype=np.float32) + finite_pairs = np.isfinite(row_medians[:-1]) & np.isfinite(row_medians[1:]) + changes = np.abs(np.diff(row_medians))[finite_pairs] + step_edge_count = int(np.sum((changes >= 0.12) & (changes <= 1.25))) + + def render(value): + return "unknown" if value is None else f"{value:.2f} m" + + statements = [ + "Low-head geometric depth summary", + f"left/centre/right near clearance is {render(left)} / {render(centre)} / {render(right)}", + ] + if invalid_fraction >= 0.35: + statements.append( + f"depth is unreliable in {invalid_fraction:.0%} of the inspected area, so verify with RGB" + ) + if centre_open: + statements.append("the centre is more open than both sides") + if stair_mode: + statements.append( + "side obstacles may be railings; prefer the open centre corridor and do not aim at the sides" + ) + elif centre_blocked: + statements.append("the centre has a near obstacle; do not move forward without visual confirmation") + if stair_mode and step_edge_count >= 2: + statements.append( + "the centre depth has repeated discontinuities consistent with steps, but this is not semantic proof" + ) + statements.append("depth describes geometry only and cannot by itself identify stairs or glass") + return DepthSummary( + text="; ".join(statements) + ".", + left_clearance=left, + centre_clearance=centre, + right_clearance=right, + invalid_fraction=invalid_fraction, + centre_open=centre_open, + centre_blocked=centre_blocked, + step_edge_count=step_edge_count, + ) + + +@dataclass(frozen=True) +class PixelGoalDecision: + duplicate: bool + failed_duplicate_count: int + reject: bool + pixel_distance: Optional[float] + pose_distance: Optional[float] + heading_difference_deg: Optional[float] + + +class PixelGoalMemory: + """Detect failed near-duplicate S2 pixel goals from nearly the same pose.""" + + def __init__(self, pixel_tolerance=32.0, pose_tolerance=0.30, heading_tolerance_deg=20.0, retry_limit=2): + self.pixel_tolerance = float(pixel_tolerance) + self.pose_tolerance = float(pose_tolerance) + self.heading_tolerance_rad = np.deg2rad(float(heading_tolerance_deg)) + self.retry_limit = int(retry_limit) + self.reset() + + def reset(self): + self.last_goal = None + self.last_gps = None + self.last_compass = None + self.failed_duplicate_count = 0 + + @staticmethod + def _compass_value(compass): + values = np.asarray(compass, dtype=np.float32).reshape(-1) + return float(values[0]) if values.size and np.isfinite(values[0]) else None + + def evaluate(self, pixel_goal, gps, compass, previous_goal_failed=False): + goal = np.asarray(pixel_goal, dtype=np.float32).reshape(-1)[:2] + position = np.asarray(gps, dtype=np.float32).reshape(-1)[:2] + heading = self._compass_value(compass) + pixel_distance = pose_distance = heading_difference = None + duplicate = False + if self.last_goal is not None: + pixel_distance = float(np.linalg.norm(goal - self.last_goal)) + if self.last_gps is not None and position.size == 2 and np.all(np.isfinite(position)): + pose_distance = float(np.linalg.norm(position - self.last_gps)) + if self.last_compass is not None and heading is not None: + delta = np.arctan2(np.sin(heading - self.last_compass), np.cos(heading - self.last_compass)) + heading_difference = float(abs(delta)) + duplicate = bool( + pixel_distance <= self.pixel_tolerance + and pose_distance is not None + and pose_distance <= self.pose_tolerance + and heading_difference is not None + and heading_difference <= self.heading_tolerance_rad + ) + + if duplicate and previous_goal_failed: + self.failed_duplicate_count += 1 + elif not duplicate: + self.failed_duplicate_count = 0 + reject = bool(duplicate and previous_goal_failed and self.failed_duplicate_count >= self.retry_limit) + if not reject: + self.last_goal = goal.copy() + self.last_gps = position.copy() if position.size == 2 and np.all(np.isfinite(position)) else None + self.last_compass = heading + return PixelGoalDecision( + duplicate=duplicate, + failed_duplicate_count=self.failed_duplicate_count, + reject=reject, + pixel_distance=pixel_distance, + pose_distance=pose_distance, + heading_difference_deg=( + float(np.rad2deg(heading_difference)) if heading_difference is not None else None + ), + ) + + +class InstructionStateTracker: + """Conservative instruction state with explicit stair-height evidence.""" + + def __init__(self, instruction, initial_height=0.0, initial_compass=0.0, initial_gps=None): + self.instruction = str(instruction) + self.clauses = split_instruction(self.instruction) or (self.instruction.strip(),) + self.current_index = 0 + self.completed = [] + self.completion_reasons = [] + self.current_start_height = float(initial_height) + self.current_start_compass = float(initial_compass) + self.current_start_gps = self._gps(initial_gps) + self.current_height = float(initial_height) + self.current_compass = float(initial_compass) + self.current_gps = self.current_start_gps + self.height_history = deque([float(initial_height)], maxlen=6) + self.stop_rejections = 0 + + @staticmethod + def _gps(gps): + values = np.asarray(gps if gps is not None else [np.nan, np.nan], dtype=np.float32).reshape(-1) + if values.size < 2 or not np.all(np.isfinite(values[:2])): + return None + return values[:2].copy() + + @property + def current_clause(self): + return self.clauses[self.current_index] if self.current_index < len(self.clauses) else None + + @property + def remaining(self): + return self.clauses[self.current_index :] + + @property + def all_complete(self): + return self.current_index >= len(self.clauses) + + @staticmethod + def is_stair_clause(clause): + return bool(clause and re.search(r"\b(stair|stairs|stairway|step|steps|upstairs|downstairs|climb|descend)\b", clause, re.I)) + + @property + def stair_mode(self): + return self.is_stair_clause(self.current_clause) + + @staticmethod + def _stair_direction(clause): + if not clause: + return None + if re.search(r"\b(down|descend|downstairs|bottom)\b", clause, re.I): + return "down" + if re.search(r"\b(up|climb|upstairs|top)\b", clause, re.I): + return "up" + return None + + @staticmethod + def _step_count(clause): + if not clause: + return None + match = re.search(r"\b(\d+)\s*(?:stair|stairs|step|steps)\b", clause, re.I) + if match: + return int(match.group(1)) + lowered = clause.lower() + for word, value in _NUMBER_WORDS.items(): + if re.search(rf"\b{word}\s+(?:stair|stairs|step|steps)\b", lowered): + return value + return None + + def _stair_required_height(self, clause): + count = self._step_count(clause) + if count is not None: + return max(0.18, min(1.8, 0.14 * count)) + return 0.45 + + def _advance(self, reason): + if self.all_complete: + return False + self.completed.append(self.current_clause) + self.completion_reasons.append(str(reason)) + self.current_index += 1 + self.current_start_height = self.current_height + self.current_start_compass = self.current_compass + self.current_start_gps = None if self.current_gps is None else self.current_gps.copy() + self.height_history.clear() + self.height_history.append(self.current_height) + return True + + def observe(self, height, compass, gps=None): + self.current_height = float(height) + compass_values = np.asarray(compass, dtype=np.float32).reshape(-1) + if compass_values.size and np.isfinite(compass_values[0]): + self.current_compass = float(compass_values[0]) + self.current_gps = self._gps(gps) + self.height_history.append(self.current_height) + clause = self.current_clause + if clause is None: + return None + + stair_direction = self._stair_direction(clause) if self.is_stair_clause(clause) else None + if stair_direction: + signed_delta = self.current_height - self.current_start_height + progress = signed_delta if stair_direction == "up" else -signed_delta + required = self._stair_required_height(clause) + height_met = progress >= required + endpoint_required = bool(re.search(r"\b(top|bottom|end|landing)\b", clause, re.I)) + plateau = len(self.height_history) >= 4 and np.ptp(np.asarray(self.height_history)[-4:]) <= 0.08 + if height_met and (not endpoint_required or plateau): + self._advance("vertical_height_and_plateau" if endpoint_required else "vertical_height") + return "stair_complete" + + lowered = clause.lower() + pure_turn = bool(re.match(r"^turn\s+(?:left|right|around)\b", lowered)) and not re.search( + r"\b(walk|go|move|enter|exit|step|stairs?)\b", lowered + ) + if pure_turn: + delta = np.arctan2( + np.sin(self.current_compass - self.current_start_compass), + np.cos(self.current_compass - self.current_start_compass), + ) + required = np.deg2rad(140.0 if "around" in lowered else 60.0) + if abs(delta) >= required: + self._advance("heading_change") + return "turn_complete" + return None + + def mark_model_completion(self, output): + if re.search(r"\b(?:SUBTASK_DONE|CURRENT_SUBTASK_DONE)\b", str(output), re.I): + if self.stair_mode: + return False + return self._advance("s2_explicit_marker") + return False + + def unresolved_stair_clauses(self): + return [clause for clause in self.remaining if self.is_stair_clause(clause)] + + def _current_stair_evidence(self): + clause = self.current_clause + direction = self._stair_direction(clause) if self.is_stair_clause(clause) else None + if direction is None: + return None + signed_delta = self.current_height - self.current_start_height + progress = signed_delta if direction == "up" else -signed_delta + endpoint_required = bool(re.search(r"\b(top|bottom|end|landing)\b", clause, re.I)) + plateau = len(self.height_history) >= 4 and np.ptp(np.asarray(self.height_history)[-4:]) <= 0.08 + return { + "progress": float(progress), + "required": float(self._stair_required_height(clause)), + "endpoint_required": endpoint_required, + "plateau": bool(plateau), + } + + def terminal_stop_evidence(self): + """Return whether height tracking found a terminal stair endpoint.""" + return bool( + self.all_complete + and self.completion_reasons + and self.completion_reasons[-1] == "vertical_height_and_plateau" + ) + + def should_force_stop(self, stall_confirmed=False): + """Force STOP only after endpoint evidence and an independent turn stall.""" + return bool(self.terminal_stop_evidence() and stall_confirmed) + + def should_reject_stop(self): + """Reject STOP while ordered stair completion still lacks evidence.""" + if self.all_complete or not self.unresolved_stair_clauses(): + return False + self.stop_rejections += 1 + evidence = self._current_stair_evidence() + if evidence is None: + # A future stair clause is still unresolved, so accepting STOP would + # skip at least one ordered subtask. + return True + if evidence["progress"] < evidence["required"]: + return True + return bool(evidence["endpoint_required"] and not evidence["plateau"]) + + def prompt_context(self): + completed = " | ".join(self.completed) if self.completed else "none confirmed" + current = self.current_clause or "all parsed subtasks are complete" + remaining = " | ".join(self.remaining[1:]) if len(self.remaining) > 1 else "none after current" + pieces = [ + "Controller-maintained instruction state (verify against the images)", + f"confirmed completed: {completed}", + f"current subtask: {current}", + f"later subtasks: {remaining}", + ] + if self.stair_mode: + direction = self._stair_direction(self.current_clause) or "unknown" + delta = self.current_height - self.current_start_height + pieces.append( + f"current stair direction is {direction}; measured vertical change since this subtask began is {delta:+.2f} m" + ) + pieces.append("do not treat a side railing as the traversable stair corridor") + pieces.append( + "If the current subtask has just been completed, include SUBTASK_DONE in the reply before selecting the next waypoint" + ) + pieces.append("output STOP only when the full instruction is complete") + return "; ".join(pieces) + "." + + def semantic_status(self, metric_success, manual_label=None): + if manual_label and manual_label.get("semantic_status"): + return str(manual_label["semantic_status"]) + if not bool(metric_success): + return "metric_failure" + if self.all_complete: + return "heuristic_complete" + if self.unresolved_stair_clauses(): + return "metric_success_stair_unverified" + return "metric_success_semantically_unverified" + + def as_dict(self): + return { + "clauses": list(self.clauses), + "completed": list(self.completed), + "completion_reasons": list(self.completion_reasons), + "current_subtask": self.current_clause, + "remaining": list(self.remaining), + "all_complete": self.all_complete, + "stair_mode": self.stair_mode, + "height_change_m": self.current_height - self.current_start_height, + "stop_rejections": self.stop_rejections, + "terminal_stop_evidence": self.terminal_stop_evidence(), + "force_stop_ready": False, + } + + +def load_semantic_labels(path): + if not path: + return {} + label_path = Path(path) + if not label_path.exists(): + return {} + with label_path.open("r", encoding="utf-8") as handle: + payload = json.load(handle) + return payload if isinstance(payload, dict) else {} + + +def semantic_label_for_episode(labels, scene_id, episode_id): + keys = (f"{scene_id}_{episode_id}", f"{scene_id}/{episode_id}", str(episode_id)) + for key in keys: + value = labels.get(key) + if isinstance(value, dict): + return value + return {} diff --git a/internnav/habitat_extensions/vln/recovery_controller.py b/internnav/habitat_extensions/vln/recovery_controller.py index 521d5131..016662e8 100644 --- a/internnav/habitat_extensions/vln/recovery_controller.py +++ b/internnav/habitat_extensions/vln/recovery_controller.py @@ -24,30 +24,106 @@ def __init__( min_forward_displacement=0.03, system2_retry_limit=3, max_consecutive_turns=24, + direct_turn_escape_clearance=0.55, ): self.min_forward_displacement = float(min_forward_displacement) self.system2_retry_limit = int(system2_retry_limit) self.max_consecutive_turns = int(max_consecutive_turns) + self.direct_turn_escape_clearance = float(direct_turn_escape_clearance) self.failed_directions = Counter() + self.failed_route_directions = Counter() self.retry_count = 0 self.goal_retry_count = 0 self.consecutive_failures = 0 + self.current_route_direction = None self.turn_direction = None self.consecutive_turns = 0 + self.pending_turn_stall = False def reset(self): self.failed_directions.clear() + self.failed_route_directions.clear() self.retry_count = 0 + self.current_route_direction = None self.turn_direction = None self.consecutive_turns = 0 + self.pending_turn_stall = False self.start_new_goal() - def start_new_goal(self): - self.goal_retry_count = 0 - self.consecutive_failures = 0 + def start_new_goal(self, preserve_failures=False): + if not preserve_failures: + self.goal_retry_count = 0 + self.consecutive_failures = 0 + self.current_route_direction = None self.turn_direction = None self.consecutive_turns = 0 + def set_route_direction(self, direction): + direction = str(direction).lower() if direction is not None else None + self.current_route_direction = direction if direction in ("left", "straight", "right") else None + + def failed_route_context(self): + if not self.failed_route_directions: + return None + ordered = sorted(self.failed_route_directions.items(), key=lambda item: (-item[1], item[0])) + attempts = ", ".join(f"{direction} x{count}" for direction, count in ordered) + return ( + f"Failed route sectors from this episode: {attempts}. Prefer a less-failed visible route, " + "but returning or reversing remains allowed when the instruction requires correction." + ) + + def exploratory_turn(self): + """Return LEFT/RIGHT action code using the less-failed route sector.""" + left = self.failed_route_directions.get("left", 0) + right = self.failed_route_directions.get("right", 0) + if self.current_route_direction == "left": + return 3 + if self.current_route_direction == "right": + return 2 + return 2 if left <= right else 3 + + def consume_turn_stall(self): + """Return a recent full-circle stall once, then clear the signal.""" + pending = self.pending_turn_stall + self.pending_turn_stall = False + return pending + + def filter_direct_actions(self, actions, centre_clearance=None, centre_blocked=False): + """Replace a repeated full-circle S2 turn with one bounded escape action.""" + sequence = [int(action) for action in actions] + if not sequence or sequence[0] not in (2, 3) or any( + action != sequence[0] for action in sequence + ): + return sequence, None + failed_turn = sequence[0] + if self.failed_directions.get(failed_turn, 0) <= 0: + return sequence, None + + clearance = None + if centre_clearance is not None: + try: + value = float(centre_clearance) + clearance = value if np.isfinite(value) else None + except (TypeError, ValueError): + clearance = None + if ( + not bool(centre_blocked) + and clearance is not None + and clearance >= self.direct_turn_escape_clearance + ): + replacement = [1] + reason = "depth_gated_forward_after_repeated_direct_turn" + else: + replacement = [3 if failed_turn == 2 else 2] + reason = "opposite_probe_after_repeated_direct_turn" + + # Consume one failure marker. After the bounded escape the changed pose + # receives a fresh normal-turn budget instead of being permanently banned. + self.failed_directions[failed_turn] -= 1 + if self.failed_directions[failed_turn] <= 0: + del self.failed_directions[failed_turn] + return replacement, reason + def update(self, action, gps_before, gps_after, collision=False, is_s1=True): action = int(action) before = np.asarray(gps_before, dtype=np.float32).reshape(-1) @@ -80,6 +156,8 @@ def update(self, action, gps_before, gps_after, collision=False, is_s1=True): self.goal_retry_count += 1 self.consecutive_failures += 1 self.failed_directions[action] += 1 + if self.current_route_direction is not None: + self.failed_route_directions[self.current_route_direction] += 1 elif is_s1 and action == 1: self.goal_retry_count = 0 self.consecutive_failures = 0 @@ -88,6 +166,7 @@ def update(self, action, gps_before, gps_after, collision=False, is_s1=True): reason is not None and self.goal_retry_count >= self.system2_retry_limit ) if reason == "repeated_turns": + self.pending_turn_stall = True self.turn_direction = None self.consecutive_turns = 0 return RecoveryDecision( diff --git a/internnav/habitat_extensions/vln/trajectory_selector.py b/internnav/habitat_extensions/vln/trajectory_selector.py index 653e9809..d123bfd2 100644 --- a/internnav/habitat_extensions/vln/trajectory_selector.py +++ b/internnav/habitat_extensions/vln/trajectory_selector.py @@ -16,6 +16,8 @@ class TrajectorySelection: clearance: float smoothness: float depth_risk: bool + route_direction: str + failed_route_penalty: float class TrajectorySelector: @@ -99,6 +101,18 @@ def _smoothness(trajectories): turns = np.diff(headings, axis=1) return np.mean(np.abs(turns), axis=1) + @staticmethod + def direction_label(trajectory): + """Classify a candidate by its endpoint bearing in the local frame.""" + trajectory = np.asarray(trajectory, dtype=np.float32) + forward, lateral = trajectory[-1, :2] + bearing = float(np.arctan2(lateral, max(float(forward), 0.05))) + if bearing >= np.deg2rad(12.0): + return "left" + if bearing <= -np.deg2rad(12.0): + return "right" + return "straight" + def _depth_clearance(self, trajectories, depth): if depth is None: return np.full(trajectories.shape[0], np.inf, dtype=np.float32) @@ -108,28 +122,39 @@ def _depth_clearance(self, trajectories, depth): height, width = depth.shape row_start, row_stop = int(height * 0.30), max(int(height * 0.82), 1) - clearance = np.full(trajectories.shape[0], np.inf, dtype=np.float32) - for candidate_index, trajectory in enumerate(trajectories): - candidate_clearance = [] - for forward, lateral in trajectory[1:]: - radial = float(np.hypot(forward, lateral)) - if forward <= 0.05 or radial > self.depth_lookahead: - continue - bearing = float(np.arctan2(lateral, forward)) - if abs(bearing) >= self.horizontal_fov_rad / 2: - continue - column = int(round((0.5 - bearing / self.horizontal_fov_rad) * (width - 1))) - left, right = max(0, column - 3), min(width, column + 4) - values = depth[row_start:row_stop, left:right] - valid = values[np.isfinite(values) & (values > 0.05)] - if valid.size: - ray_depth = float(np.quantile(valid, 0.15)) - candidate_clearance.append(ray_depth - float(forward)) - if candidate_clearance: - clearance[candidate_index] = min(candidate_clearance) - return clearance - - def select(self, dp_actions, depth=None, recent_failure=False): + depth_crop = depth[row_start:row_stop] + valid_mask = np.isfinite(depth_crop) & (depth_crop > 0.05) + valid_columns = np.any(valid_mask, axis=0) + depth_profile = np.full(width, np.inf, dtype=np.float32) + if np.any(valid_columns): + valid_values = np.where(valid_mask[:, valid_columns], depth_crop[:, valid_columns], np.inf) + sorted_values = np.sort(valid_values, axis=0) + valid_counts = np.sum(valid_mask[:, valid_columns], axis=0) + percentile_indices = np.floor(0.15 * (valid_counts - 1)).astype(np.int32) + depth_profile[valid_columns] = sorted_values[ + percentile_indices, np.arange(sorted_values.shape[1]) + ] + + # A seven-column minimum approximates the original narrow ray window + # conservatively, while avoiding one quantile call per trajectory point. + padded = np.pad(depth_profile, (3, 3), constant_values=np.inf) + ray_profile = np.min(np.lib.stride_tricks.sliding_window_view(padded, 7), axis=1) + + forward = trajectories[:, 1:, 0] + lateral = trajectories[:, 1:, 1] + radial = np.hypot(forward, lateral) + bearing = np.arctan2(lateral, forward) + visible = ( + (forward > 0.05) + & (radial <= self.depth_lookahead) + & (np.abs(bearing) < self.horizontal_fov_rad / 2) + ) + columns = np.rint((0.5 - bearing / self.horizontal_fov_rad) * (width - 1)).astype(np.int32) + columns = np.clip(columns, 0, width - 1) + point_clearance = np.where(visible, ray_profile[columns] - forward, np.inf) + return np.min(point_clearance, axis=1).astype(np.float32) + + def select(self, dp_actions, depth=None, recent_failure=False, failed_route_directions=None): trajectories = self.reconstruct(dp_actions) flattened = trajectories.reshape(trajectories.shape[0], -1) pairwise = np.sqrt(np.mean((flattened[:, None] - flattened[None, :]) ** 2, axis=2)) @@ -139,13 +164,24 @@ def select(self, dp_actions, depth=None, recent_failure=False): bimodal, cluster_sizes = self._two_medoid_clusters(pairwise, trajectories) smoothness = self._smoothness(trajectories) clearance = self._depth_clearance(trajectories, depth) + directions = [self.direction_label(trajectory) for trajectory in trajectories] + failed_route_directions = failed_route_directions or {} + route_penalties = np.asarray( + [min(float(failed_route_directions.get(direction, 0)), 3.0) for direction in directions], + dtype=np.float32, + ) selected_index = medoid_index - if depth is not None: + if depth is not None or np.any(route_penalties > 0): centrality_scale = max(float(np.median(centrality)), 1e-6) smoothness_scale = max(float(np.median(smoothness)), 1e-6) clearance_penalty = np.maximum(self.near_clearance - clearance, 0.0) / self.near_clearance - scores = centrality / centrality_scale + 0.20 * smoothness / smoothness_scale + 4.0 * clearance_penalty + scores = ( + centrality / centrality_scale + + 0.20 * smoothness / smoothness_scale + + 4.0 * clearance_penalty + + 1.25 * route_penalties + ) selected_index = int(np.argmin(scores)) selected_clearance = float(clearance[selected_index]) @@ -166,4 +202,6 @@ def select(self, dp_actions, depth=None, recent_failure=False): clearance=selected_clearance, smoothness=float(smoothness[selected_index]), depth_risk=depth_risk, + route_direction=directions[selected_index], + failed_route_penalty=float(route_penalties[selected_index]), ) diff --git a/tests/unit_test/test_navigation_state.py b/tests/unit_test/test_navigation_state.py new file mode 100644 index 00000000..d852e93f --- /dev/null +++ b/tests/unit_test/test_navigation_state.py @@ -0,0 +1,208 @@ +import importlib.util +import sys +from pathlib import Path + +import numpy as np + + +def _load_module(name): + path = Path(__file__).parents[2] / "internnav" / "habitat_extensions" / "vln" / f"{name}.py" + spec = importlib.util.spec_from_file_location(name, path) + module = importlib.util.module_from_spec(spec) + sys.modules[name] = module + spec.loader.exec_module(module) + return module + + +navigation_state = _load_module("navigation_state") +recovery_module = _load_module("recovery_controller") +trajectory_module = _load_module("trajectory_selector") + +DepthObservationSummarizer = navigation_state.DepthObservationSummarizer +CameraPitchState = navigation_state.CameraPitchState +InstructionStateTracker = navigation_state.InstructionStateTracker +PixelGoalMemory = navigation_state.PixelGoalMemory +bound_actions_at_lookdown = navigation_state.bound_actions_at_lookdown +select_history_indices = navigation_state.select_history_indices +select_uniform_history_indices = navigation_state.select_uniform_history_indices +split_instruction = navigation_state.split_instruction +RecoveryController = recovery_module.RecoveryController +TrajectorySelector = trajectory_module.TrajectorySelector + + +def _constant_candidates(offsets, steps=4): + candidates = np.zeros((len(offsets), steps, 3), dtype=np.float32) + for index, lateral_delta in enumerate(offsets): + candidates[index, :, 0] = 1.0 + candidates[index, :, 1] = lateral_delta + return candidates + + +def test_instruction_split_exposes_turn_stair_and_stop_state(): + clauses = split_instruction("Turn right and step down two stairs and stop.") + assert clauses == ("Turn right", "step down two stairs", "stop") + + +def test_two_step_descent_requires_measured_height_change(): + tracker = InstructionStateTracker("Step down two stairs and stop.", initial_height=2.0) + assert tracker.stair_mode + assert tracker.observe(1.9, [0.0], [0.0, 0.0]) is None + assert tracker.observe(1.70, [0.0], [0.1, 0.0]) == "stair_complete" + assert tracker.current_clause == "stop" + + +def test_top_of_stairs_requires_height_and_a_plateau(): + tracker = InstructionStateTracker("Walk up the stairs and stop at the top.", initial_height=0.0) + for height in (0.2, 0.4, 0.65): + assert tracker.observe(height, [0.0], [0.0, 0.0]) is None + for height in (0.66, 0.65, 0.66): + event = tracker.observe(height, [0.0], [0.0, 0.0]) + assert event == "stair_complete" + assert tracker.terminal_stop_evidence() + assert not tracker.should_force_stop() + assert tracker.should_force_stop(stall_confirmed=True) + + +def test_very_top_stop_is_merged_into_the_preceding_stair_clause(): + clauses = split_instruction( + "Turn left and walk into the small hallway and up the stairs. " + "Stop at the very top of the stairs." + ) + assert clauses == ( + "Turn left", + "walk into the small hallway and up the stairs and Stop at the very top of the stairs", + ) + + +def test_s2_marker_cannot_override_missing_stair_height_evidence(): + tracker = InstructionStateTracker("Go down the stairs, then turn right.", initial_height=1.0) + assert not tracker.mark_model_completion("SUBTASK_DONE (120, 80)") + assert tracker.stair_mode + + +def test_unverified_stair_stop_requires_height_evidence_instead_of_retry_count(): + tracker = InstructionStateTracker("Go down the stairs and stop.", initial_height=1.0) + assert tracker.should_reject_stop() + assert tracker.should_reject_stop() + assert tracker.should_reject_stop() + assert tracker.observe(0.4, [0.0], [0.0, 0.0]) == "stair_complete" + assert not tracker.should_reject_stop() + + +def test_stop_is_rejected_when_an_ordered_future_stair_clause_is_unresolved(): + tracker = InstructionStateTracker( + "Head into the hall, then go down the stairs and stop.", initial_height=1.0 + ) + assert tracker.current_clause == "Head into the hall" + assert tracker.should_reject_stop() + assert tracker.stop_rejections == 1 + + +def test_non_terminal_completion_does_not_force_stop(): + tracker = InstructionStateTracker("Turn left and stop.", initial_compass=0.0) + assert tracker.observe(0.0, [np.deg2rad(70)], [0.0, 0.0]) == "turn_complete" + assert not tracker.should_force_stop() + + +def test_low_head_depth_identifies_open_centre_and_side_barriers(): + depth = np.full((120, 180), 3.0, dtype=np.float32) + depth[:, :60] = 0.35 + depth[:, 120:] = 0.40 + summary = DepthObservationSummarizer().summarize(depth, stair_mode=True) + assert summary.centre_open + assert not summary.centre_blocked + assert "railings" in summary.text + + +def test_failed_duplicate_goal_is_rejected_at_retry_limit(): + memory = PixelGoalMemory(retry_limit=2) + first = memory.evaluate([100, 80], [0.0, 0.0], [0.0], previous_goal_failed=False) + retry = memory.evaluate([105, 83], [0.1, 0.0], [0.05], previous_goal_failed=True) + rejected = memory.evaluate([103, 82], [0.1, 0.0], [0.04], previous_goal_failed=True) + assert not first.duplicate + assert retry.duplicate and not retry.reject + assert rejected.duplicate and rejected.reject + + +def test_goal_from_a_changed_heading_is_not_a_duplicate(): + memory = PixelGoalMemory(retry_limit=1, heading_tolerance_deg=20) + memory.evaluate([100, 80], [0.0, 0.0], [0.0]) + decision = memory.evaluate([100, 80], [0.0, 0.0], [np.deg2rad(45)], previous_goal_failed=True) + assert not decision.duplicate + assert not decision.reject + + +def test_history_selection_excludes_current_and_keeps_recent_frames(): + selected = select_history_indices(frame_count=20, num_history=6, prioritize_recent=True) + assert len(selected) == 6 + assert 19 not in selected + assert selected[-1] == 18 + assert sum(index >= 15 for index in selected) >= 4 + + +def test_uniform_history_selection_restores_nonsemantic_sampling(): + selected = select_uniform_history_indices(frame_count=20, num_history=6) + assert selected == [0, 3, 7, 10, 14, 18] + assert 19 not in selected + + +def test_failed_route_sector_changes_candidate_selection_without_hard_ban(): + candidates = _constant_candidates([1.0] * 8 + [-1.0] * 8) + selection = TrajectorySelector().select(candidates, failed_route_directions={"left": 2}) + assert selection.route_direction == "right" + assert selection.failed_route_penalty == 0.0 + + +def test_recovery_records_route_sector_and_chooses_opposite_observation_turn(): + controller = RecoveryController() + controller.set_route_direction("left") + controller.update(1, [0.0, 0.0], [0.0, 0.0], collision=True) + assert controller.failed_route_directions["left"] == 1 + assert controller.exploratory_turn() == 3 + assert "left x1" in controller.failed_route_context() + + +def test_repeated_direct_turn_uses_depth_gated_forward_once(): + controller = RecoveryController(max_consecutive_turns=4, direct_turn_escape_clearance=0.55) + gps = np.asarray([0.0, 0.0], dtype=np.float32) + for _ in range(4): + decision = controller.update(3, gps, gps, is_s1=False) + assert decision.reason == "repeated_turns" + assert controller.consume_turn_stall() + assert not controller.consume_turn_stall() + + actions, reason = controller.filter_direct_actions( + [3, 3, 3, 3], centre_clearance=0.9, centre_blocked=False + ) + assert actions == [1] + assert reason == "depth_gated_forward_after_repeated_direct_turn" + assert controller.filter_direct_actions([3, 3], 0.9, False) == ([3, 3], None) + + +def test_repeated_direct_turn_uses_opposite_probe_when_blocked(): + controller = RecoveryController(max_consecutive_turns=2) + gps = np.asarray([0.0, 0.0], dtype=np.float32) + controller.update(2, gps, gps, is_s1=False) + controller.update(2, gps, gps, is_s1=False) + + actions, reason = controller.filter_direct_actions( + [2, 2, 2], centre_clearance=0.3, centre_blocked=True + ) + assert actions == [3] + assert reason == "opposite_probe_after_repeated_direct_turn" + + +def test_temporary_low_head_view_requires_matching_restore_steps(): + camera = CameraPitchState() + camera.record_look_down(count=2) + assert camera.restore_steps == 2 + camera.record_look_up() + assert camera.restore_steps == 1 + camera.mark_horizontal() + assert camera.restore_steps == 0 + + +def test_lookdown_ends_the_direct_action_queue(): + assert bound_actions_at_lookdown([3, 3, 5, 1, 1]) == [3, 3, 5] + assert bound_actions_at_lookdown([5, 5, 5]) == [5] + assert bound_actions_at_lookdown([1, 2, 3]) == [1, 2, 3] From 05b727d92bf13ec64e10965b125ae19555a3c1f2 Mon Sep 17 00:00:00 2001 From: lighthqg Date: Tue, 11 Aug 2026 16:51:22 +0800 Subject: [PATCH 5/6] Visualize S2 decision points --- .../evaluator/utils/diagnostic_logger.py | 137 ++++++++++++++++++ .../vln/habitat_vln_evaluator.py | 86 +++++++++-- tests/unit_test/test_diagnostic_logger.py | 69 +++++++++ 3 files changed, 283 insertions(+), 9 deletions(-) create mode 100644 tests/unit_test/test_diagnostic_logger.py diff --git a/internnav/evaluator/utils/diagnostic_logger.py b/internnav/evaluator/utils/diagnostic_logger.py index 7e0ee763..ddedbeaa 100644 --- a/internnav/evaluator/utils/diagnostic_logger.py +++ b/internnav/evaluator/utils/diagnostic_logger.py @@ -13,6 +13,7 @@ import numpy as np import torch +from PIL import Image, ImageDraw DEFAULT_CRITERIA = { @@ -103,6 +104,47 @@ def summarize(values: np.ndarray, prefix: str) -> Dict[str, Optional[float]]: return stats +def project_pixel_point( + point: Iterable[float], + source_size: Iterable[int], + target_size: Iterable[int], +) -> tuple[int, int]: + """Scale a pixel point between image spaces without changing its meaning.""" + x, y = (float(value) for value in point) + source_width, source_height = (max(int(value), 1) for value in source_size) + target_width, target_height = (max(int(value), 1) for value in target_size) + projected_x = x * max(target_width - 1, 0) / max(source_width - 1, 1) + projected_y = y * max(target_height - 1, 0) / max(source_height - 1, 1) + return int(round(projected_x)), int(round(projected_y)) + + +def depth_at_pixel( + depth_m: Any, + point: Iterable[float], + image_size: Iterable[int], + radius: int = 2, +) -> Optional[float]: + """Return robust local depth at a point expressed in another image size.""" + depth = np.asarray(depth_m, dtype=np.float32).squeeze() + if depth.ndim != 2 or depth.size == 0: + return None + image_width, image_height = (max(int(value), 1) for value in image_size) + depth_x, depth_y = project_pixel_point( + point, + (image_width, image_height), + (depth.shape[1], depth.shape[0]), + ) + if not (0 <= depth_x < depth.shape[1] and 0 <= depth_y < depth.shape[0]): + return None + radius = max(int(radius), 0) + patch = depth[ + max(depth_y - radius, 0) : min(depth_y + radius + 1, depth.shape[0]), + max(depth_x - radius, 0) : min(depth_x + radius + 1, depth.shape[1]), + ] + valid = patch[np.isfinite(patch) & (patch > 0)] + return float(np.median(valid)) if valid.size else None + + class DiagnosticLogger: """Write an episode timeline and derive conservative navigation failure signals.""" @@ -156,12 +198,107 @@ def start_episode(self, episode_metadata: Dict[str, Any]) -> Path: self.episode_dir.mkdir(parents=True, exist_ok=True) (self.episode_dir / "depth").mkdir(exist_ok=True) (self.episode_dir / "plans").mkdir(exist_ok=True) + (self.episode_dir / "s2_decisions").mkdir(exist_ok=True) self.timeline_path = self.episode_dir / "timeline.jsonl" self.timeline_path.write_text("", encoding="utf-8") self._write_json(self.episode_dir / "episode.json", episode_metadata) self.log("episode_start", **episode_metadata) return self.episode_dir + def save_s2_decision( + self, + s2_call_id: int, + image: Any, + *, + decision_step: int, + output_type: str, + raw_output: str, + pixel_goal: Optional[Iterable[int]] = None, + action_names: Optional[Iterable[str]] = None, + current_subtask: Optional[str] = None, + depth_m: Any = None, + model_image_size: Optional[Iterable[int]] = None, + ) -> Dict[str, Any]: + """Save the exact current S2 image with a diagnostic-only decision overlay.""" + if self.episode_dir is None: + raise RuntimeError("start_episode must be called before saving an S2 decision") + if isinstance(image, Image.Image): + decision_image = image.convert("RGB") + else: + decision_image = Image.fromarray(np.asarray(image, dtype=np.uint8)).convert("RGB") + if model_image_size is not None: + model_size = tuple(int(value) for value in model_image_size) + if decision_image.size != model_size: + decision_image = decision_image.resize(model_size) + + point = [int(value) for value in pixel_goal] if pixel_goal is not None else None + point_depth_m = ( + depth_at_pixel(depth_m, point, decision_image.size) + if point is not None and depth_m is not None + else None + ) + action_names = [str(value) for value in (action_names or [])] + safe_output = str(raw_output).encode("ascii", errors="replace").decode("ascii") + safe_subtask = str(current_subtask or "unknown").encode("ascii", errors="replace").decode("ascii") + + draw = ImageDraw.Draw(decision_image, "RGBA") + draw.rectangle((0, 0, decision_image.width, 58), fill=(0, 0, 0, 190)) + draw.text( + (8, 7), + f"S2 #{int(s2_call_id)} step {int(decision_step)} {str(output_type).upper()}", + fill=(255, 255, 255, 255), + ) + draw.text((8, 24), f"output: {safe_output[:62]}", fill=(255, 255, 0, 255)) + draw.text((8, 41), f"subtask: {safe_subtask[:60]}", fill=(210, 220, 230, 255)) + + if point is not None: + x, y = point + colour = (0, 255, 255, 255) + draw.ellipse((x - 9, y - 9, x + 9, y + 9), outline=(0, 0, 0, 255), width=5) + draw.ellipse((x - 8, y - 8, x + 8, y + 8), outline=colour, width=3) + draw.line((x - 14, y, x + 14, y), fill=colour, width=2) + draw.line((x, y - 14, x, y + 14), fill=colour, width=2) + depth_label = "invalid" if point_depth_m is None else f"{point_depth_m:.2f} m" + draw.rectangle((x + 12, y - 12, x + 116, y + 10), fill=(0, 0, 0, 190)) + draw.text((x + 16, y - 9), f"GOAL {x},{y} {depth_label}", fill=colour) + elif "STOP" in action_names or str(output_type).startswith("stop"): + draw.rectangle( + ( + decision_image.width // 2 - 72, + decision_image.height // 2 - 27, + decision_image.width // 2 + 72, + decision_image.height // 2 + 27, + ), + fill=(160, 0, 0, 210), + outline=(255, 80, 80, 255), + width=3, + ) + draw.text( + (decision_image.width // 2 - 22, decision_image.height // 2 - 6), + "STOP", + fill=(255, 255, 255, 255), + ) + elif action_names: + label = " | ".join(action_names) + draw.rectangle( + (24, decision_image.height - 43, decision_image.width - 24, decision_image.height - 12), + fill=(0, 0, 0, 190), + outline=(255, 210, 0, 255), + width=2, + ) + draw.text((34, decision_image.height - 33), label[:52], fill=(255, 220, 0, 255)) + + file_path = self.episode_dir / "s2_decisions" / f"decision_{int(s2_call_id):04d}.jpg" + decision_image.save(file_path, format="JPEG", quality=90, optimize=True) + return { + "decision_image": str(file_path.relative_to(self.root_dir)), + "decision_image_size": list(decision_image.size), + "decision_point": point, + "decision_point_depth_m": point_depth_m, + "decision_action_names": action_names, + "decision_current_subtask": current_subtask, + } + def log(self, event_type: str, **fields: Any) -> Dict[str, Any]: if self.timeline_path is None: raise RuntimeError("start_episode must be called before logging") diff --git a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py index 7bb4695a..a3e68456 100644 --- a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py +++ b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py @@ -37,6 +37,7 @@ from internnav.evaluator.utils.diagnostic_logger import ( DiagnosticLogger, depth_statistics, + project_pixel_point, seed_everything, stable_episode_seed, ) @@ -853,6 +854,24 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 latent_generate_ms = (time.perf_counter() - latent_started) * 1000 if self.diagnostic_logger is not None: + decision_snapshot = self.diagnostic_logger.save_s2_decision( + current_s2_call_id, + input_images[-1], + decision_step=step_id, + output_type='pixel_goal', + raw_output=llm_outputs, + pixel_goal=pixel_goal, + current_subtask=instruction_state.current_clause, + depth_m=( + low_head_depth_m + if action == action_code.LOOKDOWN + else front_depth_m + ), + model_image_size=( + self.model_args.resize_w, + self.model_args.resize_h, + ), + ) self.diagnostic_logger.log( 's2_inference', s2_call_id=current_s2_call_id, @@ -879,6 +898,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 duplicate_goal=goal_decision.duplicate, duplicate_goal_failed_count=goal_decision.failed_duplicate_count, duplicate_goal_rejected=goal_decision.reject, + **decision_snapshot, ) current_s1_plan_id = None @@ -1102,18 +1122,43 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 else self.recovery_controller.exploratory_turn() ] if self.diagnostic_logger is not None: + decision_output_type = ( + 'stop_forced' + if forced_stop + else 'stop_rejected' + if stop_rejected + else ('stop' if action_seq == [action_code.STOP] else 'direct_actions') + ) + decision_action_names = [ + action_code(int(item)).name + if int(item) in action_code._value2member_map_ + else f'UNKNOWN_{int(item)}' + for item in action_seq + ] + decision_snapshot = self.diagnostic_logger.save_s2_decision( + current_s2_call_id, + input_images[-1], + decision_step=step_id, + output_type=decision_output_type, + raw_output=llm_outputs, + action_names=decision_action_names, + current_subtask=instruction_state.current_clause, + depth_m=( + low_head_depth_m + if action == action_code.LOOKDOWN + else front_depth_m + ), + model_image_size=( + self.model_args.resize_w, + self.model_args.resize_h, + ), + ) self.diagnostic_logger.log( 's2_inference', s2_call_id=current_s2_call_id, decision_step=step_id, raw_output=llm_outputs, - output_type=( - 'stop_forced' - if forced_stop - else 'stop_rejected' - if stop_rejected - else ('stop' if action_seq == [action_code.STOP] else 'direct_actions') - ), + output_type=decision_output_type, parsed_actions=[int(item) for item in action_seq], history_frame_ids=history_id if action != action_code.LOOKDOWN else [], input_image_count=len(input_images), @@ -1125,6 +1170,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 depth_summary=( latest_depth_summary.__dict__ if latest_depth_summary is not None else None ), + **decision_snapshot, ) print('actions', action_seq, flush=True) @@ -1259,7 +1305,18 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 if info['top_down_map'] is not None and self.save_video: frame = observations_to_image({'rgb': np.asarray(save_raw_image)}, info) if pixel_goal is not None and flag: - cv2.circle(frame, (pixel_goal[0], pixel_goal[1]), radius=8, color=(255, 0, 0), thickness=-1) + display_goal = project_pixel_point( + pixel_goal, + (self.model_args.resize_w, self.model_args.resize_h), + save_raw_image.size, + ) + cv2.circle( + frame, + display_goal, + radius=8, + color=(255, 255, 0), + thickness=-1, + ) vis_frames.append(frame) top_down_frame_id += 1 @@ -1297,7 +1354,18 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 ) if pixel_goal is not None: if draw_pixel_goal: - cv2.circle(vis, (pixel_goal[0], pixel_goal[1]), radius=8, color=(255, 0, 0), thickness=-1) + display_goal = project_pixel_point( + pixel_goal, + (self.model_args.resize_w, self.model_args.resize_h), + save_raw_image.size, + ) + cv2.circle( + vis, + display_goal, + radius=8, + color=(255, 255, 0), + thickness=-1, + ) vis_writer.append_data(vis) first_person_frame_id += 1 diff --git a/tests/unit_test/test_diagnostic_logger.py b/tests/unit_test/test_diagnostic_logger.py new file mode 100644 index 00000000..6e032894 --- /dev/null +++ b/tests/unit_test/test_diagnostic_logger.py @@ -0,0 +1,69 @@ +import importlib.util +import sys +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +from PIL import Image + + +if importlib.util.find_spec("torch") is None: + sys.modules["torch"] = SimpleNamespace(is_tensor=lambda _value: False) + + +def _load_module(): + path = Path(__file__).parents[2] / "internnav" / "evaluator" / "utils" / "diagnostic_logger.py" + spec = importlib.util.spec_from_file_location("diagnostic_logger", path) + module = importlib.util.module_from_spec(spec) + sys.modules["diagnostic_logger"] = module + spec.loader.exec_module(module) + return module + + +diagnostic_logger = _load_module() + + +def test_project_pixel_point_scales_model_coordinates_to_video_frame(): + assert diagnostic_logger.project_pixel_point((192, 192), (384, 384), (640, 480)) == (320, 240) + + +def test_save_s2_decision_draws_goal_and_samples_depth(tmp_path): + logger = diagnostic_logger.DiagnosticLogger(str(tmp_path), {"run_id": "test"}) + logger.start_episode({"scene_id": "scene", "episode_id": 1}) + image = Image.new("RGB", (384, 384), (20, 30, 40)) + depth = np.full((480, 640), 2.5, dtype=np.float32) + + result = logger.save_s2_decision( + 3, + image, + decision_step=7, + output_type="pixel_goal", + raw_output="192 192", + pixel_goal=(192, 192), + current_subtask="walk to the doorway", + depth_m=depth, + model_image_size=(384, 384), + ) + + output = tmp_path / result["decision_image"] + assert output.is_file() + assert result["decision_image_size"] == [384, 384] + assert result["decision_point"] == [192, 192] + assert abs(result["decision_point_depth_m"] - 2.5) < 1e-6 + assert np.asarray(Image.open(output))[192, 192].tolist() != [20, 30, 40] + + +def test_save_s2_decision_marks_stop_without_a_goal(tmp_path): + logger = diagnostic_logger.DiagnosticLogger(str(tmp_path), {"run_id": "test"}) + logger.start_episode({"scene_id": "scene", "episode_id": 2}) + result = logger.save_s2_decision( + 1, + Image.new("RGB", (384, 384), (0, 0, 0)), + decision_step=4, + output_type="stop", + raw_output="STOP", + action_names=["STOP"], + ) + assert (tmp_path / result["decision_image"]).is_file() + assert result["decision_point"] is None + assert result["decision_action_names"] == ["STOP"] From 625bfa08995fe4f6209ff7c83cd531e7fa42f4c0 Mon Sep 17 00:00:00 2001 From: lighthqg Date: Wed, 12 Aug 2026 20:57:28 +0800 Subject: [PATCH 6/6] Add DualVLN safety and semantic diagnostics --- .../evaluator/utils/diagnostic_logger.py | 140 +++++- .../vln/habitat_vln_evaluator.py | 447 +++++++++++++++++- .../vln/navigation_state.py | 284 +++++++++++ tests/unit_test/test_diagnostic_logger.py | 59 +++ tests/unit_test/test_navigation_state.py | 109 +++++ 5 files changed, 1019 insertions(+), 20 deletions(-) diff --git a/internnav/evaluator/utils/diagnostic_logger.py b/internnav/evaluator/utils/diagnostic_logger.py index ddedbeaa..49384ab0 100644 --- a/internnav/evaluator/utils/diagnostic_logger.py +++ b/internnav/evaluator/utils/diagnostic_logger.py @@ -199,6 +199,8 @@ def start_episode(self, episode_metadata: Dict[str, Any]) -> Path: (self.episode_dir / "depth").mkdir(exist_ok=True) (self.episode_dir / "plans").mkdir(exist_ok=True) (self.episode_dir / "s2_decisions").mkdir(exist_ok=True) + (self.episode_dir / "s2_raw").mkdir(exist_ok=True) + (self.episode_dir / "semantic_shadow").mkdir(exist_ok=True) self.timeline_path = self.episode_dir / "timeline.jsonl" self.timeline_path.write_text("", encoding="utf-8") self._write_json(self.episode_dir / "episode.json", episode_metadata) @@ -218,6 +220,9 @@ def save_s2_decision( current_subtask: Optional[str] = None, depth_m: Any = None, model_image_size: Optional[Iterable[int]] = None, + generation_confidence: Optional[float] = None, + confidence_threshold: Optional[float] = None, + confidence_gate: Optional[str] = None, ) -> Dict[str, Any]: """Save the exact current S2 image with a diagnostic-only decision overlay.""" if self.episode_dir is None: @@ -226,14 +231,29 @@ def save_s2_decision( decision_image = image.convert("RGB") else: decision_image = Image.fromarray(np.asarray(image, dtype=np.uint8)).convert("RGB") + coordinate_image_size = decision_image.size if model_image_size is not None: model_size = tuple(int(value) for value in model_image_size) if decision_image.size != model_size: decision_image = decision_image.resize(model_size) + # Keep a clean copy for semantic annotation and later LoRA training. The + # diagnostic decision image below intentionally contains text and the + # model's selected point; training on that overlay would leak the answer. + raw_file_path = self.episode_dir / "s2_raw" / f"raw_{int(s2_call_id):04d}.jpg" + decision_image.save(raw_file_path, format="JPEG", quality=95, optimize=True) + point = [int(value) for value in pixel_goal] if pixel_goal is not None else None + # Evaluator pixel goals use NumPy indexing order [row, col] so that + # depth[row, col] is direct. PIL drawing uses Cartesian (x, y). + display_source_point = [point[1], point[0]] if point is not None else None + display_point = ( + project_pixel_point(display_source_point, coordinate_image_size, decision_image.size) + if display_source_point is not None + else None + ) point_depth_m = ( - depth_at_pixel(depth_m, point, decision_image.size) + depth_at_pixel(depth_m, display_source_point, coordinate_image_size) if point is not None and depth_m is not None else None ) @@ -242,26 +262,47 @@ def save_s2_decision( safe_subtask = str(current_subtask or "unknown").encode("ascii", errors="replace").decode("ascii") draw = ImageDraw.Draw(decision_image, "RGBA") - draw.rectangle((0, 0, decision_image.width, 58), fill=(0, 0, 0, 190)) + draw.rectangle((0, 0, decision_image.width, 75), fill=(0, 0, 0, 190)) draw.text( (8, 7), f"S2 #{int(s2_call_id)} step {int(decision_step)} {str(output_type).upper()}", fill=(255, 255, 255, 255), ) draw.text((8, 24), f"output: {safe_output[:62]}", fill=(255, 255, 0, 255)) - draw.text((8, 41), f"subtask: {safe_subtask[:60]}", fill=(210, 220, 230, 255)) + confidence_text = ( + "unknown" + if generation_confidence is None + else f"{float(generation_confidence):.4f}" + ) + threshold_text = ( + "off" if confidence_threshold is None else f"{float(confidence_threshold):.4f}" + ) + draw.text( + (8, 41), + f"confidence: {confidence_text} / {threshold_text} {str(confidence_gate or '')[:28]}", + fill=(255, 180, 80, 255), + ) + draw.text((8, 58), f"subtask: {safe_subtask[:60]}", fill=(210, 220, 230, 255)) if point is not None: - x, y = point - colour = (0, 255, 255, 255) + x, y = display_point + source_x, source_y = display_source_point + rejected = "rejected" in str(output_type) + colour = (255, 90, 60, 255) if rejected else (0, 255, 255, 255) draw.ellipse((x - 9, y - 9, x + 9, y + 9), outline=(0, 0, 0, 255), width=5) draw.ellipse((x - 8, y - 8, x + 8, y + 8), outline=colour, width=3) draw.line((x - 14, y, x + 14, y), fill=colour, width=2) draw.line((x, y - 14, x, y + 14), fill=colour, width=2) depth_label = "invalid" if point_depth_m is None else f"{point_depth_m:.2f} m" draw.rectangle((x + 12, y - 12, x + 116, y + 10), fill=(0, 0, 0, 190)) - draw.text((x + 16, y - 9), f"GOAL {x},{y} {depth_label}", fill=colour) + point_label = "REJECT" if rejected else "GOAL" + draw.text( + (x + 16, y - 9), + f"{point_label} {source_x},{source_y} {depth_label}", + fill=colour, + ) elif "STOP" in action_names or str(output_type).startswith("stop"): + rejected = "rejected" in str(output_type) draw.rectangle( ( decision_image.width // 2 - 72, @@ -269,13 +310,13 @@ def save_s2_decision( decision_image.width // 2 + 72, decision_image.height // 2 + 27, ), - fill=(160, 0, 0, 210), - outline=(255, 80, 80, 255), + fill=((130, 75, 0, 210) if rejected else (160, 0, 0, 210)), + outline=((255, 190, 60, 255) if rejected else (255, 80, 80, 255)), width=3, ) draw.text( (decision_image.width // 2 - 22, decision_image.height // 2 - 6), - "STOP", + "STOP?" if rejected else "STOP", fill=(255, 255, 255, 255), ) elif action_names: @@ -291,12 +332,93 @@ def save_s2_decision( file_path = self.episode_dir / "s2_decisions" / f"decision_{int(s2_call_id):04d}.jpg" decision_image.save(file_path, format="JPEG", quality=90, optimize=True) return { + "decision_raw_image": str(raw_file_path.relative_to(self.root_dir)), + "decision_raw_image_size": list(decision_image.size), "decision_image": str(file_path.relative_to(self.root_dir)), "decision_image_size": list(decision_image.size), "decision_point": point, + "decision_display_point": list(display_point) if display_point is not None else None, + "decision_coordinate_image_size": list(coordinate_image_size), "decision_point_depth_m": point_depth_m, "decision_action_names": action_names, "decision_current_subtask": current_subtask, + "decision_generation_confidence": generation_confidence, + "decision_confidence_threshold": confidence_threshold, + "decision_confidence_gate": confidence_gate, + } + + def save_semantic_shadow( + self, + s2_call_id: int, + image: Any, + *, + decision_step: int, + anchor: str, + anchor_point: Optional[Iterable[int]], + goal_point: Optional[Iterable[int]], + uncertain: Optional[bool], + progress_step: Optional[int], + stop_complete: Optional[bool], + reason: str, + raw_output: str, + generation_confidence: Optional[float] = None, + coordinate_image_size: Iterable[int] = (640, 480), + ) -> Dict[str, Any]: + """Save a diagnostic-only anchor/goal overlay without controlling navigation.""" + if self.episode_dir is None: + raise RuntimeError("start_episode must be called before saving semantic shadow") + if isinstance(image, Image.Image): + shadow_image = image.convert("RGB") + else: + shadow_image = Image.fromarray(np.asarray(image, dtype=np.uint8)).convert("RGB") + source_size = tuple(int(value) for value in coordinate_image_size) + anchor_xy = [int(value) for value in anchor_point] if anchor_point is not None else None + goal_xy = [int(value) for value in goal_point] if goal_point is not None else None + anchor_display = ( + project_pixel_point(anchor_xy, source_size, shadow_image.size) + if anchor_xy is not None + else None + ) + goal_display = ( + project_pixel_point(goal_xy, source_size, shadow_image.size) + if goal_xy is not None + else None + ) + + draw = ImageDraw.Draw(shadow_image, "RGBA") + draw.rectangle((0, 0, shadow_image.width, 92), fill=(0, 0, 0, 200)) + status = "UNCERTAIN" if uncertain is True else "CERTAIN" if uncertain is False else "UNKNOWN" + confidence = "unknown" if generation_confidence is None else f"{float(generation_confidence):.3f}" + safe_anchor = str(anchor or "unknown").encode("ascii", errors="replace").decode("ascii") + safe_reason = str(reason or "").encode("ascii", errors="replace").decode("ascii") + draw.text((8, 7), f"SHADOW S2 #{int(s2_call_id)} step {int(decision_step)} {status}", fill=(255, 255, 255, 255)) + draw.text((8, 25), f"anchor: {safe_anchor[:48]}", fill=(255, 90, 220, 255)) + draw.text((8, 43), f"progress: {progress_step} stop_complete: {stop_complete} conf: {confidence}", fill=(255, 210, 80, 255)) + draw.text((8, 61), f"reason: {safe_reason[:62]}", fill=(210, 220, 230, 255)) + draw.text((8, 78), "diagnostic only - not sent to navigation controller", fill=(140, 220, 255, 255)) + + def marker(display, source, colour, label): + if display is None: + return + x, y = display + draw.ellipse((x - 9, y - 9, x + 9, y + 9), outline=(0, 0, 0, 255), width=5) + draw.ellipse((x - 8, y - 8, x + 8, y + 8), outline=colour, width=3) + draw.line((x - 13, y, x + 13, y), fill=colour, width=2) + draw.line((x, y - 13, x, y + 13), fill=colour, width=2) + draw.rectangle((x + 10, y - 12, x + 128, y + 10), fill=(0, 0, 0, 190)) + draw.text((x + 14, y - 9), f"{label} {source[0]},{source[1]}", fill=colour) + + marker(anchor_display, anchor_xy, (255, 60, 220, 255), "ANCHOR") + marker(goal_display, goal_xy, (0, 255, 255, 255), "GOAL") + file_path = self.episode_dir / "semantic_shadow" / f"shadow_{int(s2_call_id):04d}.jpg" + shadow_image.save(file_path, format="JPEG", quality=90, optimize=True) + return { + "shadow_image": str(file_path.relative_to(self.root_dir)), + "shadow_image_size": list(shadow_image.size), + "shadow_coordinate_image_size": list(source_size), + "shadow_anchor_display_point": list(anchor_display) if anchor_display is not None else None, + "shadow_goal_display_point": list(goal_display) if goal_display is not None else None, + "shadow_raw_output": str(raw_output), } def log(self, event_type: str, **fields: Any) -> Dict[str, Any]: diff --git a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py index a3e68456..63488b73 100644 --- a/internnav/habitat_extensions/vln/habitat_vln_evaluator.py +++ b/internnav/habitat_extensions/vln/habitat_vln_evaluator.py @@ -54,7 +54,12 @@ DepthObservationSummarizer, InstructionStateTracker, PixelGoalMemory, + S2ConfidenceGate, + SemanticShadowMonitor, bound_actions_at_lookdown, + build_stage_probe_prompt, + estimate_stage_progress, + extract_reference_landmark, load_semantic_labels, select_history_indices, select_uniform_history_indices, @@ -95,6 +100,35 @@ def format_s2_output_for_overlay(output): return normalized.encode('ascii', errors='replace').decode('ascii') +def summarize_generation_confidence(generation, prompt_length): + """Return token-probability diagnostics for one greedy S2 generation.""" + scores = getattr(generation, 'scores', None) + sequences = getattr(generation, 'sequences', None) + if not scores or sequences is None: + return { + 'generation_confidence': None, + 'minimum_token_confidence': None, + 'confidence_token_count': 0, + } + token_ids = sequences[0, int(prompt_length) : int(prompt_length) + len(scores)] + if token_ids.numel() != len(scores): + return { + 'generation_confidence': None, + 'minimum_token_confidence': None, + 'confidence_token_count': 0, + } + token_log_probs = [] + for logits, token_id in zip(scores, token_ids): + log_probability = torch.log_softmax(logits[0].float(), dim=-1)[int(token_id)] + token_log_probs.append(log_probability) + stacked = torch.stack(token_log_probs) + return { + 'generation_confidence': float(torch.exp(stacked.mean()).item()), + 'minimum_token_confidence': float(torch.exp(stacked.min()).item()), + 'confidence_token_count': int(stacked.numel()), + } + + class action_code(IntEnum): STOP = 0 FORWARD = 1 @@ -136,6 +170,34 @@ def __init__(self, cfg: EvalCfg): heading_tolerance_deg=float(getattr(args, 's2_duplicate_goal_heading_tolerance_deg', 20.0)), retry_limit=int(getattr(args, 's2_duplicate_goal_retry_limit', 2)), ) + self.s2_confidence_gate = S2ConfidenceGate( + pixel_goal_threshold=float( + getattr(args, 's2_min_pixel_goal_generation_confidence', 0.0) + ), + stop_threshold=float( + getattr(args, 's2_min_stop_generation_confidence', 0.0) + ), + ) + self.semantic_shadow_enabled = bool(getattr(args, 's2_semantic_shadow_mode', False)) + self.semantic_shadow_monitor = SemanticShadowMonitor( + generation_threshold=float( + getattr(args, 's2_shadow_generation_confidence_threshold', 0.55) + ), + minimum_token_threshold=float( + getattr(args, 's2_shadow_minimum_token_confidence_threshold', 0.20) + ), + query_interval=int(getattr(args, 's2_shadow_query_interval', 3)), + max_queries=int(getattr(args, 's2_shadow_max_queries_per_episode', 10)), + ) + # Diagnostic-only experiment: infer progress from chronological images + # without mutating the primary navigation controller. + self.s2_stage_probe_enabled = bool(getattr(args, 's2_stage_probe_mode', False)) + self.s2_stage_probe_history = max( + 1, int(getattr(args, 's2_stage_probe_history_frames', 5)) + ) + self.s2_stage_probe_max_clauses = max( + 1, int(getattr(args, 's2_stage_probe_max_clauses_per_call', 4)) + ) self.trajectory_selector = TrajectorySelector( cluster_min_fraction=float(getattr(args, 's1_cluster_min_fraction', 0.25)), cluster_lateral_gap=float(getattr(args, 's1_cluster_lateral_gap', 0.25)), @@ -246,6 +308,19 @@ def __init__(self, cfg: EvalCfg): 'dataset_id': getattr(args, 'dataset_id', None), 'deterministic_algorithms': True, 'cublas_workspace_config': os.environ.get('CUBLAS_WORKSPACE_CONFIG'), + 's2_min_pixel_goal_generation_confidence': ( + self.s2_confidence_gate.pixel_goal_threshold + ), + 's2_min_stop_generation_confidence': ( + self.s2_confidence_gate.stop_threshold + ), + 's2_semantic_shadow_mode': self.semantic_shadow_enabled, + 's2_shadow_generation_confidence_threshold': ( + self.semantic_shadow_monitor.generation_threshold + ), + 's2_shadow_minimum_token_confidence_threshold': ( + self.semantic_shadow_monitor.minimum_token_threshold + ), }, criteria=self.diagnostic_criteria, ) @@ -480,6 +555,183 @@ def _parse_pixel_goal(output): values = [int(value) for value in re.findall(r"-?\d+", output)] return [values[1], values[0]] if len(values) >= 2 else None + def _run_native_shadow_generation(self, image, prompt): + """Use only the coordinate/turn/STOP vocabulary supported by navigation S2.""" + images = list(image) if isinstance(image, (list, tuple)) else [image] + messages = [ + { + 'role': 'user', + 'content': ( + [{'type': 'image', 'image': item} for item in images] + + [{'type': 'text', 'text': prompt}] + ), + } + ] + text = self.processor.apply_chat_template( + messages, tokenize=False, add_generation_prompt=True + ) + inputs = self.processor(text=[text], images=images, return_tensors='pt').to( + self.model.device + ) + started = time.perf_counter() + with torch.no_grad(): + generation = self.model.generate( + **inputs, + max_new_tokens=112, + do_sample=False, + use_cache=True, + past_key_values=None, + return_dict_in_generate=True, + output_scores=True, + ) + elapsed_ms = (time.perf_counter() - started) * 1000 + output = self.processor.tokenizer.decode( + generation.sequences[0][inputs.input_ids.shape[1] :], + skip_special_tokens=True, + ) + result = summarize_generation_confidence(generation, inputs.input_ids.shape[1]) + result.update({'output': output, 'generate_ms': elapsed_ms}) + return result + + def _run_semantic_shadow_probe( + self, + image, + instruction, + instruction_state, + main_output, + ): + """Run native-vocabulary landmark/progress probes with no action effect.""" + clause = instruction_state.current_clause or instruction + landmark = extract_reference_landmark(clause) + anchor_prompt = ( + "You are an autonomous navigation assistant. Your task is to approach the " + f"{landmark} itself. Do not follow any route beyond the {landmark}. Where should " + "you go next? Output the waypoint's x y coordinates in the 640 by 480 image when " + "the landmark is visible. If it is not visible, output one turn direction." + ) + progress_prompt = ( + "You are an autonomous navigation assistant. Consider only this one navigation " + f"step: '{clause}'. Output STOP only if this step is already complete in the current " + "image. Otherwise output the next waypoint coordinates or one turn direction." + ) + anchor_probe = self._run_native_shadow_generation(image, anchor_prompt) + progress_probe = self._run_native_shadow_generation(image, progress_prompt) + main_is_stop = bool(re.search(r"\bSTOP\b", str(main_output), re.I)) + stop_probe = None + if main_is_stop: + stop_prompt = ( + "You are an autonomous navigation assistant. The full task is: " + f"'{instruction}'. Inspect the current image without relying on the previous " + "decision. Output STOP only if the entire task is complete. Otherwise output " + "the next waypoint coordinates or one turn direction." + ) + stop_probe = self._run_native_shadow_generation(image, stop_prompt) + + anchor_row_col = self._parse_pixel_goal(anchor_probe['output']) + goal_row_col = self._parse_pixel_goal(main_output) + anchor_xy = ( + [int(anchor_row_col[1]), int(anchor_row_col[0])] + if anchor_row_col is not None + else None + ) + goal_xy = ( + [int(goal_row_col[1]), int(goal_row_col[0])] + if goal_row_col is not None + else None + ) + progress_complete = bool( + re.search(r"\bSTOP\b", str(progress_probe['output']), re.I) + ) + stop_complete = ( + bool(re.search(r"\bSTOP\b", str(stop_probe['output']), re.I)) + if stop_probe is not None + else None + ) + confidences = [ + probe.get('generation_confidence') + for probe in (anchor_probe, progress_probe, stop_probe) + if probe is not None and probe.get('generation_confidence') is not None + ] + minimum_confidences = [ + probe.get('minimum_token_confidence') + for probe in (anchor_probe, progress_probe, stop_probe) + if probe is not None and probe.get('minimum_token_confidence') is not None + ] + raw_payload = { + 'anchor_output': anchor_probe['output'], + 'progress_output': progress_probe['output'], + 'stop_output': stop_probe['output'] if stop_probe is not None else None, + } + return { + 'uncertain': anchor_xy is None, + 'anchor': landmark, + 'anchor_point': anchor_xy, + 'goal_point': goal_xy, + 'progress_step': int(instruction_state.current_index + int(progress_complete)), + 'stop_complete': stop_complete, + 'reason': ( + f"anchor_localized={anchor_xy is not None}; " + f"progress_complete={progress_complete}; native_vocabulary_probe" + ), + 'raw_output': json.dumps(raw_payload, ensure_ascii=False), + 'anchor_output': anchor_probe['output'], + 'progress_output': progress_probe['output'], + 'stop_output': stop_probe['output'] if stop_probe is not None else None, + 'anchor_localized': anchor_xy is not None, + 'progress_complete': progress_complete, + 'generation_confidence': ( + float(np.mean(confidences)) if confidences else None + ), + 'minimum_token_confidence': ( + float(min(minimum_confidences)) if minimum_confidences else None + ), + 'confidence_token_count': int( + sum( + int(probe.get('confidence_token_count') or 0) + for probe in (anchor_probe, progress_probe, stop_probe) + if probe is not None + ) + ), + 'generate_ms': float( + sum( + float(probe.get('generate_ms') or 0.0) + for probe in (anchor_probe, progress_probe, stop_probe) + if probe is not None + ) + ), + } + + def _run_stage_progress_probe(self, images, instruction, instruction_state): + """Estimate an ordered completion prefix; never update controller state.""" + clauses = tuple(instruction_state.clauses) + controller_index = int(instruction_state.current_index) + outputs = [] + confidences = [] + stop = min(len(clauses), controller_index + self.s2_stage_probe_max_clauses) + for clause_index in range(controller_index, stop): + prompt = build_stage_probe_prompt( + instruction, + clauses, + clause_index, + image_count=len(images), + ) + result = self._run_native_shadow_generation(images, prompt) + outputs.append(result['output']) + confidences.append(result.get('generation_confidence')) + estimate = estimate_stage_progress(controller_index, outputs, len(clauses)) + if estimate.status in {'INCOMPLETE', 'UNCERTAIN'}: + break + estimate = estimate_stage_progress(controller_index, outputs, len(clauses)) + return { + 'controller_index': estimate.controller_index, + 'inferred_index': estimate.inferred_index, + 'status': estimate.status, + 'clause_results': list(estimate.clause_results), + 'raw_outputs': outputs, + 'generation_confidences': confidences, + 'diagnostic_only': True, + } + def _diagnostic_env_step(self, action, observations_before, decision_step, phase, **fields): """Execute one Habitat action and log both model and camera-only steps.""" metrics_before = self.env.get_metrics() @@ -559,6 +811,8 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 self._diagnostic_env_step_id = 0 self.recovery_controller.reset() self.pixel_goal_memory.reset() + self.s2_confidence_gate.reset() + self.semantic_shadow_monitor.reset() initial_compass = np.asarray(observations.get('compass', [0.0])).reshape(-1) instruction_state = InstructionStateTracker( episode_instruction, @@ -783,21 +1037,107 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 s2_call_id += 1 s2_started = time.perf_counter() with torch.no_grad(): - output_ids = self.model.generate( + generation = self.model.generate( **inputs, max_new_tokens=128, do_sample=False, use_cache=True, past_key_values=None, return_dict_in_generate=True, - ).sequences + output_scores=True, + ) + output_ids = generation.sequences s2_generate_ms = (time.perf_counter() - s2_started) * 1000 + confidence_summary = summarize_generation_confidence( + generation, inputs.input_ids.shape[1] + ) + generation_confidence = confidence_summary['generation_confidence'] llm_outputs = self.processor.tokenizer.decode( output_ids[0][inputs.input_ids.shape[1] :], skip_special_tokens=True ) latest_s2_output = llm_outputs print('step_id:', step_id, 'output text:', llm_outputs) + if self.s2_stage_probe_enabled and self.diagnostic_logger is not None: + stage_images = input_images[-self.s2_stage_probe_history :] + stage_probe = self._run_stage_progress_probe( + stage_images, + episode_instruction, + instruction_state, + ) + self.diagnostic_logger.log( + 's2_stage_progress_probe', + s2_call_id=current_s2_call_id, + decision_step=step_id, + image_count=len(stage_images), + instruction_clauses=list(instruction_state.clauses), + **stage_probe, + ) + shadow_decision = self.semantic_shadow_monitor.assess( + current_s2_call_id, + instruction_state.current_clause, + llm_outputs, + generation_confidence, + confidence_summary['minimum_token_confidence'], + ) + shadow_result = None + if self.semantic_shadow_enabled and self.diagnostic_logger is not None: + if shadow_decision.query: + shadow_result = self._run_semantic_shadow_probe( + input_images[-1], + episode_instruction, + instruction_state, + llm_outputs, + ) + shadow_snapshot = self.diagnostic_logger.save_semantic_shadow( + current_s2_call_id, + input_images[-1], + decision_step=step_id, + anchor=shadow_result['anchor'], + anchor_point=shadow_result['anchor_point'], + goal_point=shadow_result['goal_point'], + uncertain=shadow_result['uncertain'], + progress_step=shadow_result['progress_step'], + stop_complete=shadow_result['stop_complete'], + reason=shadow_result['reason'], + raw_output=shadow_result['raw_output'], + generation_confidence=shadow_result['generation_confidence'], + ) + self.diagnostic_logger.log( + 'semantic_shadow_probe', + s2_call_id=current_s2_call_id, + decision_step=step_id, + diagnostic_only=True, + primary_output=llm_outputs, + primary_uncertainty_signal=shadow_decision.uncertainty_signal, + primary_uncertainty_reasons=list(shadow_decision.reasons), + semantic_risk=shadow_decision.semantic_risk, + controller_instruction_state=instruction_state.as_dict(), + **shadow_result, + **shadow_snapshot, + ) + self.diagnostic_logger.log( + 'semantic_shadow_signal', + s2_call_id=current_s2_call_id, + decision_step=step_id, + diagnostic_only=True, + queried=shadow_decision.query, + primary_uncertainty_signal=shadow_decision.uncertainty_signal, + primary_uncertainty_reasons=list(shadow_decision.reasons), + semantic_risk=shadow_decision.semantic_risk, + verifier_uncertain=( + shadow_result['uncertain'] if shadow_result is not None else None + ), + combined_uncertainty_signal=bool( + shadow_decision.uncertainty_signal + or ( + shadow_result is not None + and shadow_result['uncertain'] is True + ) + ), + current_subtask=instruction_state.current_clause, + instruction_state=instruction_state.as_dict(), + ) if action == action_code.LOOKDOWN and camera_pitch.restore_steps: horizontal_observations = observations for _ in range(camera_pitch.restore_steps): @@ -823,6 +1163,15 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 ) parsed_pixel_goal = None if forced_stop else self._parse_pixel_goal(llm_outputs) + low_confidence_pixel_goal = None + pixel_goal_confidence_decision = None + if parsed_pixel_goal is not None: + pixel_goal_confidence_decision = self.s2_confidence_gate.evaluate_pixel_goal( + generation_confidence + ) + if pixel_goal_confidence_decision.reject: + low_confidence_pixel_goal = list(parsed_pixel_goal) + parsed_pixel_goal = None if parsed_pixel_goal is not None: # output pixel goal forward_action = 0 pixel_goal = parsed_pixel_goal @@ -871,6 +1220,11 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 self.model_args.resize_w, self.model_args.resize_h, ), + generation_confidence=generation_confidence, + confidence_threshold=( + self.s2_confidence_gate.pixel_goal_threshold + ), + confidence_gate='accepted', ) self.diagnostic_logger.log( 's2_inference', @@ -898,6 +1252,12 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 duplicate_goal=goal_decision.duplicate, duplicate_goal_failed_count=goal_decision.failed_duplicate_count, duplicate_goal_rejected=goal_decision.reject, + confidence_gate='accepted', + pixel_goal_confidence_threshold=( + self.s2_confidence_gate.pixel_goal_threshold + ), + stop_confidence_threshold=self.s2_confidence_gate.stop_threshold, + **confidence_summary, **decision_snapshot, ) @@ -1052,14 +1412,42 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 print('predicted goal', pixel_goal, flush=True) else: - action_seq = ( - [action_code.STOP] - if forced_stop - else self.parse_actions(llm_outputs) - ) + if low_confidence_pixel_goal is not None: + action_seq = [self.recovery_controller.exploratory_turn()] + recovery_context = ( + f"The pixel goal {low_confidence_pixel_goal} was rejected because its " + f"generation confidence {generation_confidence:.4f} was below " + f"{self.s2_confidence_gate.pixel_goal_threshold:.4f}. Reassess the " + "new observation after one bounded turn before choosing a waypoint." + ) + else: + action_seq = ( + [action_code.STOP] + if forced_stop + else self.parse_actions(llm_outputs) + ) action_seq = bound_actions_at_lookdown( action_seq, lookdown_action=action_code.LOOKDOWN ) + stop_confidence_decision = None + stop_rejected_low_confidence = False + stop_confirmed_low_confidence = False + if action_seq == [action_code.STOP] and not forced_stop: + stop_confidence_decision = self.s2_confidence_gate.evaluate_stop( + generation_confidence + ) + stop_confirmed_low_confidence = stop_confidence_decision.confirmed + if stop_confidence_decision.reject: + action_seq = [self.recovery_controller.exploratory_turn()] + stop_rejected_low_confidence = True + recovery_context = ( + f"STOP was rejected because generation confidence " + f"{generation_confidence:.4f} was below " + f"{self.s2_confidence_gate.stop_threshold:.4f}. Observe from one " + "bounded turn; a consecutive second STOP may confirm termination." + ) + elif low_confidence_pixel_goal is None and not forced_stop: + self.s2_confidence_gate.observe_non_stop() original_direct_actions = [int(item) for item in action_seq] action_seq, direct_action_override = ( self.recovery_controller.filter_direct_actions( @@ -1125,6 +1513,12 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 decision_output_type = ( 'stop_forced' if forced_stop + else 'pixel_goal_rejected_low_confidence' + if low_confidence_pixel_goal is not None + else 'stop_rejected_low_confidence' + if stop_rejected_low_confidence + else 'stop_confirmed_low_confidence' + if stop_confirmed_low_confidence else 'stop_rejected' if stop_rejected else ('stop' if action_seq == [action_code.STOP] else 'direct_actions') @@ -1141,6 +1535,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 decision_step=step_id, output_type=decision_output_type, raw_output=llm_outputs, + pixel_goal=low_confidence_pixel_goal, action_names=decision_action_names, current_subtask=instruction_state.current_clause, depth_m=( @@ -1152,6 +1547,22 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 self.model_args.resize_w, self.model_args.resize_h, ), + generation_confidence=generation_confidence, + confidence_threshold=( + self.s2_confidence_gate.pixel_goal_threshold + if low_confidence_pixel_goal is not None + else self.s2_confidence_gate.stop_threshold + if decision_output_type.startswith('stop') + else None + ), + confidence_gate=( + pixel_goal_confidence_decision.reason + if pixel_goal_confidence_decision is not None + and pixel_goal_confidence_decision.reject + else (stop_confidence_decision.reason or 'accepted') + if stop_confidence_decision is not None + else 'accepted' + ), ) self.diagnostic_logger.log( 's2_inference', @@ -1159,6 +1570,7 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 decision_step=step_id, raw_output=llm_outputs, output_type=decision_output_type, + pixel_goal=low_confidence_pixel_goal, parsed_actions=[int(item) for item in action_seq], history_frame_ids=history_id if action != action_code.LOOKDOWN else [], input_image_count=len(input_images), @@ -1166,6 +1578,19 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 generate_ms=s2_generate_ms, recovery_context=s2_recovery_context, forced_stop=forced_stop, + confidence_gate=( + pixel_goal_confidence_decision.reason + if pixel_goal_confidence_decision is not None + and pixel_goal_confidence_decision.reject + else (stop_confidence_decision.reason or 'accepted') + if stop_confidence_decision is not None + else 'accepted' + ), + pixel_goal_confidence_threshold=( + self.s2_confidence_gate.pixel_goal_threshold + ), + stop_confidence_threshold=self.s2_confidence_gate.stop_threshold, + **confidence_summary, instruction_state=instruction_state.as_dict(), depth_summary=( latest_depth_summary.__dict__ if latest_depth_summary is not None else None @@ -1306,8 +1731,8 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 frame = observations_to_image({'rgb': np.asarray(save_raw_image)}, info) if pixel_goal is not None and flag: display_goal = project_pixel_point( - pixel_goal, - (self.model_args.resize_w, self.model_args.resize_h), + (pixel_goal[1], pixel_goal[0]), + save_raw_image.size, save_raw_image.size, ) cv2.circle( @@ -1355,8 +1780,8 @@ def _run_eval_dual_system(self) -> tuple: # noqa: C901 if pixel_goal is not None: if draw_pixel_goal: display_goal = project_pixel_point( - pixel_goal, - (self.model_args.resize_w, self.model_args.resize_h), + (pixel_goal[1], pixel_goal[0]), + save_raw_image.size, save_raw_image.size, ) cv2.circle( diff --git a/internnav/habitat_extensions/vln/navigation_state.py b/internnav/habitat_extensions/vln/navigation_state.py index 483aa2ac..883a356b 100644 --- a/internnav/habitat_extensions/vln/navigation_state.py +++ b/internnav/habitat_extensions/vln/navigation_state.py @@ -55,6 +55,289 @@ def mark_horizontal(self): self.down_steps = 0 +@dataclass(frozen=True) +class ConfidenceGateDecision: + reject: bool + reason: Optional[str] = None + confirmed: bool = False + + +class S2ConfidenceGate: + """Conservative confidence gates for pixel goals and model STOP outputs. + + Confidence here is generation confidence, not calibrated semantic + correctness. It can reject unusually uncertain outputs, but cannot prove + that a referenced landmark was identified correctly. + """ + + def __init__(self, pixel_goal_threshold=0.35, stop_threshold=0.75): + self.pixel_goal_threshold = max(0.0, float(pixel_goal_threshold)) + self.stop_threshold = max(0.0, float(stop_threshold)) + self.pending_low_confidence_stop = False + + def reset(self): + self.pending_low_confidence_stop = False + + @staticmethod + def _below(confidence, threshold): + return confidence is not None and threshold > 0.0 and float(confidence) < threshold + + def evaluate_pixel_goal(self, confidence): + self.pending_low_confidence_stop = False + if self._below(confidence, self.pixel_goal_threshold): + return ConfidenceGateDecision(True, "low_pixel_goal_generation_confidence") + return ConfidenceGateDecision(False) + + def evaluate_stop(self, confidence): + if not self._below(confidence, self.stop_threshold): + self.pending_low_confidence_stop = False + return ConfidenceGateDecision(False) + if self.pending_low_confidence_stop: + self.pending_low_confidence_stop = False + return ConfidenceGateDecision(False, "repeated_low_confidence_stop", confirmed=True) + self.pending_low_confidence_stop = True + return ConfidenceGateDecision(True, "low_stop_generation_confidence") + + def observe_non_stop(self): + self.pending_low_confidence_stop = False + + +_RELATIONAL_LANGUAGE = re.compile( + r"\b(first|second|left of|right of|opposite|past|after|before|between|" + r"leading into|through|towards?|facing|away from|top of|bottom of)\b", + re.I, +) + + +@dataclass(frozen=True) +class SemanticShadowDecision: + """Diagnostic-only probe decision; it must never change navigation actions.""" + + query: bool + uncertainty_signal: bool + reasons: tuple[str, ...] + semantic_risk: bool + + +class SemanticShadowMonitor: + """Rate-limit semantic probes and expose uncertainty without action gating.""" + + def __init__( + self, + generation_threshold=0.55, + minimum_token_threshold=0.20, + query_interval=3, + max_queries=10, + ): + self.generation_threshold = float(generation_threshold) + self.minimum_token_threshold = float(minimum_token_threshold) + self.query_interval = max(1, int(query_interval)) + self.max_queries = max(0, int(max_queries)) + self.reset() + + def reset(self): + self.query_count = 0 + self.last_query_call = -10**9 + self.last_clause = None + + @staticmethod + def _is_stop(output): + return bool(re.search(r"\bSTOP\b", str(output), re.I)) + + def assess(self, call_id, clause, output, generation_confidence, minimum_token_confidence): + reasons = [] + if ( + generation_confidence is not None + and float(generation_confidence) < self.generation_threshold + ): + reasons.append("low_generation_confidence") + if ( + minimum_token_confidence is not None + and float(minimum_token_confidence) < self.minimum_token_threshold + ): + reasons.append("low_minimum_token_confidence") + + clause_text = str(clause or "") + semantic_risk = bool(_RELATIONAL_LANGUAGE.search(clause_text)) + clause_changed = clause_text != self.last_clause + stop_output = self._is_stop(output) + uncertainty_signal = bool(reasons) + due = int(call_id) - self.last_query_call >= self.query_interval + query = bool( + self.query_count < self.max_queries + and due + and ( + self.query_count == 0 + or clause_changed + or uncertainty_signal + or semantic_risk + or stop_output + ) + ) + if query: + self.query_count += 1 + self.last_query_call = int(call_id) + self.last_clause = clause_text + return SemanticShadowDecision( + query=query, + uncertainty_signal=uncertainty_signal, + reasons=tuple(reasons), + semantic_risk=semantic_risk, + ) + + +@dataclass(frozen=True) +class StageProgressEstimate: + """Diagnostic estimate of how far S2 has progressed through ordered clauses.""" + + controller_index: int + inferred_index: int + status: str + clause_results: tuple[str, ...] + + +def parse_stage_completion_output(output): + """Map S2's native navigation vocabulary to one progress judgement. + + STOP means the queried subtask is definitely complete. RIGHT means it is + definitely not complete, and LEFT is reserved for uncertainty. Keeping + the output vocabulary native avoids assuming that the base S2 can reliably + generate a new JSON schema before any fine-tuning. + """ + text = re.sub(r"\s+", "", str(output or "")).upper() + if re.search(r"\bSTOP\b", text): + return "COMPLETE" + if "→" in text or text in {"RIGHT", "TURNRIGHT"}: + return "INCOMPLETE" + if "←" in text or text in {"LEFT", "TURNLEFT"}: + return "UNCERTAIN" + return "UNCERTAIN" + + +def estimate_stage_progress(controller_index, clause_outputs, clause_count): + """Advance only across a consecutive prefix of definitely complete clauses.""" + start = max(0, min(int(controller_index), int(clause_count))) + inferred = start + parsed = [] + terminal_status = "TASK_COMPLETE" if inferred >= int(clause_count) else "UNCERTAIN" + for output in clause_outputs: + result = parse_stage_completion_output(output) + parsed.append(result) + if result == "COMPLETE" and inferred < int(clause_count): + inferred += 1 + terminal_status = "TASK_COMPLETE" if inferred >= int(clause_count) else "COMPLETE_PREFIX" + continue + terminal_status = result + break + return StageProgressEstimate( + controller_index=start, + inferred_index=inferred, + status=terminal_status, + clause_results=tuple(parsed), + ) + + +def build_stage_probe_prompt(instruction, clauses, clause_index, image_count): + """Build a narrow chronological-image prompt for one subtask completion check.""" + numbered = " ".join(f"[{index + 1}] {clause}" for index, clause in enumerate(clauses)) + target = clauses[int(clause_index)] + return ( + "You are only verifying navigation progress, not choosing the next action. " + f"The full task is: '{instruction}'. Its ordered subtasks are: {numbered}. " + f"Decide whether subtask [{int(clause_index) + 1}] '{target}' was definitely " + f"completed at or before the last of these {int(image_count)} chronological images. " + "Keep every modifier such as color, ordinal, direction, and nearby landmark. " + "A later subtask may be underway even when an external controller reports an older one. " + "Output exactly STOP if definitely completed, one RIGHT arrow if definitely not " + "completed, or one LEFT arrow if the images are insufficient or ambiguous." + ) + + +def _shadow_point(value): + if not isinstance(value, (list, tuple)) or len(value) < 2: + return None + try: + x, y = int(round(float(value[0]))), int(round(float(value[1]))) + except (TypeError, ValueError): + return None + return [x, y] if 0 <= x < 640 and 0 <= y < 480 else None + + +def parse_semantic_shadow_output(output): + """Parse best-effort JSON from a diagnostic S2 semantic probe.""" + text = str(output).strip() + payload = {} + match = re.search(r"\{.*\}", text, re.S) + if match: + try: + decoded = json.loads(match.group(0)) + if isinstance(decoded, dict): + payload = decoded + except json.JSONDecodeError: + payload = {} + + def boolean(name): + value = payload.get(name) + if isinstance(value, bool): + return value + if isinstance(value, str): + lowered = value.strip().lower() + if lowered in {"yes", "true"}: + return True + if lowered in {"no", "false"}: + return False + fallback = re.search(rf"\b{name}\s*:\s*(yes|no|true|false)\b", text, re.I) + return fallback.group(1).lower() in {"yes", "true"} if fallback else None + + def point(name): + parsed = _shadow_point(payload.get(name)) + if parsed is not None: + return parsed + fallback = re.search( + rf"\b{name}\s*:\s*[\[(]?\s*(\d{{1,3}})\s*[, ]\s*(\d{{1,3}})", + text, + re.I, + ) + return _shadow_point(fallback.groups()) if fallback else None + + progress = payload.get("progress_step") + try: + progress = int(progress) if progress is not None else None + except (TypeError, ValueError): + progress = None + return { + "uncertain": boolean("uncertain"), + "anchor": str(payload.get("anchor") or "unknown")[:120], + "anchor_point": point("anchor_point"), + "goal_point": point("goal_point"), + "progress_step": progress, + "stop_complete": boolean("stop_complete"), + "reason": str(payload.get("reason") or "")[:500], + "raw_output": text, + } + + +def extract_reference_landmark(clause): + """Extract a compact landmark phrase for a native-vocabulary shadow probe.""" + text = re.sub(r"\s+", " ", str(clause or "").strip(" .,")) + if not text: + return "visible destination" + patterns = ( + r"\b(?:left|right)\s+of\s+(?:the\s+)?(.+?)(?=\s+(?:and|then|before|after)\b|[,.;]|$)", + r"\bopposite\s+(?:of\s+)?(?:the\s+)?(.+?)(?=\s+(?:and|then|before|after)\b|[,.;]|$)", + r"\baway\s+from\s+(?:the\s+)?(.+?)(?=\s+(?:and|then|before|after)\b|[,.;]|$)", + r"\b(?:past|towards?|facing)\s+(?:the\s+)?(.+?)(?=\s+(?:and|then|before|after)\b|[,.;]|$)", + r"\b(?:through|enter|into|approach)\s+(?:the\s+)?(.+?)(?=\s+(?:that|which|to|and|then|on)\b|[,.;]|$)", + ) + for pattern in patterns: + match = re.search(pattern, text, re.I) + if match: + landmark = match.group(1).strip(" ,.") + if landmark: + return landmark[:120] + return text[:120] + + def bound_actions_at_lookdown(actions, lookdown_action=5): """Make LOOKDOWN an observation boundary, not a queued persistent tilt.""" sequence = [int(action) for action in actions] @@ -520,6 +803,7 @@ def semantic_status(self, metric_success, manual_label=None): def as_dict(self): return { "clauses": list(self.clauses), + "current_index": int(self.current_index), "completed": list(self.completed), "completion_reasons": list(self.completion_reasons), "current_subtask": self.current_clause, diff --git a/tests/unit_test/test_diagnostic_logger.py b/tests/unit_test/test_diagnostic_logger.py index 6e032894..484db661 100644 --- a/tests/unit_test/test_diagnostic_logger.py +++ b/tests/unit_test/test_diagnostic_logger.py @@ -43,14 +43,49 @@ def test_save_s2_decision_draws_goal_and_samples_depth(tmp_path): current_subtask="walk to the doorway", depth_m=depth, model_image_size=(384, 384), + generation_confidence=0.82, + confidence_threshold=0.35, + confidence_gate="accepted", ) output = tmp_path / result["decision_image"] + raw_output = tmp_path / result["decision_raw_image"] assert output.is_file() + assert raw_output.is_file() + assert result["decision_raw_image_size"] == [384, 384] assert result["decision_image_size"] == [384, 384] assert result["decision_point"] == [192, 192] + assert result["decision_display_point"] == [192, 192] + assert result["decision_coordinate_image_size"] == [384, 384] assert abs(result["decision_point_depth_m"] - 2.5) < 1e-6 + assert result["decision_generation_confidence"] == 0.82 + assert result["decision_confidence_threshold"] == 0.35 + assert result["decision_confidence_gate"] == "accepted" assert np.asarray(Image.open(output))[192, 192].tolist() != [20, 30, 40] + assert np.max(np.abs(np.asarray(Image.open(raw_output), dtype=np.int16) - [20, 30, 40])) <= 2 + + +def test_save_s2_decision_scales_original_row_col_coordinates(tmp_path): + logger = diagnostic_logger.DiagnosticLogger(str(tmp_path), {"run_id": "test"}) + logger.start_episode({"scene_id": "scene", "episode_id": 3}) + image = Image.new("RGB", (640, 480), (20, 30, 40)) + depth = np.full((480, 640), 1.75, dtype=np.float32) + + result = logger.save_s2_decision( + 2, + image, + decision_step=8, + output_type="pixel_goal", + raw_output="500 300", + pixel_goal=(300, 500), + depth_m=depth, + model_image_size=(384, 384), + ) + + assert result["decision_point"] == [300, 500] + assert result["decision_display_point"] == [300, 240] + assert result["decision_coordinate_image_size"] == [640, 480] + assert abs(result["decision_point_depth_m"] - 1.75) < 1e-6 def test_save_s2_decision_marks_stop_without_a_goal(tmp_path): @@ -67,3 +102,27 @@ def test_save_s2_decision_marks_stop_without_a_goal(tmp_path): assert (tmp_path / result["decision_image"]).is_file() assert result["decision_point"] is None assert result["decision_action_names"] == ["STOP"] + + +def test_save_semantic_shadow_draws_separate_anchor_and_goal(tmp_path): + logger = diagnostic_logger.DiagnosticLogger(str(tmp_path), {"run_id": "test"}) + logger.start_episode({"scene_id": "scene", "episode_id": 4}) + result = logger.save_semantic_shadow( + 5, + Image.new("RGB", (384, 384), (20, 30, 40)), + decision_step=9, + anchor="stove", + anchor_point=(500, 160), + goal_point=(120, 220), + uncertain=True, + progress_step=1, + stop_complete=False, + reason="multiple appliances", + raw_output="{}", + generation_confidence=0.7, + ) + output = tmp_path / result["shadow_image"] + assert output.is_file() + assert result["shadow_anchor_display_point"] == [300, 128] + assert result["shadow_goal_display_point"] == [72, 176] + assert result["shadow_coordinate_image_size"] == [640, 480] diff --git a/tests/unit_test/test_navigation_state.py b/tests/unit_test/test_navigation_state.py index d852e93f..bfaab4ac 100644 --- a/tests/unit_test/test_navigation_state.py +++ b/tests/unit_test/test_navigation_state.py @@ -22,7 +22,14 @@ def _load_module(name): CameraPitchState = navigation_state.CameraPitchState InstructionStateTracker = navigation_state.InstructionStateTracker PixelGoalMemory = navigation_state.PixelGoalMemory +S2ConfidenceGate = navigation_state.S2ConfidenceGate +SemanticShadowMonitor = navigation_state.SemanticShadowMonitor +build_stage_probe_prompt = navigation_state.build_stage_probe_prompt +estimate_stage_progress = navigation_state.estimate_stage_progress +parse_stage_completion_output = navigation_state.parse_stage_completion_output bound_actions_at_lookdown = navigation_state.bound_actions_at_lookdown +parse_semantic_shadow_output = navigation_state.parse_semantic_shadow_output +extract_reference_landmark = navigation_state.extract_reference_landmark select_history_indices = navigation_state.select_history_indices select_uniform_history_indices = navigation_state.select_uniform_history_indices split_instruction = navigation_state.split_instruction @@ -43,6 +50,42 @@ def test_instruction_split_exposes_turn_stair_and_stop_state(): assert clauses == ("Turn right", "step down two stairs", "stop") +def test_stage_probe_keeps_landmark_modifier_and_all_ordered_clauses(): + instruction = ( + "Walk towards the hallway with a blue painting. " + "Turn left and wait by the entrance of the empty room." + ) + clauses = split_instruction(instruction) + assert clauses == ( + "Walk towards the hallway with a blue painting", + "Turn left", + "wait by the entrance of the empty room", + ) + prompt = build_stage_probe_prompt(instruction, clauses, 0, image_count=5) + assert "hallway with a blue painting" in prompt + assert "entrance of the empty room" in prompt + assert "5 chronological images" in prompt + + +def test_stage_progress_catches_up_only_across_consecutive_complete_prefix(): + estimate = estimate_stage_progress(0, ["STOP", "STOP", "→"], clause_count=3) + assert estimate.inferred_index == 2 + assert estimate.status == "INCOMPLETE" + assert estimate.clause_results == ("COMPLETE", "COMPLETE", "INCOMPLETE") + + uncertain = estimate_stage_progress(0, ["STOP", "←", "STOP"], clause_count=3) + assert uncertain.inferred_index == 1 + assert uncertain.status == "UNCERTAIN" + assert uncertain.clause_results == ("COMPLETE", "UNCERTAIN") + + +def test_stage_probe_native_vocabulary_parser_is_conservative(): + assert parse_stage_completion_output("STOP") == "COMPLETE" + assert parse_stage_completion_output("→→→→") == "INCOMPLETE" + assert parse_stage_completion_output("←←←←") == "UNCERTAIN" + assert parse_stage_completion_output("318 276") == "UNCERTAIN" + + def test_two_step_descent_requires_measured_height_change(): tracker = InstructionStateTracker("Step down two stairs and stop.", initial_height=2.0) assert tracker.stair_mode @@ -206,3 +249,69 @@ def test_lookdown_ends_the_direct_action_queue(): assert bound_actions_at_lookdown([3, 3, 5, 1, 1]) == [3, 3, 5] assert bound_actions_at_lookdown([5, 5, 5]) == [5] assert bound_actions_at_lookdown([1, 2, 3]) == [1, 2, 3] + + +def test_low_confidence_pixel_goal_requires_observation_turn(): + gate = S2ConfidenceGate(pixel_goal_threshold=0.35) + assert gate.evaluate_pixel_goal(0.34).reject + assert not gate.evaluate_pixel_goal(0.35).reject + assert not gate.evaluate_pixel_goal(None).reject + + +def test_low_confidence_stop_requires_two_consecutive_decisions(): + gate = S2ConfidenceGate(stop_threshold=0.75) + first = gate.evaluate_stop(0.40) + second = gate.evaluate_stop(0.45) + assert first.reject and not first.confirmed + assert not second.reject and second.confirmed + + +def test_non_stop_breaks_low_confidence_stop_confirmation(): + gate = S2ConfidenceGate(stop_threshold=0.75) + assert gate.evaluate_stop(0.40).reject + gate.observe_non_stop() + assert gate.evaluate_stop(0.40).reject + assert not gate.evaluate_stop(0.80).reject + + +def test_semantic_shadow_signal_is_diagnostic_and_rate_limited(): + monitor = SemanticShadowMonitor( + generation_threshold=0.55, + minimum_token_threshold=0.20, + query_interval=3, + max_queries=2, + ) + first = monitor.assess(0, "go through the first door", "120 200", 0.8, 0.5) + assert first.query and first.semantic_risk + assert not first.uncertainty_signal + second = monitor.assess(1, "go through the first door", "120 200", 0.4, 0.1) + assert not second.query + assert second.uncertainty_signal + assert set(second.reasons) == { + "low_generation_confidence", + "low_minimum_token_confidence", + } + third = monitor.assess(3, "go through the first door", "STOP", 0.8, 0.5) + assert third.query + + +def test_parse_semantic_shadow_json_keeps_xy_coordinate_order(): + parsed = parse_semantic_shadow_output( + '{"uncertain": true, "anchor": "stove", "anchor_point": [500, 160], ' + '"goal_point": [120, 220], "progress_step": 2, "stop_complete": false, ' + '"reason": "multiple appliances"}' + ) + assert parsed["uncertain"] is True + assert parsed["anchor_point"] == [500, 160] + assert parsed["goal_point"] == [120, 220] + assert parsed["progress_step"] == 2 + assert parsed["stop_complete"] is False + + +def test_extract_reference_landmark_prefers_relation_anchor(): + assert extract_reference_landmark( + "Walk through the entry way to the left of the stove" + ) == "stove" + assert extract_reference_landmark( + "Walk through the arched entry way that leads into the tiled room" + ) == "arched entry way"