new

.gitignore

@@ -10,3 +10,5 @@ wheels/
 .venv
 .hypothesis
 samples
+*.jpg
+*.parquet

app/camera/__init__.py

@@ -1,6 +1,7 @@
 from collections import OrderedDict, defaultdict
 from dataclasses import dataclass
 from datetime import datetime
+import string
 from typing import Any, TypeAlias, TypedDict, Optional, Sequence
 
 from beartype import beartype
@@ -522,10 +523,14 @@ def to_homogeneous(points: Num[Array, "N 2"] | Num[Array, "N 3"]) -> Num[Array,
         raise ValueError(f"Invalid shape for points: {points.shape}")
 
 
+import awkward as ak
+
+
 @jaxtyped(typechecker=beartype)
 def point_line_distance(
     points: Num[Array, "N 3"] | Num[Array, "N 2"],
     line: Num[Array, "N 3"],
+    description: str,
     eps: float = 1e-9,
 ):
     """
@@ -544,6 +549,12 @@ def point_line_distance(
     """
     numerator = abs(line[:, 0] * points[:, 0] + line[:, 1] * points[:, 1] + line[:, 2])
     denominator = jnp.sqrt(line[:, 0] * line[:, 0] + line[:, 1] * line[:, 1])
+    # line_data = {"a": line[:, 0], "b": line[:, 1], "c": line[:, 2]}
+    # line_x_y = {"x": points[:, 0], "y": points[:, 1]}
+    # ak.to_parquet(
+    #     line_data, f"/home/admin/Code/CVTH3PE/line_a_b_c_{description}.parquet"
+    # )
+    # ak.to_parquet(line_x_y, f"/home/admin/Code/CVTH3PE/line_x_y_{description}.parquet")
     return numerator / (denominator + eps)
 
 
@@ -571,7 +582,7 @@ def left_to_right_epipolar_distance(
     """
     F_t = fundamental_matrix.transpose()
     line1_in_2 = jnp.matmul(left, F_t)
-    return point_line_distance(right, line1_in_2)
+    return point_line_distance(right, line1_in_2, "left_to_right")
 
 
 @jaxtyped(typechecker=beartype)
@@ -597,7 +608,7 @@ def right_to_left_epipolar_distance(
         $$x^{\\prime T}Fx = 0$$
     """
     line2_in_1 = jnp.matmul(right, fundamental_matrix)
-    return point_line_distance(left, line2_in_1)
+    return point_line_distance(left, line2_in_1, "right_to_left")
 
 
 def distance_between_epipolar_lines(

app/tracking/__init__.py

@@ -1,3 +1,4 @@
+import warnings
 import weakref
 from collections import deque
 from dataclasses import dataclass
@@ -114,13 +115,23 @@ 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 delta_t_s <= 0:
+            warnings.warn(
+                "delta_t={}; last={}; current={}".format(
+                    delta_t_s, self._last_timestamp, timestamp
+                )
+            )
         if self._last_velocity is None:
             return TrackingPrediction(
                 velocity=None,
                 keypoints=self._last_keypoints,
             )
         else:
+            if delta_t_s <= 0:
+                return TrackingPrediction(
+                    velocity=self._last_velocity,
+                    keypoints=self._last_keypoints,
+                )
             return TrackingPrediction(
                 velocity=self._last_velocity,
                 keypoints=self._last_keypoints + self._last_velocity * delta_t_s,
@@ -128,10 +139,12 @@ 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
+        if delta_t_s <= 0:
+            pass
+        else:
+            self._last_timestamp = timestamp
+            self._last_velocity = (keypoints - self._last_keypoints) / delta_t_s
         self._last_keypoints = keypoints
-        self._last_timestamp = timestamp
 
     def get(self) -> TrackingPrediction:
         if self._last_velocity is None:
@@ -162,20 +175,8 @@ class LeastMeanSquareVelocityFilter(GenericVelocityFilter):
         historical_timestamps: Sequence[datetime],
         max_samples: int = 10,
     ):
-        """
-        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")
-
+        assert len(historical_3d_poses) == len(historical_timestamps)
         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
@@ -201,7 +202,6 @@ 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(
@@ -243,6 +243,7 @@ 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]
@@ -559,8 +560,8 @@ class AffinityResult:
     matrix: Float[Array, "T D"]
     trackings: Sequence[Tracking]
     detections: Sequence[Detection]
-    indices_T: Int[Array, "T"]  # pylint: disable=invalid-name
-    indices_D: Int[Array, "D"]  # pylint: disable=invalid-name
+    indices_T: Int[Array, "A"]  # pylint: disable=invalid-name
+    indices_D: Int[Array, "A"]  # pylint: disable=invalid-name
 
     def tracking_association(
         self,

filter_object_by_box.py

@@ -0,0 +1,208 @@
+from narwhals import Boolean
+import numpy as np
+import cv2
+from typing import (
+    TypeAlias,
+    TypedDict,
+)
+from jaxtyping import Array, Num
+from shapely.geometry import Polygon
+from sympy import false, true
+
+NDArray: TypeAlias = np.ndarray
+
+
+# 盒子各个面的三维三角形集合
+box_triangles_list = [
+    ["4", "6", "7"],
+    ["4", "5", "6"],
+    ["2", "5", "6"],
+    ["1", "2", "5"],
+    ["1", "2", "3"],
+    ["0", "1", "3"],
+    ["0", "3", "7"],
+    ["0", "4", "7"],
+    ["2", "6", "7"],
+    ["2", "3", "7"],
+    ["0", "4", "5"],
+    ["0", "1", "5"],
+]
+
+
+class Camera_Params(TypedDict):
+    rvec: Num[NDArray, "3"]
+    tvec: Num[NDArray, "3"]
+    camera_matrix: Num[Array, "3 3"]
+    dist: Num[Array, "N"]
+    width: int
+    height: int
+
+
+class KeypointDataset(TypedDict):
+    frame_index: int
+    boxes: Num[NDArray, "N 4"]
+    kps: Num[NDArray, "N J 2"]
+    kps_scores: Num[NDArray, "N J"]
+
+
+# 三维坐标系根据相机内外参计算该镜头下的二维重投影坐标
+def reprojet_3d_to_2d(point_3d, camera_param):
+    point_2d, _ = cv2.projectPoints(
+        objectPoints=point_3d,
+        rvec=np.array(camera_param.params.Rt[:3, :3]),
+        tvec=np.array(camera_param.params.Rt[:3, 3]),
+        cameraMatrix=np.array(camera_param.params.K),
+        distCoeffs=np.array(camera_param.params.dist_coeffs),
+    )
+
+    point_2d = point_2d.reshape(-1).astype(int)
+    return point_2d
+
+
+# 计算盒子三维坐标系
+def calculaterCubeVersices(position, dimensions):
+    [cx, cy, cz] = position
+    [width, height, depth] = dimensions
+    halfWidth = width / 2
+    halfHeight = height / 2
+    halfDepth = depth / 2
+
+    return [
+        [cx - halfWidth, cy - halfHeight, cz - halfDepth],
+        [cx + halfWidth, cy - halfHeight, cz - halfDepth],
+        [cx + halfWidth, cy + halfHeight, cz - halfDepth],
+        [cx - halfWidth, cy + halfHeight, cz - halfDepth],
+        [cx - halfWidth, cy - halfHeight, cz + halfDepth],
+        [cx + halfWidth, cy - halfHeight, cz + halfDepth],
+        [cx + halfWidth, cy + halfHeight, cz + halfDepth],
+        [cx - halfWidth, cy + halfHeight, cz + halfDepth],
+    ]
+
+
+# 获得盒子三维坐标系
+def calculater_box_3d_points():
+    # 盒子原点位置，相对于六面体中心偏移
+    box_ori_potision = [0.205 + 0.2, 0.205 + 0.50, -0.205 - 0.45]
+    # 盒子边长，宽：1.5米，高：1.5米，深度：1.8米
+    box_geometry = [0.65, 1.8, 1]
+    filter_box_points_3d = calculaterCubeVersices(box_ori_potision, box_geometry)
+    filter_box_points_3d = {
+        str(index): element for index, element in enumerate(filter_box_points_3d)
+    }
+
+    return filter_box_points_3d
+
+
+# 计算盒子坐标系的二维重投影数据
+def calculater_box_2d_points(filter_box_points_3d, camera_param):
+    box_points_2d = dict()
+    for element_index, elment_point_3d in enumerate(filter_box_points_3d.values()):
+        box_points_2d[str(element_index)] = reprojet_3d_to_2d(
+            np.array(elment_point_3d), camera_param
+        ).tolist()
+
+    return box_points_2d
+
+
+# 盒子总的二维平面各三角形坐标点
+def calculater_box_common_scope(box_points_2d):
+    box_triangles_all_points = []
+    # 遍历三角形个数
+    for i in range(len(box_triangles_list)):
+        # 获取单个三角形二维平面坐标点
+        single_triangles = []
+        for element_key in box_triangles_list[i]:
+            single_triangles.append(box_points_2d[element_key])
+        box_triangles_all_points.append(single_triangles)
+
+    return box_triangles_all_points
+
+
+def calculate_triangle_union(triangles):
+    """
+    计算多个三角形的并集区域
+
+    参数:
+    triangles: 包含多个三角形的列表，每个三角形由三个点的坐标组成
+
+    返回:
+    union_area: 并集区域的面积
+    union_polygon: 表示并集区域的多边形对象
+    """
+    # 创建多边形对象列表
+    polygons = [Polygon(tri) for tri in triangles]
+
+    # 计算并集
+    union_polygon = polygons[0]
+    for polygon in polygons[1:]:
+        union_polygon = union_polygon.union(polygon)
+
+    # 计算并集面积
+    union_area = union_polygon.area
+
+    return union_area, union_polygon
+
+
+# 射线法，判断坐标点是否在box二维重投影的区域内
+def point_in_polygon(p, polygon):
+    x, y = p
+    n = len(polygon)
+    intersections = 0
+    on_boundary = False
+
+    for i in range(n):
+        xi, yi = polygon[i]
+        xj, yj = polygon[(i + 1) % n]  # 闭合多边形
+
+        # 检查点是否在顶点上
+        if (x == xi and y == yi) or (x == xj and y == yj):
+            on_boundary = True
+            break
+
+        # 检查点是否在线段上（非顶点情况）
+        if (min(xi, xj) <= x <= max(xi, xj)) and (min(yi, yj) <= y <= max(yi, yj)):
+            cross = (x - xi) * (yj - yi) - (y - yi) * (xj - xi)
+            if cross == 0:
+                on_boundary = True
+                break
+
+        # 计算射线与边的交点（非水平边）
+        if (yi > y) != (yj > y):
+            slope = (xj - xi) / (yj - yi) if (yj - yi) != 0 else float("inf")
+            x_intersect = xi + (y - yi) * slope
+            if x <= x_intersect:
+                intersections += 1
+
+    if on_boundary:
+        return false
+    return intersections % 2 == 1  # 奇数为内部，返回True
+
+
+# 获取并集区域坐标点
+def get_contours(union_polygon):
+    if union_polygon.geom_type == "Polygon":
+        # 单一多边形
+        x, y = union_polygon.exterior.xy
+
+        contours = [(list(x)[i], list(y)[i]) for i in range(len(x))]
+        contours = np.array(contours, np.int32)
+    return contours
+
+
+# 筛选落在盒子二维重投影区域内的关键点信息
+def filter_kps_in_contours(kps, contours) -> Boolean:
+
+    # 4 5 16 17
+    keypoint_index: list[list[int]] = [[4, 5], [16, 17]]
+    centers = []
+    for element_keypoint in keypoint_index:
+        x1, y1 = kps[element_keypoint[0]]
+        x2, y2 = kps[element_keypoint[1]]
+        centers.append([(x1 + x2) / 2, (y1 + y2) / 2])
+
+    if point_in_polygon(centers[0], contours) and point_in_polygon(
+        centers[1], contours
+    ):
+        return true
+    else:
+        return false

optical_detect_result/5606_demo.json

@@ -0,0 +1,282 @@
+[
+    {
+        "kps": [
+            419.0,
+            154.0
+        ],
+        "kps_scores": 1.0,
+        "index": 0
+    },
+    {
+        "kps": [
+            419.0521240234375,
+            154.07498168945312
+        ],
+        "kps_scores": 1.0,
+        "index": 1
+    },
+    {
+        "kps": [
+            418.5992736816406,
+            154.3507080078125
+        ],
+        "kps_scores": 1.0,
+        "index": 2
+    },
+    {
+        "kps": [
+            417.0777893066406,
+            154.17327880859375
+        ],
+        "kps_scores": 1.0,
+        "index": 3
+    },
+    {
+        "kps": [
+            416.8981628417969,
+            154.15330505371094
+        ],
+        "kps_scores": 1.0,
+        "index": 4
+    },
+    {
+        "kps": [
+            415.1317443847656,
+            153.68324279785156
+        ],
+        "kps_scores": 1.0,
+        "index": 5
+    },
+    {
+        "kps": [
+            413.2596130371094,
+            153.39761352539062
+        ],
+        "kps_scores": 1.0,
+        "index": 6
+    },
+    {
+        "kps": [
+            412.7089538574219,
+            153.3645782470703
+        ],
+        "kps_scores": 1.0,
+        "index": 7
+    },
+    {
+        "kps": [
+            409.3253173828125,
+            152.9347686767578
+        ],
+        "kps_scores": 1.0,
+        "index": 8
+    },
+    {
+        "kps": [
+            404.74853515625,
+            152.21153259277344
+        ],
+        "kps_scores": 1.0,
+        "index": 9
+    },
+    {
+        "kps": [
+            404.3977355957031,
+            152.19647216796875
+        ],
+        "kps_scores": 1.0,
+        "index": 10
+    },
+    {
+        "kps": [
+            396.53131103515625,
+            152.09912109375
+        ],
+        "kps_scores": 1.0,
+        "index": 11
+    },
+    {
+        "kps": [
+            393.76605224609375,
+            151.91282653808594
+        ],
+        "kps_scores": 1.0,
+        "index": 12
+    },
+    {
+        "kps": [
+            393.28106689453125,
+            151.76124572753906
+        ],
+        "kps_scores": 1.0,
+        "index": 13
+    },
+    {
+        "kps": [
+            383.2342834472656,
+            152.3790740966797
+        ],
+        "kps_scores": 1.0,
+        "index": 14
+    },
+    {
+        "kps": [
+            379.7545471191406,
+            152.79055786132812
+        ],
+        "kps_scores": 1.0,
+        "index": 15
+    },
+    {
+        "kps": [
+            379.8231506347656,
+            152.8155975341797
+        ],
+        "kps_scores": 1.0,
+        "index": 16
+    },
+    {
+        "kps": [
+            370.0028076171875,
+            155.16213989257812
+        ],
+        "kps_scores": 1.0,
+        "index": 17
+    },
+    {
+        "kps": [
+            366.5267639160156,
+            155.72059631347656
+        ],
+        "kps_scores": 1.0,
+        "index": 18
+    },
+    {
+        "kps": [
+            366.69610595703125,
+            156.3056182861328
+        ],
+        "kps_scores": 1.0,
+        "index": 19
+    },
+    {
+        "kps": [
+            359.8770751953125,
+            158.69798278808594
+        ],
+        "kps_scores": 1.0,
+        "index": 20
+    },
+    {
+        "kps": [
+            356.67681884765625,
+            160.0414581298828
+        ],
+        "kps_scores": 1.0,
+        "index": 21
+    },
+    {
+        "kps": [
+            348.1063232421875,
+            163.32858276367188
+        ],
+        "kps_scores": 1.0,
+        "index": 22
+    },
+    {
+        "kps": [
+            343.6862487792969,
+            165.0043182373047
+        ],
+        "kps_scores": 1.0,
+        "index": 23
+    },
+    {
+        "kps": [
+            339.2411804199219,
+            167.18580627441406
+        ],
+        "kps_scores": 1.0,
+        "index": 24
+    },
+    {
+        "kps": [
+            330.0,
+            170.0
+        ],
+        "kps_scores": 0.0,
+        "index": 25
+    },
+    {
+        "kps": [
+            322.0425720214844,
+            174.9293975830078
+        ],
+        "kps_scores": 1.0,
+        "index": 26
+    },
+    {
+        "kps": [
+            310.0,
+            176.0
+        ],
+        "kps_scores": 0.0,
+        "index": 27
+    },
+    {
+        "kps": [
+            305.0433349609375,
+            178.03123474121094
+        ],
+        "kps_scores": 1.0,
+        "index": 28
+    },
+    {
+        "kps": [
+            293.71295166015625,
+            183.8294219970703
+        ],
+        "kps_scores": 1.0,
+        "index": 29
+    },
+    {
+        "kps": [
+            291.28656005859375,
+            184.33445739746094
+        ],
+        "kps_scores": 1.0,
+        "index": 30
+    },
+    {
+        "kps": [
+            281.0,
+            190.0
+        ],
+        "kps_scores": 0.0,
+        "index": 31
+    },
+    {
+        "kps": [
+            272.0,
+            200.0
+        ],
+        "kps_scores": 0.0,
+        "index": 32
+    },
+    {
+        "kps": [
+            261.0457763671875,
+            211.67132568359375
+        ],
+        "kps_scores": 1.0,
+        "index": 33
+    },
+    {
+        "kps": [
+            239.03567504882812,
+            248.68519592285156
+        ],
+        "kps_scores": 1.0,
+        "index": 34
+    }
+]

optical_detect_result/5608_demo.json

@@ -0,0 +1,282 @@
+[
+    {
+        "kps": [
+            474.0,
+            215.00003051757812
+        ],
+        "kps_scores": 1.0,
+        "index": 0
+    },
+    {
+        "kps": [
+            474.0710754394531,
+            215.04542541503906
+        ],
+        "kps_scores": 1.0,
+        "index": 1
+    },
+    {
+        "kps": [
+            476.81365966796875,
+            215.0387420654297
+        ],
+        "kps_scores": 1.0,
+        "index": 2
+    },
+    {
+        "kps": [
+            479.3288269042969,
+            214.4371795654297
+        ],
+        "kps_scores": 1.0,
+        "index": 3
+    },
+    {
+        "kps": [
+            479.3817443847656,
+            214.49256896972656
+        ],
+        "kps_scores": 1.0,
+        "index": 4
+    },
+    {
+        "kps": [
+            483.0047302246094,
+            213.85231018066406
+        ],
+        "kps_scores": 1.0,
+        "index": 5
+    },
+    {
+        "kps": [
+            484.1208801269531,
+            213.64219665527344
+        ],
+        "kps_scores": 1.0,
+        "index": 6
+    },
+    {
+        "kps": [
+            484.140869140625,
+            213.63470458984375
+        ],
+        "kps_scores": 1.0,
+        "index": 7
+    },
+    {
+        "kps": [
+            487.458251953125,
+            213.45497131347656
+        ],
+        "kps_scores": 1.0,
+        "index": 8
+    },
+    {
+        "kps": [
+            488.8343505859375,
+            213.4651336669922
+        ],
+        "kps_scores": 1.0,
+        "index": 9
+    },
+    {
+        "kps": [
+            488.899658203125,
+            213.48526000976562
+        ],
+        "kps_scores": 1.0,
+        "index": 10
+    },
+    {
+        "kps": [
+            493.831787109375,
+            214.70533752441406
+        ],
+        "kps_scores": 1.0,
+        "index": 11
+    },
+    {
+        "kps": [
+            495.60980224609375,
+            215.26271057128906
+        ],
+        "kps_scores": 1.0,
+        "index": 12
+    },
+    {
+        "kps": [
+            495.5881042480469,
+            215.2436065673828
+        ],
+        "kps_scores": 1.0,
+        "index": 13
+    },
+    {
+        "kps": [
+            502.015380859375,
+            217.81201171875
+        ],
+        "kps_scores": 1.0,
+        "index": 14
+    },
+    {
+        "kps": [
+            504.2356262207031,
+            218.78392028808594
+        ],
+        "kps_scores": 1.0,
+        "index": 15
+    },
+    {
+        "kps": [
+            504.2625427246094,
+            218.81021118164062
+        ],
+        "kps_scores": 1.0,
+        "index": 16
+    },
+    {
+        "kps": [
+            511.97552490234375,
+            222.26150512695312
+        ],
+        "kps_scores": 1.0,
+        "index": 17
+    },
+    {
+        "kps": [
+            514.9180908203125,
+            224.3387908935547
+        ],
+        "kps_scores": 1.0,
+        "index": 18
+    },
+    {
+        "kps": [
+            514.7620239257812,
+            224.2892608642578
+        ],
+        "kps_scores": 1.0,
+        "index": 19
+    },
+    {
+        "kps": [
+            524.9593505859375,
+            230.30003356933594
+        ],
+        "kps_scores": 1.0,
+        "index": 20
+    },
+    {
+        "kps": [
+            528.3402709960938,
+            232.76568603515625
+        ],
+        "kps_scores": 1.0,
+        "index": 21
+    },
+    {
+        "kps": [
+            528.371826171875,
+            232.73399353027344
+        ],
+        "kps_scores": 1.0,
+        "index": 22
+    },
+    {
+        "kps": [
+            538.7906494140625,
+            240.9889678955078
+        ],
+        "kps_scores": 1.0,
+        "index": 23
+    },
+    {
+        "kps": [
+            538.7630004882812,
+            241.00299072265625
+        ],
+        "kps_scores": 1.0,
+        "index": 24
+    },
+    {
+        "kps": [
+            550.0248413085938,
+            248.24708557128906
+        ],
+        "kps_scores": 1.0,
+        "index": 25
+    },
+    {
+        "kps": [
+            554.3512573242188,
+            250.6501922607422
+        ],
+        "kps_scores": 1.0,
+        "index": 26
+    },
+    {
+        "kps": [
+            554.0921020507812,
+            250.47769165039062
+        ],
+        "kps_scores": 1.0,
+        "index": 27
+    },
+    {
+        "kps": [
+            567.93212890625,
+            266.1629943847656
+        ],
+        "kps_scores": 1.0,
+        "index": 28
+    },
+    {
+        "kps": [
+            571.8528442382812,
+            273.5104675292969
+        ],
+        "kps_scores": 1.0,
+        "index": 29
+    },
+    {
+        "kps": [
+            571.9888305664062,
+            273.5711669921875
+        ],
+        "kps_scores": 1.0,
+        "index": 30
+    },
+    {
+        "kps": [
+            586.6533203125,
+            309.09576416015625
+        ],
+        "kps_scores": 1.0,
+        "index": 31
+    },
+    {
+        "kps": [
+            591.8392944335938,
+            325.38385009765625
+        ],
+        "kps_scores": 1.0,
+        "index": 32
+    },
+    {
+        "kps": [
+            592.3212280273438,
+            325.2934265136719
+        ],
+        "kps_scores": 1.0,
+        "index": 33
+    },
+    {
+        "kps": [
+            603.3639526367188,
+            362.4980773925781
+        ],
+        "kps_scores": 1.0,
+        "index": 34
+    }
+]

optical_detect_result/5609_demo.json

@@ -0,0 +1,282 @@
+[
+    {
+        "kps": [
+            461.0,
+            164.0
+        ],
+        "kps_scores": 1.0,
+        "index": 0
+    },
+    {
+        "kps": [
+            460.9234619140625,
+            164.2275390625
+        ],
+        "kps_scores": 1.0,
+        "index": 1
+    },
+    {
+        "kps": [
+            460.93524169921875,
+            164.19480895996094
+        ],
+        "kps_scores": 1.0,
+        "index": 2
+    },
+    {
+        "kps": [
+            460.4592590332031,
+            164.14320373535156
+        ],
+        "kps_scores": 1.0,
+        "index": 3
+    },
+    {
+        "kps": [
+            459.9245910644531,
+            164.054931640625
+        ],
+        "kps_scores": 1.0,
+        "index": 4
+    },
+    {
+        "kps": [
+            459.8656921386719,
+            164.08154296875
+        ],
+        "kps_scores": 1.0,
+        "index": 5
+    },
+    {
+        "kps": [
+            456.9087219238281,
+            163.1707305908203
+        ],
+        "kps_scores": 1.0,
+        "index": 6
+    },
+    {
+        "kps": [
+            455.7566223144531,
+            162.69784545898438
+        ],
+        "kps_scores": 1.0,
+        "index": 7
+    },
+    {
+        "kps": [
+            455.740478515625,
+            162.74818420410156
+        ],
+        "kps_scores": 1.0,
+        "index": 8
+    },
+    {
+        "kps": [
+            449.8667907714844,
+            161.95462036132812
+        ],
+        "kps_scores": 1.0,
+        "index": 9
+    },
+    {
+        "kps": [
+            447.55975341796875,
+            162.12559509277344
+        ],
+        "kps_scores": 1.0,
+        "index": 10
+    },
+    {
+        "kps": [
+            447.5325012207031,
+            162.12460327148438
+        ],
+        "kps_scores": 1.0,
+        "index": 11
+    },
+    {
+        "kps": [
+            439.9998474121094,
+            162.59873962402344
+        ],
+        "kps_scores": 1.0,
+        "index": 12
+    },
+    {
+        "kps": [
+            437.3090515136719,
+            162.88577270507812
+        ],
+        "kps_scores": 1.0,
+        "index": 13
+    },
+    {
+        "kps": [
+            437.2088623046875,
+            162.84994506835938
+        ],
+        "kps_scores": 1.0,
+        "index": 14
+    },
+    {
+        "kps": [
+            429.199951171875,
+            164.5860595703125
+        ],
+        "kps_scores": 1.0,
+        "index": 15
+    },
+    {
+        "kps": [
+            429.32745361328125,
+            164.66001892089844
+        ],
+        "kps_scores": 1.0,
+        "index": 16
+    },
+    {
+        "kps": [
+            424.8293762207031,
+            166.40106201171875
+        ],
+        "kps_scores": 1.0,
+        "index": 17
+    },
+    {
+        "kps": [
+            419.6496887207031,
+            168.80294799804688
+        ],
+        "kps_scores": 1.0,
+        "index": 18
+    },
+    {
+        "kps": [
+            419.6795349121094,
+            168.93418884277344
+        ],
+        "kps_scores": 1.0,
+        "index": 19
+    },
+    {
+        "kps": [
+            414.8919677734375,
+            172.65428161621094
+        ],
+        "kps_scores": 1.0,
+        "index": 20
+    },
+    {
+        "kps": [
+            410.0992431640625,
+            175.77218627929688
+        ],
+        "kps_scores": 1.0,
+        "index": 21
+    },
+    {
+        "kps": [
+            410.0442810058594,
+            175.911376953125
+        ],
+        "kps_scores": 1.0,
+        "index": 22
+    },
+    {
+        "kps": [
+            400.20159912109375,
+            184.33380126953125
+        ],
+        "kps_scores": 1.0,
+        "index": 23
+    },
+    {
+        "kps": [
+            396.4606628417969,
+            186.7172088623047
+        ],
+        "kps_scores": 1.0,
+        "index": 24
+    },
+    {
+        "kps": [
+            396.3185119628906,
+            186.76808166503906
+        ],
+        "kps_scores": 1.0,
+        "index": 25
+    },
+    {
+        "kps": [
+            382.623291015625,
+            192.941650390625
+        ],
+        "kps_scores": 1.0,
+        "index": 26
+    },
+    {
+        "kps": [
+            376.8236999511719,
+            195.2269744873047
+        ],
+        "kps_scores": 1.0,
+        "index": 27
+    },
+    {
+        "kps": [
+            376.66937255859375,
+            195.1109161376953
+        ],
+        "kps_scores": 1.0,
+        "index": 28
+    },
+    {
+        "kps": [
+            362.7231750488281,
+            209.30923461914062
+        ],
+        "kps_scores": 1.0,
+        "index": 29
+    },
+    {
+        "kps": [
+            355.9901123046875,
+            216.26303100585938
+        ],
+        "kps_scores": 1.0,
+        "index": 30
+    },
+    {
+        "kps": [
+            356.3956298828125,
+            216.3310546875
+        ],
+        "kps_scores": 1.0,
+        "index": 31
+    },
+    {
+        "kps": [
+            343.6780090332031,
+            235.2663116455078
+        ],
+        "kps_scores": 1.0,
+        "index": 32
+    },
+    {
+        "kps": [
+            332.50238037109375,
+            261.8990783691406
+        ],
+        "kps_scores": 1.0,
+        "index": 33
+    },
+    {
+        "kps": [
+            332.8721923828125,
+            261.7060546875
+        ],
+        "kps_scores": 1.0,
+        "index": 34
+    }
+]

playground.py

@@ -64,7 +64,6 @@ from app.tracking import (
     TrackingID,
     AffinityResult,
     LastDifferenceVelocityFilter,
-    LeastMeanSquareVelocityFilter,
     Tracking,
     TrackingState,
 )
@@ -439,12 +438,12 @@ def triangulate_one_point_from_multiple_views_linear(
 ) -> Float[Array, "3"]:
     """
     Args:
-        proj_matrices: (N, 3, 4) projection matrices
-        points:       (N, 2) image-coordinates per view
-        confidences:  (N,) optional per-view confidences in [0,1]
+        proj_matrices: 形状为(N, 3, 4)的投影矩阵序列
+        points: 形状为(N, 2)的点坐标序列
+        confidences: 形状为(N,)的置信度序列，范围[0.0, 1.0]
 
     Returns:
-        (3,) 3D point
+        point_3d: 形状为(3,)的三角测量得到的3D点
     """
     assert len(proj_matrices) == len(points)
 
@@ -470,7 +469,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
     )
@@ -494,14 +493,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
+        (P, 3) 3D point for each of the P tracks
     """
     N, P, _ = points.shape
     assert proj_matrices.shape[0] == N
     if confidences is None:
         conf = jnp.ones((N, P), dtype=jnp.float32)
     else:
-        conf = confidences
+        conf = jnp.sqrt(jnp.clip(confidences, 0.0, 1.0))
 
     # vectorize your one-point routine over P
     vmap_triangulate = jax.vmap(
@@ -579,7 +578,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,
     )
@@ -680,8 +679,6 @@ 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)
 
 
@@ -717,11 +714,7 @@ class GlobalTrackingState:
         tracking = Tracking(
             id=next_id,
             state=tracking_state,
-            # velocity_filter=LastDifferenceVelocityFilter(kps_3d, latest_timestamp),
-            velocity_filter=LeastMeanSquareVelocityFilter(
-                historical_3d_poses=[kps_3d],
-                historical_timestamps=[latest_timestamp],
-            ),
+            velocity_filter=LastDifferenceVelocityFilter(kps_3d, latest_timestamp),
         )
         self._trackings[next_id] = tracking
         self._last_id = next_id

pyproject.toml

@@ -14,6 +14,7 @@ dependencies = [
     "jaxtyping>=0.2.38",
     "jupytext>=1.17.0",
     "matplotlib>=3.10.1",
+    "more-itertools>=10.7.0",
     "opencv-python-headless>=4.11.0.86",
     "optax>=0.2.4",
     "orjson>=3.10.15",
@@ -23,6 +24,7 @@ dependencies = [
     "pyrsistent>=0.20.0",
     "pytest>=8.3.5",
     "scipy>=1.15.2",
+    "shapely>=2.1.1",
     "torch>=2.6.0",
     "torchvision>=0.21.0",
     "typeguard>=4.4.2",

smooth_3d_kps.ipynb

@@ -0,0 +1,122 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 16,
+   "id": "0d48b7eb",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import json\n",
+    "from pathlib import Path\n",
+    "import numpy as np"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 17,
+   "id": "dfd27584",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "KPS_PATH = Path(\"samples/WeiHua_03.json\")\n",
+    "with open(KPS_PATH, \"r\") as file:\n",
+    "    data = json.load(file)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 18,
+   "id": "360f9c50",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "'index:1, shape: (33, 133, 3)'"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    },
+    {
+     "data": {
+      "text/plain": [
+       "'index:2, shape: (662, 133, 3)'"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "for item_object_index in data.keys():\n",
+    "    item_object = np.array(data[item_object_index])\n",
+    "    display(f'index:{item_object_index}, shape: {item_object.shape}')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# 对data['2']的662帧3d关键点数据进行滑动窗口平滑处理\n",
+    "object_points = np.array(data['2'])  # shape: (662, 133, 3)\n",
+    "window_size = 5\n",
+    "kernel = np.ones(window_size) / window_size\n",
+    "# 对每个关键点的每个坐标轴分别做滑动平均\n",
+    "smoothed_points = np.zeros_like(object_points)\n",
+    "# 遍历133个关节\n",
+    "for kp_idx in range(object_points.shape[1]):\n",
+    "    # 遍历每个关节的空间三维坐标点\n",
+    "    for axis in range(3):\n",
+    "        # 对第i帧的滑动平滑方式 smoothed[i] = (point[i-2] + point[i-1] + point[i] + point[i+1] + point[i+2]) / 5\n",
+    "        smoothed_points[:, kp_idx, axis] = np.convolve(object_points[:, kp_idx, axis], kernel, mode='same')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 20,
+   "id": "24c6c0c9",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "'smoothed_points shape: (662, 133, 3)'"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "display(f'smoothed_points shape: {smoothed_points.shape}')\n",
+    "with open(\"samples/smoothed_3d_kps.json\", \"w\") as file:\n",
+    "    json.dump({'1':smoothed_points.tolist()}, file)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "cvth3pe",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.12.9"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}

temp_test_camera_ex_params.ipynb

@@ -0,0 +1,193 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "id": "11cc2345",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import awkward as ak\n",
+    "import numpy as np\n",
+    "from pathlib import Path"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 9,
+   "id": "84348d97",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "CAMERA_INDEX ={\n",
+    "    2:\"5602\",\n",
+    "    4:\"5604\",\n",
+    "}\n",
+    "index = 4\n",
+    "CAMERA_PATH = Path(\"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params\")\n",
+    "camera_data = ak.from_parquet(CAMERA_PATH / CAMERA_INDEX[index]/ \"extrinsic.parquet\")"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 10,
+   "id": "1d771740",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/html": [
+       "<pre>[{rvec: [[-2.26], [0.0669], [-2.15]], tvec: [[0.166], ...]},\n",
+       " {rvec: [[2.07], [0.144], [2.21]], tvec: [[0.143], ...]},\n",
+       " {rvec: [[2.09], [0.0872], [2.25]], tvec: [[0.141], ...]},\n",
+       " {rvec: [[2.16], [0.172], [2.09]], tvec: [[0.162], ...]},\n",
+       " {rvec: [[2.15], [0.18], [2.09]], tvec: [[0.162], ...]},\n",
+       " {rvec: [[-2.22], [0.117], [-2.14]], tvec: [[0.162], ...]},\n",
+       " {rvec: [[2.18], [0.176], [2.08]], tvec: [[0.166], ...]},\n",
+       " {rvec: [[2.18], [0.176], [2.08]], tvec: [[0.166], ...]},\n",
+       " {rvec: [[-2.26], [0.116], [-2.1]], tvec: [[0.17], ...]},\n",
+       " {rvec: [[-2.26], [0.124], [-2.09]], tvec: [[0.171], ...]},\n",
+       " ...,\n",
+       " {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
+       " {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
+       " {rvec: [[2.12], [0.151], [2.16]], tvec: [[0.152], ...]},\n",
+       " {rvec: [[-2.3], [0.0733], [-2.1]], tvec: [[0.175], ...]},\n",
+       " {rvec: [[2.1], [0.16], [2.17]], tvec: [[0.149], ...]},\n",
+       " {rvec: [[2.1], [0.191], [2.13]], tvec: [[0.153], ...]},\n",
+       " {rvec: [[2.11], [0.196], [2.12]], tvec: [[0.154], ...]},\n",
+       " {rvec: [[2.19], [0.171], [2.08]], tvec: [[0.166], ...]},\n",
+       " {rvec: [[2.24], [0.0604], [2.12]], tvec: [[0.166], ...]}]\n",
+       "---------------------------------------------------------------------------\n",
+       "backend: cpu\n",
+       "nbytes: 10.1 kB\n",
+       "type: 90 * {\n",
+       "    rvec: var * var * float64,\n",
+       "    tvec: var * var * float64\n",
+       "}</pre>"
+      ],
+      "text/plain": [
+       "<Array [{rvec: [...], tvec: [...]}, ..., {...}] type='90 * {rvec: var * var...'>"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "display(camera_data)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 11,
+   "id": "59fde11b",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "data = []\n",
+    "for element in camera_data:\n",
+    "    rvec = element[\"rvec\"]\n",
+    "    if rvec[0]<0:\n",
+    "        data.append({\"rvec\": rvec, \"tvec\": element[\"tvec\"]})"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 12,
+   "id": "4792cbc4",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "<pyarrow._parquet.FileMetaData object at 0x7799cbf62d40>\n",
+       "  created_by: parquet-cpp-arrow version 19.0.1\n",
+       "  num_columns: 2\n",
+       "  num_rows: 30\n",
+       "  num_row_groups: 1\n",
+       "  format_version: 2.6\n",
+       "  serialized_size: 0"
+      ]
+     },
+     "execution_count": 12,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "ak.to_parquet(ak.from_iter(data),\"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params/5604/re_extrinsic.parquet\")"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 13,
+   "id": "8225ee33",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/html": [
+       "<pre>[{rvec: [[-2.26], [0.0669], [-2.15]], tvec: [[0.166], ...]},\n",
+       " {rvec: [[-2.22], [0.117], [-2.14]], tvec: [[0.162], ...]},\n",
+       " {rvec: [[-2.26], [0.116], [-2.1]], tvec: [[0.17], ...]},\n",
+       " {rvec: [[-2.26], [0.124], [-2.09]], tvec: [[0.171], ...]},\n",
+       " {rvec: [[-2.24], [0.133], [-2.11]], tvec: [[0.167], ...]},\n",
+       " {rvec: [[-2.22], [0.0556], [-2.2]], tvec: [[0.158], ...]},\n",
+       " {rvec: [[-2.27], [0.119], [-2.09]], tvec: [[0.172], ...]},\n",
+       " {rvec: [[-2.34], [0.0663], [-2.06]], tvec: [[0.181], ...]},\n",
+       " {rvec: [[-2.21], [0.117], [-2.15]], tvec: [[0.161], ...]},\n",
+       " {rvec: [[-2.33], [0.0731], [-2.08]], tvec: [[0.179], ...]},\n",
+       " ...,\n",
+       " {rvec: [[-2.23], [0.106], [-2.13]], tvec: [[0.166], ...]},\n",
+       " {rvec: [[-2.21], [0.054], [-2.2]], tvec: [[0.157], ...]},\n",
+       " {rvec: [[-2.19], [0.0169], [-2.25]], tvec: [[0.151], ...]},\n",
+       " {rvec: [[-2.2], [0.0719], [-2.19]], tvec: [[0.157], ...]},\n",
+       " {rvec: [[-2.22], [0.0726], [-2.18]], tvec: [[0.161], ...]},\n",
+       " {rvec: [[-2.2], [0.0742], [-2.19]], tvec: [[0.158], ...]},\n",
+       " {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
+       " {rvec: [[-2.2], [0.0998], [-2.17]], tvec: [[0.158], ...]},\n",
+       " {rvec: [[-2.3], [0.0733], [-2.1]], tvec: [[0.175], ...]}]\n",
+       "---------------------------------------------------------------------------\n",
+       "backend: cpu\n",
+       "nbytes: 3.4 kB\n",
+       "type: 30 * {\n",
+       "    rvec: var * var * float64,\n",
+       "    tvec: var * var * float64\n",
+       "}</pre>"
+      ],
+      "text/plain": [
+       "<Array [{rvec: [...], tvec: [...]}, ..., {...}] type='30 * {rvec: var * var...'>"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "temp_data = ak.from_parquet(\"/home/admin/Documents/ActualTest_QuanCheng/camera_ex_params_1_2025_4_20/camera_params/5604/re_extrinsic.parquet\")\n",
+    "display(temp_data)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": ".venv",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.12.9"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}