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.

app/tracking/__init__.py

@@ -114,6 +114,7 @@ class LastDifferenceVelocityFilter(GenericVelocityFilter):
 
     def predict(self, timestamp: datetime) -> TrackingPrediction:
         delta_t_s = (timestamp - self._last_timestamp).total_seconds()
+        assert delta_t_s >= 0, f"delta_t_s is negative: {delta_t_s}"
         if self._last_velocity is None:
             return TrackingPrediction(
                 velocity=None,
@@ -127,6 +128,7 @@ class LastDifferenceVelocityFilter(GenericVelocityFilter):
 
     def update(self, keypoints: Float[Array, "J 3"], timestamp: datetime) -> None:
         delta_t_s = (timestamp - self._last_timestamp).total_seconds()
+        assert delta_t_s >= 0, f"delta_t_s is negative: {delta_t_s}"
         self._last_velocity = (keypoints - self._last_keypoints) / delta_t_s
         self._last_keypoints = keypoints
         self._last_timestamp = timestamp
@@ -160,8 +162,20 @@ class LeastMeanSquareVelocityFilter(GenericVelocityFilter):
         historical_timestamps: Sequence[datetime],
         max_samples: int = 10,
     ):
-        assert len(historical_3d_poses) == len(historical_timestamps)
+        """
+        Args:
+            historical_3d_poses: sequence of 3D poses, at least one element is required
+            historical_timestamps: sequence of timestamps, whose length is the same as `historical_3d_poses`
+            max_samples: maximum number of samples to keep
+        """
+        assert (N := len(historical_3d_poses)) == len(
+            historical_timestamps
+        ), f"the length of `historical_3d_poses` and `historical_timestamps` must be the same; got {N} and {len(historical_timestamps)}"
+        if N < 1:
+            raise ValueError("at least one historical 3D pose is required")
+
         temp = zip(historical_3d_poses, historical_timestamps)
+        # sorted by timestamp
         temp_sorted = sorted(temp, key=lambda x: x[1])
         self._historical_3d_poses = deque(
             map(lambda x: x[0], temp_sorted), maxlen=max_samples
@@ -187,6 +201,7 @@ class LeastMeanSquareVelocityFilter(GenericVelocityFilter):
         latest_timestamp = self._historical_timestamps[-1]
 
         delta_t_s = (timestamp - latest_timestamp).total_seconds()
+        assert delta_t_s >= 0, f"delta_t_s is negative: {delta_t_s}"
 
         if self._velocity is None:
             return TrackingPrediction(
@@ -228,7 +243,6 @@ class LeastMeanSquareVelocityFilter(GenericVelocityFilter):
         keypoints_reshaped = keypoints.reshape(n_samples, -1)
 
         # Use JAX's lstsq to solve the least squares problem
-        # This is more numerically stable than manually computing pseudoinverse
         coefficients, _, _, _ = jnp.linalg.lstsq(X, keypoints_reshaped, rcond=None)
 
         # Coefficients shape is [2, J*3]