feat: Introduce LastDifferenceVelocityFilter for improved tracking velocity estimation

- Added a new `LastDifferenceVelocityFilter` class to estimate tracking velocities based on the last observed keypoints, enhancing the tracking capabilities.
- Updated the `Tracking` class to utilize the new velocity filter, allowing for more accurate predictions of keypoints over time.
- Refactored the `predict` method to support various input types (float, timedelta, datetime) for better flexibility in time handling.
- Improved timestamp handling in the `perpendicular_distance_camera_2d_points_to_tracking_raycasting` function to ensure adherence to minimum delta time constraints.
- Cleaned up imports and type hints for better organization and clarity across the codebase.

playground.py

@@ -37,7 +37,6 @@ import awkward as ak
 import jax
 import jax.numpy as jnp
 import numpy as np
-import orjson
 from beartype import beartype
 from beartype.typing import Mapping, Sequence
 from cv2 import undistortPoints
@@ -46,7 +45,7 @@ from jaxtyping import Array, Float, Num, jaxtyped
 from matplotlib import pyplot as plt
 from numpy.typing import ArrayLike
 from optax.assignment import hungarian_algorithm as linear_sum_assignment
-from pyrsistent import v, pvector
+from pyrsistent import pvector, v
 from scipy.spatial.transform import Rotation as R
 from typing_extensions import deprecated
 
@@ -59,15 +58,15 @@ from app.camera import (
     classify_by_camera,
 )
 from app.solver._old import GLPKSolver
-from app.tracking import AffinityResult, Tracking
+from app.tracking import AffinityResult, LastDifferenceVelocityFilter, Tracking
 from app.visualize.whole_body import visualize_whole_body
 
 NDArray: TypeAlias = np.ndarray
 
 # %%
 DATASET_PATH = Path("samples") / "04_02"
-AK_CAMERA_DATASET: ak.Array = ak.from_parquet(DATASET_PATH / "camera_params.parquet")
-DELTA_T_MIN = timedelta(milliseconds=10)
+AK_CAMERA_DATASET: ak.Array = ak.from_parquet(DATASET_PATH / "camera_params.parquet")  # type: ignore
+DELTA_T_MIN = timedelta(milliseconds=1)
 display(AK_CAMERA_DATASET)
 
 
@@ -549,6 +548,7 @@ class GlobalTrackingState:
             keypoints=kps_3d,
             last_active_timestamp=latest_timestamp,
             historical_detections=v(*cluster),
+            velocity_filter=LastDifferenceVelocityFilter(kps_3d, latest_timestamp),
         )
         self._trackings[next_id] = tracking
         self._last_id = next_id
@@ -673,9 +673,7 @@ def perpendicular_distance_camera_2d_points_to_tracking_raycasting(
     camera = detection.camera
     # Use the delta_t supplied by the caller, but clamp to DELTA_T_MIN to
     # avoid division-by-zero / exploding affinities.
-    delta_t = max(delta_t, DELTA_T_MIN)
-    delta_t_s = delta_t.total_seconds()
-    predicted_pose = tracking.predict(delta_t_s)
+    predicted_pose = tracking.predict(max(delta_t, DELTA_T_MIN))
 
     # Back-project the 2D points to 3D space
     # intersection with z=0 plane