Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/DistanceTransformHelper.cs

@@ -0,0 +1,124 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Euclidean Distance Transform using Felzenszwalb-Huttenlocher algorithm O(n).
+/// Shared implementation used by both MclService and ScanMatchingQualityEvaluator.
+/// Reference: "Distance Transforms of Sampled Functions", Felzenszwalb & Huttenlocher, 2012.
+/// </summary>
+public static class DistanceTransformHelper
+{
+    #region Public API
+
+    /// <summary>
+    /// Compute Euclidean distance (in meters) from each cell to the nearest occupied cell.
+    /// binaryMap[v,u] == 0 → occupied, != 0 → free.
+    /// </summary>
+    public static double[,] ComputeEuclidean(byte[,] binaryMap, int width, int height, double resolution)
+    {
+        // Step 1: Initialize squared distances (0 for occupied, inf for free)
+        const int inf = int.MaxValue / 2;
+        var distSq = new int[height, width];
+        for (int v = 0; v < height; v++)
+            for (int u = 0; u < width; u++)
+                distSq[v, u] = binaryMap[v, u] == 0 ? 0 : inf;
+
+        // Step 2: 1D distance transform along rows (horizontal pass)
+        var tempDist = new int[Math.Max(width, height)];
+        for (int y = 0; y < height; y++)
+        {
+            for (int x = 0; x < width; x++)
+                tempDist[x] = distSq[y, x];
+
+            DistanceTransform1D(tempDist, width);
+
+            for (int x = 0; x < width; x++)
+                distSq[y, x] = tempDist[x];
+        }
+
+        // Step 3: 1D distance transform along columns (vertical pass)
+        for (int x = 0; x < width; x++)
+        {
+            for (int y = 0; y < height; y++)
+                tempDist[y] = distSq[y, x];
+
+            DistanceTransform1D(tempDist, height);
+
+            for (int y = 0; y < height; y++)
+                distSq[y, x] = tempDist[y];
+        }
+
+        // Step 4: Convert squared distance (in pixels) to Euclidean distance (in meters)
+        var result = new double[height, width];
+        for (int v = 0; v < height; v++)
+            for (int u = 0; u < width; u++)
+                result[v, u] = Math.Sqrt(distSq[v, u]) * resolution;
+
+        return result;
+    }
+
+    #endregion
+
+    #region 1D Transform
+
+    /// <summary>
+    /// 1D squared Euclidean distance transform using parabola lower envelope algorithm.
+    /// Operates in-place on the input array.
+    /// </summary>
+    private static void DistanceTransform1D(int[] f, int n)
+    {
+        if (n == 0) return;
+
+        // v stores parabola indices, z stores intersection points
+        var v = new int[n];
+        var z = new double[n + 1];
+        int k = 0; // index of rightmost parabola
+        v[0] = 0;
+        z[0] = double.NegativeInfinity;
+        z[1] = double.PositiveInfinity;
+
+        // Build lower envelope of parabolas
+        for (int q = 1; q < n; q++)
+        {
+            double s;
+            while (true)
+            {
+                int vk = v[k];
+                double fq = f[q];
+                double fvk = f[vk];
+                s = ((fq + q * q) - (fvk + vk * vk)) / (2.0 * (q - vk));
+
+                if (s > z[k])
+                    break;
+
+                k--;
+                if (k < 0)
+                {
+                    k = 0;
+                    break;
+                }
+            }
+
+            k++;
+            v[k] = q;
+            z[k] = s;
+            z[k + 1] = double.PositiveInfinity;
+        }
+
+        // Fill in values of distance transform
+        k = 0;
+        var result = new int[n];
+        for (int q = 0; q < n; q++)
+        {
+            while (z[k + 1] < q)
+                k++;
+            int vk = v[k];
+            int dx = q - vk;
+            result[q] = dx * dx + f[vk];
+        }
+
+        // Copy result back
+        Array.Copy(result, f, n);
+    }
+
+    #endregion
+}