测试添加视觉代码

config.py

@@ -0,0 +1,36 @@
+import numpy as np
+
+# 相机内参（需实际标定）
+CAMERA_MATRIX = np.array([
+    [600, 0, 320],
+    [0, 600, 240],
+    [0, 0, 1]
+], dtype=np.float32)
+DIST_COEFFS = np.zeros(5)  # 畸变系数（无畸变设为0）
+
+# 装甲板尺寸（毫米）
+ARMOR_WIDTH = 135
+ARMOR_HEIGHT = 125
+ARMOR_THICKNESS = 20
+ARMOR_3D_POINTS = np.array([
+    [-ARMOR_WIDTH / 2, -ARMOR_HEIGHT / 2, 0],
+    [ARMOR_WIDTH / 2, -ARMOR_HEIGHT / 2, 0],
+    [ARMOR_WIDTH / 2, ARMOR_HEIGHT / 2, 0],
+    [-ARMOR_WIDTH / 2, ARMOR_HEIGHT / 2, 0]
+], dtype=np.float32)
+ARMOR_BOX_3D_POINTS = np.vstack([
+    ARMOR_3D_POINTS,
+    ARMOR_3D_POINTS + np.array([0, 0, -ARMOR_THICKNESS])
+])
+
+# 检测阈值
+HORIZONTAL_ANGLE_THRESHOLD = 25  # 灯条水平角度阈值（度）
+NEARBY_LIGHT_BAR_THRESHOLD = 500  # 灯条合并距离阈值（像素）
+LIGHT_BAR_IOU_THRESHOLD = 0.05    # 灯条合并IOU阈值
+ARMOR_DISTANCE_RATIO_RANGE = (0.6, 3)  # 装甲板灯条距离比例范围
+ARMOR_LENGTH_DIFF_RATIO = 0.5    # 装甲板灯条长度差异比例
+ARMOR_ANGLE_DIFF_THRESHOLD = np.radians(20)  # 装甲板灯条角度差异阈值（弧度）
+
+# 卡尔曼滤波参数
+KF_PROCESS_NOISE = 0.02    # 过程噪声协方差
+KF_MEASUREMENT_NOISE = 0.5  # 测量噪声协方差