新增pose模型，注意参数设置

Vision/tool/utils.py

@@ -14,6 +14,59 @@ import psutil
 from psutil._common import bytes2human
 
 
+def shrink_quadrilateral(points, d):
+    """
+    给定4个点围成的四边形，沿着对角线内缩小d个像素
+    :param points: 四边形的4个顶点，形状为 (4, 2)
+    :param d: 内缩的像素距离
+    :return: 缩小后的4个顶点
+    """
+    # 将点转换为 numpy 数组
+    points = np.array(points, dtype=np.float32)
+
+    # 计算四边形的中心点
+    center = np.mean(points, axis=0)
+
+    # 计算每个点到中心点的向量
+    vectors = points - center
+
+    # 计算每个向量的长度
+    lengths = np.linalg.norm(vectors, axis=1)
+
+    # 计算缩放比例
+    scale = (lengths - d) / lengths
+
+    # 对每个点进行缩放
+    new_points = center + vectors * scale[:, np.newaxis]
+    new_points = new_points.astype(np.int32)
+
+    return new_points
+
+
+def find_closest_point_index(point_cloud, x1, y1):
+    x_coords = point_cloud[:, :, 0]
+    y_coords = point_cloud[:, :, 1]
+
+    # 创建一个掩码，标记非 NaN 的点
+    valid_mask = ~(np.isnan(x_coords) & ~np.isnan(y_coords))
+
+    # 初始化最小距离为一个很大的值
+    min_distance = np.inf
+    min_index = (None, None)
+
+    # 遍历所有有效点
+    for i in range(point_cloud.shape[0]):
+        for j in range(point_cloud.shape[1]):
+            if valid_mask[i, j]:
+                # 计算当前点到 (x1, y1) 的欧几里得距离
+                distance = np.sqrt((x_coords[i, j] - x1) ** 2 + (y_coords[i, j] - y1) ** 2)
+                # 如果当前距离小于最小距离，则更新最小距离和索引
+                if distance < min_distance:
+                    min_distance = distance
+                    min_index = (i, j)
+
+    return min_index
+
 def uv_to_XY(cameraType, u, v):
     """
     像素坐标转相机坐标