189 lines
8.4 KiB
Python
189 lines
8.4 KiB
Python
|
"""世界坐标上的轻量跨摄像头全局轨迹融合。"""
|
||
|
|
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"])
|