Use wrapped pose deltas for DeMemWM dynamic scoring

algorithms/dememwm/df_video.py

@@ -21,6 +21,7 @@ from algorithms.common.metrics import (
 from datasets.video.memory_selection import (
     _event_triggered_anchor_candidates_from_deltas,
     _nearest_unique_event_anchors,
+    _pose_delta_values,
     _select_anchor,
     _select_dynamic_by_policy,
     _select_revisit,
@@ -442,7 +443,8 @@ def _extend_online_event_cache(cache, latents, actions, poses, stop: int) -> Non
         cosine = torch.where(valid_pair, cosine.clamp(-1.0, 1.0), torch.ones_like(cosine))
         d_vis.index_copy_(0, curr_rows - old_stop, 1.0 - cosine)
 
-    d_pose = _online_l2_new_deltas(poses, old_stop, target_stop, device)
+    pose_frames = np.arange(old_stop, target_stop, dtype=np.int64)
+    d_pose = _pose_delta_values(poses, pose_frames, max(0, old_stop - 1), target_stop, device)
     d_act = _online_l2_new_deltas(actions, old_stop, target_stop, device)
     cache["d_vis"] = _append_online_event_values(cache["d_vis"], d_vis, old_stop, target_stop, capacity)
     cache["d_pose"] = _append_online_event_values(cache["d_pose"], d_pose, old_stop, target_stop, capacity)

datasets/video/memory_selection.py

@@ -13,6 +13,8 @@ _FOV_NUM_POINTS = 10000
 _FOV_RADIUS = 30.0
 _FOV_HALF_H = 105.0 / 2.0
 _FOV_HALF_V = 75.0 / 2.0
+_POSE_DISTANCE_SCALE = 30.0
+_ANGLE_DISTANCE_SCALE = 180.0
 
 
 def cfg_get(cfg, key: str, default=None):
@@ -413,6 +415,30 @@ def _frame_l2_deltas(values, frames: np.ndarray, history_start: int, stop: int,
     return deltas
 
 
+def _pose_delta_values(poses, frames: np.ndarray, history_start: int, stop: int, device) -> torch.Tensor:
+    deltas = torch.zeros((len(frames),), device=device, dtype=torch.float32)
+    if poses is None or len(frames) == 0:
+        return deltas
+
+    length = len(poses)
+    value_start = max(0, min(int(history_start), length))
+    value_stop = max(value_start, min(int(stop), length))
+    if value_stop - value_start <= 1:
+        return deltas
+
+    pose_tensor = _as_pose_tensor(poses[value_start:value_stop]).to(device=device, dtype=torch.float32)
+    # Consecutive pose novelty keeps yaw/pitch in degrees modulo 360.
+    spatial = torch.linalg.vector_norm(pose_tensor[1:, :3] - pose_tensor[:-1, :3], dim=-1) / _POSE_DISTANCE_SCALE
+    angular = torch.linalg.vector_norm(_wrap_degrees(pose_tensor[1:, 3:5] - pose_tensor[:-1, 3:5]), dim=-1) / _ANGLE_DISTANCE_SCALE
+    consecutive = spatial + angular
+
+    valid = (frames >= value_start + 1) & (frames < value_stop)
+    if bool(valid.any()):
+        rows = torch.as_tensor(frames[valid] - (value_start + 1), device=device, dtype=torch.long)
+        deltas[torch.as_tensor(valid, device=device, dtype=torch.bool)] = consecutive.index_select(0, rows)
+    return deltas
+
+
 def _robust_z(values: torch.Tensor, eps: float = 1e-6) -> torch.Tensor:
     if values.numel() == 0:
         return values
@@ -534,7 +560,7 @@ def _event_triggered_anchor_candidates(
         rows = torch.as_tensor(frames[valid_vis] - (history_start + 1), device=vectors.device, dtype=torch.long)
         d_vis[torch.as_tensor(valid_vis, device=vectors.device, dtype=torch.bool)] = consecutive_vis.index_select(0, rows)
 
-    d_pose = _frame_l2_deltas(poses, frames, history_start, stop, vectors.device)
+    d_pose = _pose_delta_values(poses, frames, history_start, stop, vectors.device)
     d_act = _frame_l2_deltas(actions, frames, history_start, stop, vectors.device)
     return _event_triggered_anchor_candidates_from_deltas(frames, d_vis, d_pose, d_act, cfg)