diff --git a/Mapping/DistanceMap/distance_map.py b/Mapping/DistanceMap/distance_map.py new file mode 100644 index 0000000000..54c98c6a75 --- /dev/null +++ b/Mapping/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() diff --git a/docs/modules/3_mapping/distance_map/distance_map.png b/docs/modules/3_mapping/distance_map/distance_map.png new file mode 100644 index 0000000000..2d89252a70 Binary files /dev/null and b/docs/modules/3_mapping/distance_map/distance_map.png differ diff --git a/docs/modules/3_mapping/distance_map/distance_map_main.rst b/docs/modules/3_mapping/distance_map/distance_map_main.rst new file mode 100644 index 0000000000..45273cd3bb --- /dev/null +++ b/docs/modules/3_mapping/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 `_ paper by Pedro F. Felzenszwalb and Daniel P. Huttenlocher. \ No newline at end of file diff --git a/docs/modules/3_mapping/mapping_main.rst b/docs/modules/3_mapping/mapping_main.rst index 28e18984d3..825b08d3ec 100644 --- a/docs/modules/3_mapping/mapping_main.rst +++ b/docs/modules/3_mapping/mapping_main.rst @@ -17,3 +17,4 @@ Mapping is the ability of a robot to understand its surroundings with external s circle_fitting/circle_fitting rectangle_fitting/rectangle_fitting normal_vector_estimation/normal_vector_estimation + distance_map/distance_map diff --git a/tests/test_distance_map.py b/tests/test_distance_map.py new file mode 100644 index 0000000000..df6e394e2c --- /dev/null +++ b/tests/test_distance_map.py @@ -0,0 +1,118 @@ +import conftest # noqa +import numpy as np +from Mapping.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__)