"""世界坐标上的轻量跨摄像头全局轨迹融合。""" from functools import lru_cache import math import time def _distance(a, b): return math.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])) def _embedding_similarity(a, b): if a is None or b is None: return None try: import numpy as np av, bv = np.asarray(a, dtype=float), np.asarray(b, dtype=float) denom = float(np.linalg.norm(av) * np.linalg.norm(bv)) return float(np.dot(av, bv) / denom) if denom > 1e-12 else None except Exception: return None def minimum_cost_pairs(costs, max_cost): """小规模精确最小代价分配；目标较多时退化为确定性的全局贪心。""" rows = len(costs) cols = len(costs[0]) if rows else 0 if not rows or not cols: return [] if rows > 12 or cols > 12: candidates = sorted((float(costs[r][c]), r, c) for r in range(rows) for c in range(cols) if float(costs[r][c]) <= max_cost) used_r, used_c, result = set(), set(), [] for cost, r, c in candidates: if r not in used_r and c not in used_c: used_r.add(r); used_c.add(c); result.append((r, c, cost)) return result unmatched = float(max_cost) + 0.001 @lru_cache(None) def solve(row, used_mask): if row >= rows: return 0.0, () best_cost, best_pairs = solve(row + 1, used_mask) best_cost += unmatched for col in range(cols): cost = float(costs[row][col]) if used_mask & (1 << col) or cost > max_cost: continue tail_cost, tail_pairs = solve(row + 1, used_mask | (1 << col)) total = cost + tail_cost if total < best_cost: best_cost = total best_pairs = ((row, col, cost),) + tail_pairs return best_cost, best_pairs return list(solve(0, 0)[1]) def cluster_observations(observations, radius_m, time_window_sec): clusters = [] ordered = sorted(observations or [], key=lambda x: (-float(x.get("score", 0)), int(x.get("camera_id", 0)))) for obs in ordered: best = None for cluster in clusters: if any(int(x["camera_id"]) == int(obs["camera_id"]) for x in cluster): continue if max(abs(float(x["timestamp"]) - float(obs["timestamp"])) for x in cluster) > time_window_sec: continue cx = sum(float(x["x"]) for x in cluster) / len(cluster) cy = sum(float(x["y"]) for x in cluster) / len(cluster) dist = _distance((cx, cy), (obs["x"], obs["y"])) if dist > radius_m: continue similarities = [_embedding_similarity(x.get("embedding"), obs.get("embedding")) for x in cluster] known = [x for x in similarities if x is not None] if known and max(known) < 0.45: continue if best is None or dist < best[0]: best = (dist, cluster) if best: best[1].append(obs) else: clusters.append([obs]) return clusters class GlobalFusionTracker: def __init__(self, radius_m=1.5, time_window_sec=1.0, max_speed_mps=3.0, lost_ttl_sec=30.0, trail_sec=30.0): self.radius_m = float(radius_m) self.time_window_sec = float(time_window_sec) self.max_speed_mps = float(max_speed_mps) self.lost_ttl_sec = float(lost_ttl_sec) self.trail_sec = float(trail_sec) self._tracks = {} self._next_id = 1 @staticmethod def _aggregate(cluster): weights = [] for obs in cluster: calibration_error = max(0.25, float(obs.get("calibration_error_m") or 1.0)) weights.append(max(0.05, float(obs.get("score", 0.5))) / (calibration_error ** 2)) total = sum(weights) or 1.0 x = sum(float(o["x"]) * w for o, w in zip(cluster, weights)) / total y = sum(float(o["y"]) * w for o, w in zip(cluster, weights)) / total latest = max(float(o["timestamp"]) for o in cluster) embeddings = [o.get("embedding") for o in cluster if o.get("embedding") is not None] embedding = None if embeddings: try: import numpy as np embedding = np.mean(np.asarray(embeddings, dtype=float), axis=0) norm = np.linalg.norm(embedding) if norm > 1e-12: embedding = embedding / norm except Exception: embedding = None return { "class": cluster[0].get("class", "person"), "x": x, "y": y, "timestamp": latest, "confidence": sum(float(o.get("score", 0)) for o in cluster) / len(cluster), "source_camera_ids": sorted({int(o["camera_id"]) for o in cluster}), "observations": cluster, "embedding": embedding, } def update(self, observations, now=None): now = float(now if now is not None else time.time()) fresh = [o for o in observations or [] if now - float(o.get("timestamp", 0)) <= self.time_window_sec] candidates = [self._aggregate(c) for c in cluster_observations( fresh, self.radius_m, self.time_window_sec)] existing = list(self._tracks.values()) costs = [] for candidate in candidates: row = [] for track in existing: dt = max(0.0, candidate["timestamp"] - track["timestamp"]) predicted = (track["x"] + track["vx"] * dt, track["y"] + track["vy"] * dt) dist = _distance(predicted, (candidate["x"], candidate["y"])) reachable = self.radius_m + self.max_speed_mps * dt if candidate["class"] != track["class"] or dt < -self.time_window_sec or dist > reachable: row.append(1e9); continue sim = _embedding_similarity(candidate.get("embedding"), track.get("embedding")) if sim is not None and sim < 0.35: row.append(1e9); continue row.append(dist + (0.0 if sim is None else (1.0 - sim) * self.radius_m * 0.5)) costs.append(row) matched_candidates = set() for ci, ti, _cost in minimum_cost_pairs(costs, self.radius_m * 2.0): candidate, track = candidates[ci], existing[ti] dt = max(0.05, candidate["timestamp"] - track["timestamp"]) vx = (candidate["x"] - track["x"]) / dt vy = (candidate["y"] - track["y"]) / dt track["vx"] = track["vx"] * 0.5 + vx * 0.5 track["vy"] = track["vy"] * 0.5 + vy * 0.5 track.update({k: candidate[k] for k in ("x", "y", "timestamp", "confidence", "source_camera_ids", "observations")}) if candidate.get("embedding") is not None: track["embedding"] = candidate["embedding"] track["trail"].append([candidate["timestamp"], candidate["x"], candidate["y"]]) matched_candidates.add(ci) for ci, candidate in enumerate(candidates): if ci in matched_candidates: continue gid = "G%06d" % self._next_id self._next_id += 1 self._tracks[gid] = { "global_id": gid, **candidate, "vx": 0.0, "vy": 0.0, "trail": [[candidate["timestamp"], candidate["x"], candidate["y"]]], } for gid, track in list(self._tracks.items()): cutoff = now - self.trail_sec track["trail"] = [p for p in track["trail"] if p[0] >= cutoff] if now - track["timestamp"] > self.lost_ttl_sec: del self._tracks[gid] return self.snapshot(now) def snapshot(self, now=None): now = float(now if now is not None else time.time()) result = [] for track in self._tracks.values(): age = max(0.0, now - float(track["timestamp"])) item = {k: track[k] for k in ("global_id", "class", "x", "y", "confidence", "timestamp", "source_camera_ids")} item["state"] = "active" if age <= self.time_window_sec else "lost" item["fusion_confidence"] = "high" if len(track["source_camera_ids"]) > 1 else ("medium" if age <= self.time_window_sec else "low") item["trail"] = [[round(p[1], 3), round(p[2], 3)] for p in track["trail"]] item["age_sec"] = age result.append(item) return sorted(result, key=lambda x: x["global_id"])