update

2026-06-30 16:12:57 +08:00
parent 14b967f889
commit 306be14a41
3 changed files with 96 additions and 4 deletions
@@ -86,12 +86,25 @@ public class TwoLegDetect : MovementTest
        };
 #pragma warning restore CS0612, CS0618

-        return detector.DetectWithGuess(
+        var result = detector.DetectWithGuess(
            lidarName,
            new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
            guessCoordinateSystem: CoordinateSystem.Car2D,
            outCoordinateSystem: CoordinateSystem.Car2D,
            filters);
+        return ApplyOutputBias(result);
+    }
+
+    private static LineSegment ApplyOutputBias(LineSegment result)
+    {
+        if (result == null) return null;
+
+        var conf = PilotDefinition.Conf;
+        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
+            return result;
+
+        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
+        return new LineSegment(result.Src + bias, result.Dst + bias);
    }

    /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>