feat: add DistanceMap

PathPlanning/DistanceMap/distance_map.py

@@ -0,0 +1,151 @@
+"""
+Distance Map
+
+author: Wang Zheng (@Aglargil)
+
+Ref:
+
+- [Distance Map]
+(https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf)
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+INF = 1e20
+ENABLE_PLOT = True
+
+
+def compute_sdf(obstacles):
+    """
+    Compute the signed distance field (SDF) from a boolean field.
+
+    Parameters
+    ----------
+    obstacles : array_like
+        A 2D boolean array where '1' represents obstacles and '0' represents free space.
+
+    Returns
+    -------
+    array_like
+        A 2D array representing the signed distance field, where positive values indicate distance
+        to the nearest obstacle, and negative values indicate distance to the nearest free space.
+    """
+    a = compute_udf(obstacles)
+    b = compute_udf(obstacles == 0)
+    return a - b
+
+
+def compute_udf(obstacles):
+    """
+    Compute the unsigned distance field (UDF) from a boolean field.
+
+    Parameters
+    ----------
+    obstacles : array_like
+        A 2D boolean array where '1' represents obstacles and '0' represents free space.
+
+    Returns
+    -------
+    array_like
+        A 2D array of distances from the nearest obstacle, with the same dimensions as `bool_field`.
+    """
+    edt = obstacles.copy()
+    if not np.all(np.isin(edt, [0, 1])):
+        raise ValueError("Input array should only contain 0 and 1")
+    edt = np.where(edt == 0, INF, edt)
+    edt = np.where(edt == 1, 0, edt)
+    for row in range(len(edt)):
+        dt(edt[row])
+    edt = edt.T
+    for row in range(len(edt)):
+        dt(edt[row])
+    edt = edt.T
+    return np.sqrt(edt)
+
+
+def dt(d):
+    """
+    Compute 1D distance transform under the squared Euclidean distance
+
+    Parameters
+    ----------
+    d : array_like
+        Input array containing the distances.
+
+    Returns:
+    --------
+    d : array_like
+        The transformed array with computed distances.
+    """
+    v = np.zeros(len(d) + 1)
+    z = np.zeros(len(d) + 1)
+    k = 0
+    v[0] = 0
+    z[0] = -INF
+    z[1] = INF
+    for q in range(1, len(d)):
+        s = ((d[q] + q * q) - (d[int(v[k])] + v[k] * v[k])) / (2 * q - 2 * v[k])
+        while s <= z[k]:
+            k = k - 1
+            s = ((d[q] + q * q) - (d[int(v[k])] + v[k] * v[k])) / (2 * q - 2 * v[k])
+        k = k + 1
+        v[k] = q
+        z[k] = s
+        z[k + 1] = INF
+    k = 0
+    for q in range(len(d)):
+        while z[k + 1] < q:
+            k = k + 1
+        dx = q - v[k]
+        d[q] = dx * dx + d[int(v[k])]
+
+
+def main():
+    obstacles = np.array(
+        [
+            [1, 0, 0, 0, 0],
+            [0, 1, 1, 1, 0],
+            [0, 1, 1, 1, 0],
+            [0, 0, 1, 1, 0],
+            [0, 0, 1, 0, 0],
+            [0, 0, 0, 0, 0],
+            [0, 0, 0, 0, 0],
+            [0, 0, 0, 0, 0],
+            [0, 0, 1, 0, 0],
+            [0, 0, 0, 0, 0],
+            [0, 0, 0, 0, 0],
+        ]
+    )
+
+    # Compute the signed distance field
+    sdf = compute_sdf(obstacles)
+    udf = compute_udf(obstacles)
+
+    if ENABLE_PLOT:
+        _, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(15, 5))
+
+        obstacles_plot = ax1.imshow(obstacles, cmap="binary")
+        ax1.set_title("Obstacles")
+        ax1.set_xlabel("x")
+        ax1.set_ylabel("y")
+        plt.colorbar(obstacles_plot, ax=ax1)
+
+        udf_plot = ax2.imshow(udf, cmap="viridis")
+        ax2.set_title("Unsigned Distance Field")
+        ax2.set_xlabel("x")
+        ax2.set_ylabel("y")
+        plt.colorbar(udf_plot, ax=ax2)
+
+        sdf_plot = ax3.imshow(sdf, cmap="RdBu")
+        ax3.set_title("Signed Distance Field")
+        ax3.set_xlabel("x")
+        ax3.set_ylabel("y")
+        plt.colorbar(sdf_plot, ax=ax3)
+
+        plt.tight_layout()
+        plt.show()
+
+
+if __name__ == "__main__":
+    main()

docs/modules/5_path_planning/distance_map/distance_map_main.rst

@@ -0,0 +1,27 @@
+Distance Map
+------------
+
+This is an implementation of the Distance Map algorithm for path planning.
+
+The Distance Map algorithm computes the unsigned distance field (UDF) and signed distance field (SDF) from a boolean field representing obstacles. 
+
+The UDF gives the distance from each point to the nearest obstacle. The SDF gives positive distances for points outside obstacles and negative distances for points inside obstacles.
+
+Example
+~~~~~~~
+
+The algorithm is demonstrated on a simple 2D grid with obstacles:
+
+.. image:: distance_map.png
+
+API
+~~~
+
+.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_sdf
+
+.. autofunction:: PathPlanning.DistanceMap.distance_map.compute_udf
+
+References
+~~~~~~~~~~
+
+- `Distance Transforms of Sampled Functions <https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf>`_ paper by Pedro F. Felzenszwalb and Daniel P. Huttenlocher. 

docs/modules/5_path_planning/path_planning_main.rst

@@ -31,3 +31,4 @@ Path planning is the ability of a robot to search feasible and efficient path to
    hybridastar/hybridastar
    frenet_frame_path/frenet_frame_path
    coverage_path/coverage_path
+   distance_map/distance_map

tests/test_distance_map.py

@@ -0,0 +1,118 @@
+import conftest  # noqa
+import numpy as np
+from PathPlanning.DistanceMap import distance_map as m
+
+
+def test_compute_sdf():
+    """Test the computation of Signed Distance Field (SDF)"""
+    # Create a simple obstacle map for testing
+    obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]])
+
+    sdf = m.compute_sdf(obstacles)
+
+    # Verify basic properties of SDF
+    assert sdf.shape == obstacles.shape, "SDF should have the same shape as input map"
+    assert np.all(np.isfinite(sdf)), "SDF should not contain infinite values"
+
+    # Verify SDF value is negative at obstacle position
+    assert sdf[1, 1] < 0, "SDF value should be negative at obstacle position"
+
+    # Verify SDF value is positive in free space
+    assert sdf[0, 0] > 0, "SDF value should be positive in free space"
+
+
+def test_compute_udf():
+    """Test the computation of Unsigned Distance Field (UDF)"""
+    # Create obstacle map for testing
+    obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]])
+
+    udf = m.compute_udf(obstacles)
+
+    # Verify basic properties of UDF
+    assert udf.shape == obstacles.shape, "UDF should have the same shape as input map"
+    assert np.all(np.isfinite(udf)), "UDF should not contain infinite values"
+    assert np.all(udf >= 0), "All UDF values should be non-negative"
+
+    # Verify UDF value is 0 at obstacle position
+    assert np.abs(udf[1, 1]) < 1e-10, "UDF value should be 0 at obstacle position"
+
+    # Verify UDF value is 1 for adjacent cells
+    assert np.abs(udf[0, 1] - 1.0) < 1e-10, (
+        "UDF value should be 1 for cells adjacent to obstacle"
+    )
+    assert np.abs(udf[1, 0] - 1.0) < 1e-10, (
+        "UDF value should be 1 for cells adjacent to obstacle"
+    )
+    assert np.abs(udf[1, 2] - 1.0) < 1e-10, (
+        "UDF value should be 1 for cells adjacent to obstacle"
+    )
+    assert np.abs(udf[2, 1] - 1.0) < 1e-10, (
+        "UDF value should be 1 for cells adjacent to obstacle"
+    )
+
+
+def test_dt():
+    """Test the computation of 1D distance transform"""
+    # Create test data
+    d = np.array([m.INF, 0, m.INF])
+    m.dt(d)
+
+    # Verify distance transform results
+    assert np.all(np.isfinite(d)), (
+        "Distance transform result should not contain infinite values"
+    )
+    assert d[1] == 0, "Distance at obstacle position should be 0"
+    assert d[0] == 1, "Distance at adjacent position should be 1"
+    assert d[2] == 1, "Distance at adjacent position should be 1"
+
+
+def test_compute_sdf_empty():
+    """Test SDF computation with empty map"""
+    # Test with empty map (no obstacles)
+    empty_map = np.zeros((5, 5))
+    sdf = m.compute_sdf(empty_map)
+
+    assert np.all(sdf > 0), "All SDF values should be positive for empty map"
+    assert sdf.shape == empty_map.shape, "Output shape should match input shape"
+
+
+def test_compute_sdf_full():
+    """Test SDF computation with fully occupied map"""
+    # Test with fully occupied map
+    full_map = np.ones((5, 5))
+    sdf = m.compute_sdf(full_map)
+
+    assert np.all(sdf < 0), "All SDF values should be negative for fully occupied map"
+    assert sdf.shape == full_map.shape, "Output shape should match input shape"
+
+
+def test_compute_udf_invalid_input():
+    """Test UDF computation with invalid input values"""
+    # Test with invalid values (not 0 or 1)
+    invalid_map = np.array([[0, 2, 0], [0, -1, 0], [0, 0.5, 0]])
+
+    try:
+        m.compute_udf(invalid_map)
+        assert False, "Should raise ValueError for invalid input values"
+    except ValueError:
+        pass
+
+
+def test_compute_udf_empty():
+    """Test UDF computation with empty map"""
+    # Test with empty map
+    empty_map = np.zeros((5, 5))
+    udf = m.compute_udf(empty_map)
+
+    assert np.all(udf > 0), "All UDF values should be positive for empty map"
+    assert np.all(np.isfinite(udf)), "UDF should not contain infinite values"
+
+
+def test_main():
+    """Test the execution of main function"""
+    m.ENABLE_PLOT = False
+    m.main()
+
+
+if __name__ == "__main__":
+    conftest.run_this_test(__file__)