forked from AtsushiSakai/PythonRobotics
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathdistance_map.py
More file actions
202 lines (165 loc) · 5.3 KB
/
distance_map.py
File metadata and controls
202 lines (165 loc) · 5.3 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
"""
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
import scipy
INF = 1e20
ENABLE_PLOT = True
def compute_sdf_scipy(obstacles):
"""
Compute the signed distance field (SDF) from a boolean field using scipy.
This function has the same functionality as compute_sdf.
However, by using scipy.ndimage.distance_transform_edt, it can compute much faster.
Example: 500×500 map
• compute_sdf: 3 sec
• compute_sdf_scipy: 0.05 sec
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.
"""
# distance_transform_edt use '0' as obstacles, so we need to convert the obstacles to '0'
a = scipy.ndimage.distance_transform_edt(obstacles == 0)
b = scipy.ndimage.distance_transform_edt(obstacles == 1)
return a - b
def compute_udf_scipy(obstacles):
"""
Compute the unsigned distance field (UDF) from a boolean field using scipy.
This function has the same functionality as compute_udf.
However, by using scipy.ndimage.distance_transform_edt, it can compute much faster.
Example: 500×500 map
• compute_udf: 1.5 sec
• compute_udf_scipy: 0.02 sec
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`.
"""
return scipy.ndimage.distance_transform_edt(obstacles == 0)
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()