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);
+    }
+}

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

@@ -0,0 +1,56 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
+
+#region MclPose2d
+
+/// <summary>
+/// 2D pose (x, y, yaw) for MCL. Matches xloc mcl_2d_localization::Pose.
+/// </summary>
+public struct MclPose2d(double x, double y, double yaw)
+{
+    public double X { get; set; } = x;
+    public double Y { get; set; } = y;
+    public double Yaw { get; set; } = NormalizeYaw(yaw);
+
+    public static double NormalizeYaw(double yaw)
+    {
+        while (yaw < -Math.PI) yaw += 2.0 * Math.PI;
+        while (yaw > Math.PI) yaw -= 2.0 * Math.PI;
+        return yaw;
+    }
+
+    public void SetPose(double x, double y, double yaw)
+    {
+        X = x;
+        Y = y;
+        Yaw = NormalizeYaw(yaw);
+    }
+}
+
+#endregion
+
+#region MclParticle
+
+/// <summary>
+/// Particle (pose + weight) for MCL. Matches xloc mcl_2d_localization::Particle.
+/// </summary>
+public class MclParticle
+{
+    public MclPose2d Pose { get; set; }
+    public double W { get; set; }
+
+    public MclParticle() : this(0, 0, 0, 0) { }
+
+    public MclParticle(double x, double y, double yaw, double w)
+    {
+        Pose = new MclPose2d(x, y, yaw);
+        W = w;
+    }
+
+    public MclParticle(MclPose2d pose, double w)
+    {
+        Pose = pose;
+        W = w;
+    }
+}
+
+#endregion