triangle algo refind

triangle_target.py

@@ -491,9 +491,17 @@ def try_triangle_scoring(
 
     h_orig, w_orig = img_rgb.shape[:2]
 
-    # 缩图加速：嵌入式 CPU 上图像处理耗时与面积成正比，缩到最长边 320px 可获得 ~4× 加速
+    # 缩图加速：嵌入式 CPU 上图像处理耗时与面积成正比。
+    # 不再写死 320/640：默认按相机最长边缩到 1/2（由 config.TRIANGLE_DETECT_SCALE 控制）。
     # 检测完后把像素坐标乘以 inv_scale 还原到原始分辨率，再送入单应性/PnP（与 K 标定分辨率一致）
-    MAX_DETECT_DIM = 640
+    try:
+        import config as _cfg
+        scale = float(getattr(_cfg, "TRIANGLE_DETECT_SCALE", 0.5))
+    except Exception:
+        scale = 0.5
+    if not (0.05 <= scale <= 1.0):
+        scale = 0.5
+    MAX_DETECT_DIM = max(64, int(max(h_orig, w_orig) * scale))
     long_side = max(h_orig, w_orig)
     if long_side > MAX_DETECT_DIM:
         det_scale = MAX_DETECT_DIM / long_side
@@ -564,6 +572,40 @@ def try_triangle_scoring(
         marker_centers = [[float(c[0]), float(c[1])] for c in marker_centers]
         offset_tag = "triangle_homography_3pt"
 
+    # ---------- 结果有效性校验（防 nan/inf 与退化角点） ----------
+    try:
+        import config as _cfg
+        min_center_dist_px = float(getattr(_cfg, "TRIANGLE_MIN_CENTER_DIST_PX", 3.0))
+        max_dist_m = float(getattr(_cfg, "TRIANGLE_MAX_DISTANCE_M", 20.0))
+    except Exception:
+        min_center_dist_px = 3.0
+        max_dist_m = 20.0
+
+    def _all_finite(v) -> bool:
+        try:
+            return bool(np.all(np.isfinite(v)))
+        except Exception:
+            return False
+
+    # 1) 4 个角点中心不能退化/重复（两两距离要大于阈值）
+    try:
+        pts = np.array(marker_centers, dtype=np.float64).reshape(-1, 2)
+        ok_centers = True
+        for i in range(len(pts)):
+            for j in range(i + 1, len(pts)):
+                if float(np.linalg.norm(pts[i] - pts[j])) <= min_center_dist_px:
+                    ok_centers = False
+                    break
+            if not ok_centers:
+                break
+        if not ok_centers:
+            _log(f"[TRI] 角点退化/重复：center_dist <= {min_center_dist_px:.1f}px，判定三角形失败")
+            return out
+    except Exception:
+        # 校验异常时不信任结果，直接回退
+        _log("[TRI] 角点校验异常，判定三角形失败")
+        return out
+
     ok_h, tx, ty, _H = homography_calibration(
         marker_centers, marker_ids, marker_positions, [lx, ly]
     )
@@ -571,6 +613,16 @@ def try_triangle_scoring(
         _log("[TRI] 单应性失败")
         return out
 
+    # 2) 单应性矩阵必须是有限数
+    if (not _all_finite(_H)):
+        _log("[TRI] 单应性出现 nan/inf，判定三角形失败")
+        return out
+
+    # 3) dx/dy 必须是有限数
+    if (not _all_finite([tx, ty])):
+        _log("[TRI] 偏移出现 nan/inf，判定三角形失败")
+        return out
+
     # 与 laser_manager.compute_laser_position 现网约定一致：(x_cm, -y_cm_target)
     out["dx_cm"] = tx
     out["dy_cm"] = -ty
@@ -580,7 +632,8 @@ def try_triangle_scoring(
     out["homography"] = _H         # 供上层反推靶心像素位置用
 
     dist_m = pnp_distance_meters(marker_ids, marker_centers, marker_positions, camera_matrix, dist_coeffs)
-    if dist_m is not None and 0.3 < dist_m < 50.0:
+    # 4) distance_m 若存在也必须是有限数且在合理范围（默认 <20m）
+    if dist_m is not None and _all_finite([dist_m]) and 0.3 < dist_m < max_dist_m:
         out["distance_m"] = dist_m
         out["distance_method"] = "pnp_triangle"
         _log(f"[TRI] PnP 距离={dist_m:.2f}m, 偏移=({out['dx_cm']:.2f},{out['dy_cm']:.2f})cm")