125 lines
3.8 KiB
C#
125 lines
3.8 KiB
C#
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
|
|
}
|