Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Mcl/MclScanHelper.cs

@@ -0,0 +1,75 @@
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
+
+/// <summary>
+/// Converts point cloud (x,y) to MCL scan format (angle_min, angle_max, angle_increment, range_min, range_max, ranges[]).
+/// Used when feeding TimedPointCloudData to MCL (bin by angle, take min range per bin).
+/// </summary>
+public static class MclScanHelper
+{
+    /// <summary>
+    /// Transforms points from base_link frame to sensor (lidar) frame.
+    /// baseFromLidar: p_base = R(yaw)*p_lidar + (tx, ty). So p_lidar = R(-yaw)*(p_base - (tx, ty)).
+    /// </summary>
+    /// <param name="pointsBase">Points (x, y) in base_link frame</param>
+    /// <param name="tx">X of lidar origin in base_link (baseFromLidar translation)</param>
+    /// <param name="ty">Y of lidar origin in base_link</param>
+    /// <param name="yawRad">Yaw of lidar frame in base_link (radians)</param>
+    /// <returns>Points (x, y) in sensor/lidar frame</returns>
+    public static IEnumerable<(double x, double y)> BaseToSensorFrame(
+        IEnumerable<(double x, double y)> pointsBase,
+        double tx, double ty, double yawRad)
+    {
+        double c = Math.Cos(-yawRad);
+        double s = Math.Sin(-yawRad);
+        foreach (var (xb, yb) in pointsBase)
+        {
+            double dx = xb - tx;
+            double dy = yb - ty;
+            yield return (c * dx - s * dy, s * dx + c * dy);
+        }
+    }
+
+    /// <summary>
+    /// Bins points by angle and returns ranges (min range per bin). Angles in radians; ranges in meters.
+    /// Points should be in sensor (lidar) frame so that angles are relative to sensor (required by MCL likelihood).
+    /// </summary>
+    public static (double angleMin, double angleMax, double angleIncrement, double rangeMin, double rangeMax, double[] ranges) ConvertToScan(
+        IEnumerable<Vector2> points,
+        double angleMinDeg,
+        double angleMaxDeg,
+        int numBins,
+        double rangeMin,
+        double rangeMax)
+    {
+        double angleMin = angleMinDeg * Math.PI / 180.0;
+        double angleMax = angleMaxDeg * Math.PI / 180.0;
+        double angleIncrement = (angleMax - angleMin) / Math.Max(1, numBins);
+        var bins = new List<double>[numBins];
+        for (int i = 0; i < numBins; i++)
+            bins[i] = [];
+
+        foreach (var point in points)
+        {
+            var range = Math.Sqrt(point.X * point.X + point.Y * point.Y);
+            if (range < 1e-6) continue;
+            var angle = Math.Atan2(point.Y, point.X);
+            int bin = (int)Math.Floor((angle - angleMin) / angleIncrement);
+            if (bin < 0) bin = 0;
+            if (bin >= numBins) bin = numBins - 1;
+            bins[bin].Add(range);
+        }
+
+        var ranges = new double[numBins];
+        for (int i = 0; i < numBins; i++)
+        {
+            if (bins[i].Count == 0)
+                ranges[i] = rangeMax;
+            else
+                ranges[i] = bins[i].Min();
+        }
+        return (angleMin, angleMax, angleIncrement, rangeMin, rangeMax, ranges);
+    }
+}