|
| 1 | +import conftest # noqa |
| 2 | +import numpy as np |
| 3 | +from Mapping.DistanceMap import distance_map as m |
| 4 | + |
| 5 | + |
| 6 | +def test_compute_sdf(): |
| 7 | + """Test the computation of Signed Distance Field (SDF)""" |
| 8 | + # Create a simple obstacle map for testing |
| 9 | + obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]]) |
| 10 | + |
| 11 | + sdf = m.compute_sdf(obstacles) |
| 12 | + |
| 13 | + # Verify basic properties of SDF |
| 14 | + assert sdf.shape == obstacles.shape, "SDF should have the same shape as input map" |
| 15 | + assert np.all(np.isfinite(sdf)), "SDF should not contain infinite values" |
| 16 | + |
| 17 | + # Verify SDF value is negative at obstacle position |
| 18 | + assert sdf[1, 1] < 0, "SDF value should be negative at obstacle position" |
| 19 | + |
| 20 | + # Verify SDF value is positive in free space |
| 21 | + assert sdf[0, 0] > 0, "SDF value should be positive in free space" |
| 22 | + |
| 23 | + |
| 24 | +def test_compute_udf(): |
| 25 | + """Test the computation of Unsigned Distance Field (UDF)""" |
| 26 | + # Create obstacle map for testing |
| 27 | + obstacles = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]]) |
| 28 | + |
| 29 | + udf = m.compute_udf(obstacles) |
| 30 | + |
| 31 | + # Verify basic properties of UDF |
| 32 | + assert udf.shape == obstacles.shape, "UDF should have the same shape as input map" |
| 33 | + assert np.all(np.isfinite(udf)), "UDF should not contain infinite values" |
| 34 | + assert np.all(udf >= 0), "All UDF values should be non-negative" |
| 35 | + |
| 36 | + # Verify UDF value is 0 at obstacle position |
| 37 | + assert np.abs(udf[1, 1]) < 1e-10, "UDF value should be 0 at obstacle position" |
| 38 | + |
| 39 | + # Verify UDF value is 1 for adjacent cells |
| 40 | + assert np.abs(udf[0, 1] - 1.0) < 1e-10, ( |
| 41 | + "UDF value should be 1 for cells adjacent to obstacle" |
| 42 | + ) |
| 43 | + assert np.abs(udf[1, 0] - 1.0) < 1e-10, ( |
| 44 | + "UDF value should be 1 for cells adjacent to obstacle" |
| 45 | + ) |
| 46 | + assert np.abs(udf[1, 2] - 1.0) < 1e-10, ( |
| 47 | + "UDF value should be 1 for cells adjacent to obstacle" |
| 48 | + ) |
| 49 | + assert np.abs(udf[2, 1] - 1.0) < 1e-10, ( |
| 50 | + "UDF value should be 1 for cells adjacent to obstacle" |
| 51 | + ) |
| 52 | + |
| 53 | + |
| 54 | +def test_dt(): |
| 55 | + """Test the computation of 1D distance transform""" |
| 56 | + # Create test data |
| 57 | + d = np.array([m.INF, 0, m.INF]) |
| 58 | + m.dt(d) |
| 59 | + |
| 60 | + # Verify distance transform results |
| 61 | + assert np.all(np.isfinite(d)), ( |
| 62 | + "Distance transform result should not contain infinite values" |
| 63 | + ) |
| 64 | + assert d[1] == 0, "Distance at obstacle position should be 0" |
| 65 | + assert d[0] == 1, "Distance at adjacent position should be 1" |
| 66 | + assert d[2] == 1, "Distance at adjacent position should be 1" |
| 67 | + |
| 68 | + |
| 69 | +def test_compute_sdf_empty(): |
| 70 | + """Test SDF computation with empty map""" |
| 71 | + # Test with empty map (no obstacles) |
| 72 | + empty_map = np.zeros((5, 5)) |
| 73 | + sdf = m.compute_sdf(empty_map) |
| 74 | + |
| 75 | + assert np.all(sdf > 0), "All SDF values should be positive for empty map" |
| 76 | + assert sdf.shape == empty_map.shape, "Output shape should match input shape" |
| 77 | + |
| 78 | + |
| 79 | +def test_compute_sdf_full(): |
| 80 | + """Test SDF computation with fully occupied map""" |
| 81 | + # Test with fully occupied map |
| 82 | + full_map = np.ones((5, 5)) |
| 83 | + sdf = m.compute_sdf(full_map) |
| 84 | + |
| 85 | + assert np.all(sdf < 0), "All SDF values should be negative for fully occupied map" |
| 86 | + assert sdf.shape == full_map.shape, "Output shape should match input shape" |
| 87 | + |
| 88 | + |
| 89 | +def test_compute_udf_invalid_input(): |
| 90 | + """Test UDF computation with invalid input values""" |
| 91 | + # Test with invalid values (not 0 or 1) |
| 92 | + invalid_map = np.array([[0, 2, 0], [0, -1, 0], [0, 0.5, 0]]) |
| 93 | + |
| 94 | + try: |
| 95 | + m.compute_udf(invalid_map) |
| 96 | + assert False, "Should raise ValueError for invalid input values" |
| 97 | + except ValueError: |
| 98 | + pass |
| 99 | + |
| 100 | + |
| 101 | +def test_compute_udf_empty(): |
| 102 | + """Test UDF computation with empty map""" |
| 103 | + # Test with empty map |
| 104 | + empty_map = np.zeros((5, 5)) |
| 105 | + udf = m.compute_udf(empty_map) |
| 106 | + |
| 107 | + assert np.all(udf > 0), "All UDF values should be positive for empty map" |
| 108 | + assert np.all(np.isfinite(udf)), "UDF should not contain infinite values" |
| 109 | + |
| 110 | + |
| 111 | +def test_main(): |
| 112 | + """Test the execution of main function""" |
| 113 | + m.ENABLE_PLOT = False |
| 114 | + m.main() |
| 115 | + |
| 116 | + |
| 117 | +if __name__ == "__main__": |
| 118 | + conftest.run_this_test(__file__) |
0 commit comments