107 lines
3.4 KiB
Python
107 lines
3.4 KiB
Python
"""Stereo pair building: time-window matching with filename-key fallback."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from typing import Dict, List, Optional, Tuple
|
|
|
|
from calibrationclasses.feature_json import FeatureRecord
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class StereoPair:
|
|
left: FeatureRecord
|
|
right: FeatureRecord
|
|
delta_sec: float
|
|
method: str # "time_window" | "pair_key"
|
|
|
|
|
|
def _chessboard_compatible(left: FeatureRecord, right: FeatureRecord) -> bool:
|
|
if not left.is_chessboard or not right.is_chessboard:
|
|
return False
|
|
return left.corner_count == right.corner_count
|
|
|
|
|
|
def pair_by_time_window(
|
|
left_records: List[FeatureRecord],
|
|
right_records: List[FeatureRecord],
|
|
window_sec: float,
|
|
) -> List[StereoPair]:
|
|
"""Match each left image to the closest unused right image within window_sec."""
|
|
pairs: List[StereoPair] = []
|
|
used_right: set[int] = set()
|
|
|
|
left_sorted = sorted(
|
|
[r for r in left_records if r.is_chessboard and r.timestamp_sec is not None],
|
|
key=lambda r: r.timestamp_sec,
|
|
)
|
|
right_candidates = [
|
|
(i, r)
|
|
for i, r in enumerate(right_records)
|
|
if r.is_chessboard and r.timestamp_sec is not None
|
|
]
|
|
|
|
for left in left_sorted:
|
|
best_idx = None
|
|
best_dt = None
|
|
for idx, right in right_candidates:
|
|
if idx in used_right:
|
|
continue
|
|
if not _chessboard_compatible(left, right):
|
|
continue
|
|
dt = abs(left.timestamp_sec - right.timestamp_sec)
|
|
if dt <= window_sec and (best_dt is None or dt < best_dt):
|
|
best_idx = idx
|
|
best_dt = dt
|
|
if best_idx is not None:
|
|
used_right.add(best_idx)
|
|
right = right_candidates[best_idx][1]
|
|
pairs.append(StereoPair(left, right, best_dt, "time_window"))
|
|
|
|
return pairs
|
|
|
|
|
|
def pair_by_key(
|
|
left_records: List[FeatureRecord],
|
|
right_records: List[FeatureRecord],
|
|
) -> List[StereoPair]:
|
|
"""Legacy exact pair_key matching (IR scan ids, shared numeric suffix)."""
|
|
right_lookup: Dict[str, FeatureRecord] = {}
|
|
for right in right_records:
|
|
if right.is_chessboard and right.pair_key:
|
|
right_lookup[right.pair_key] = right
|
|
|
|
pairs: List[StereoPair] = []
|
|
used_right: set[str] = set()
|
|
for left in left_records:
|
|
if not left.is_chessboard or not left.pair_key:
|
|
continue
|
|
right = right_lookup.get(left.pair_key)
|
|
if right is None or left.pair_key in used_right:
|
|
continue
|
|
if not _chessboard_compatible(left, right):
|
|
continue
|
|
used_right.add(left.pair_key)
|
|
pairs.append(StereoPair(left, right, 0.0, "pair_key"))
|
|
|
|
return pairs
|
|
|
|
|
|
def build_stereo_pairs(
|
|
left_records: List[FeatureRecord],
|
|
right_records: List[FeatureRecord],
|
|
time_window_sec: float = 0.1,
|
|
) -> List[StereoPair]:
|
|
"""
|
|
Prefer time-window pairs; fill remaining with pair_key matches not already paired.
|
|
"""
|
|
time_pairs = pair_by_time_window(left_records, right_records, time_window_sec)
|
|
paired_left = {p.left.image_path for p in time_pairs}
|
|
paired_right = {p.right.image_path for p in time_pairs}
|
|
|
|
remaining_left = [r for r in left_records if r.image_path not in paired_left]
|
|
remaining_right = [r for r in right_records if r.image_path not in paired_right]
|
|
key_pairs = pair_by_key(remaining_left, remaining_right)
|
|
|
|
return time_pairs + key_pairs
|