diff --git a/src/pyrecest/tracking/tracklet_graph.py b/src/pyrecest/tracking/tracklet_graph.py index 063b992eca..710fb01b55 100644 --- a/src/pyrecest/tracking/tracklet_graph.py +++ b/src/pyrecest/tracking/tracklet_graph.py @@ -207,9 +207,8 @@ def edge_cost(left: Tracklet, right: Tracklet) -> float: dt = max(gap, 1.0e-9) left_state = left.end_state[state_slice] right_state = right.start_state[state_slice] - distance = float( - np.linalg.norm(np.asarray(right_state) - np.asarray(left_state)) - ) + delta = np.asarray(right_state) - np.asarray(left_state) + distance = float(np.hypot.reduce(np.abs(delta))) speed = distance / dt if max_speed_value is not None and speed > max_speed_value: return float("inf") diff --git a/tests/tracking/test_tracklet_graph.py b/tests/tracking/test_tracklet_graph.py index 57b96398b0..3617daffcc 100644 --- a/tests/tracking/test_tracklet_graph.py +++ b/tests/tracking/test_tracklet_graph.py @@ -51,6 +51,30 @@ def test_k_best_tracklet_paths_prefers_feasible_low_cost_chain() -> None: assert paths[0].length == 2 +def test_constant_velocity_edge_cost_preserves_extreme_finite_distance() -> None: + left = Tracklet( + "left", + 0.0, + 1.0, + np.zeros(2), + np.zeros(2), + ) + right_state = np.full(2, 1.0e308) + right = Tracklet( + "right", + 2.0, + 3.0, + right_state, + right_state, + ) + edge_cost = constant_velocity_edge_cost() + + with np.errstate(over="raise", invalid="raise"): + cost = edge_cost(left, right) + + assert np.isfinite(cost) + assert cost == pytest.approx(np.hypot(1.0e308, 1.0e308)) + def test_duplicate_tracklet_ids_are_rejected() -> None: tracklets = [ _tracklet("dup", 0.0, 1.0, 0.0, 1.0),