fix: various

- Added the `LeastMeanSquareVelocityFilter` to improve tracking velocity estimation using historical 3D poses.
- Updated the `triangulate_one_point_from_multiple_views_linear` and `triangulate_points_from_multiple_views_linear` functions to enhance documentation and ensure proper handling of input parameters.
- Refined the logic in triangulation functions to ensure correct handling of homogeneous coordinates.
- Improved error handling in the `LastDifferenceVelocityFilter` to assert non-negative time deltas, enhancing robustness.

playground.py

@@ -64,6 +64,7 @@ from app.tracking import (
     TrackingID,
     AffinityResult,
     LastDifferenceVelocityFilter,
+    LeastMeanSquareVelocityFilter,
     Tracking,
     TrackingState,
 )
@@ -438,12 +439,12 @@ def triangulate_one_point_from_multiple_views_linear(
 ) -> Float[Array, "3"]:
     """
     Args:
-        proj_matrices: 形状为(N, 3, 4)的投影矩阵序列
-        points: 形状为(N, 2)的点坐标序列
-        confidences: 形状为(N,)的置信度序列，范围[0.0, 1.0]
+        proj_matrices: (N, 3, 4) projection matrices
+        points:       (N, 2) image-coordinates per view
+        confidences:  (N,) optional per-view confidences in [0,1]
 
     Returns:
-        point_3d: 形状为(3,)的三角测量得到的3D点
+        (3,) 3D point
     """
     assert len(proj_matrices) == len(points)
 
@@ -469,7 +470,7 @@ def triangulate_one_point_from_multiple_views_linear(
 
     # replace the Python `if` with a jnp.where
     point_3d_homo = jnp.where(
-        point_3d_homo[3] < 0,  # predicate (scalar bool tracer)
+        point_3d_homo[3] <= 0,  # predicate (scalar bool tracer)
         -point_3d_homo,  # if True
         point_3d_homo,  # if False
     )
@@ -493,14 +494,14 @@ def triangulate_points_from_multiple_views_linear(
         confidences:  (N, P, 1) optional per-view confidences in [0,1]
 
     Returns:
-        (P, 3) 3D point for each of the P tracks
+        (P, 3) 3D point for each of the P
     """
     N, P, _ = points.shape
     assert proj_matrices.shape[0] == N
     if confidences is None:
         conf = jnp.ones((N, P), dtype=jnp.float32)
     else:
-        conf = jnp.sqrt(jnp.clip(confidences, 0.0, 1.0))
+        conf = confidences
 
     # vectorize your one-point routine over P
     vmap_triangulate = jax.vmap(
@@ -578,7 +579,7 @@ def triangulate_one_point_from_multiple_views_linear_time_weighted(
 
     # Ensure homogeneous coordinate is positive
     point_3d_homo = jnp.where(
-        point_3d_homo[3] < 0,
+        point_3d_homo[3] <= 0,
         -point_3d_homo,
         point_3d_homo,
     )
@@ -679,6 +680,8 @@ def group_by_cluster_by_camera(
             eld = r[el.camera.id]
             preserved = max([eld, el], key=lambda x: x.timestamp)
             r[el.camera.id] = preserved
+        else:
+            r[el.camera.id] = el
     return pmap(r)
 
 
@@ -714,7 +717,11 @@ class GlobalTrackingState:
         tracking = Tracking(
             id=next_id,
             state=tracking_state,
-            velocity_filter=LastDifferenceVelocityFilter(kps_3d, latest_timestamp),
+            # velocity_filter=LastDifferenceVelocityFilter(kps_3d, latest_timestamp),
+            velocity_filter=LeastMeanSquareVelocityFilter(
+                historical_3d_poses=[kps_3d],
+                historical_timestamps=[latest_timestamp],
+            ),
         )
         self._trackings[next_id] = tracking
         self._last_id = next_id