Update.
[picoclvr.git] / maze.py
1 #!/usr/bin/env python
2
3 # Any copyright is dedicated to the Public Domain.
4 # https://creativecommons.org/publicdomain/zero/1.0/
5
6 # Written by Francois Fleuret <francois@fleuret.org>
7
8 import torch, torchvision
9
10 ######################################################################
11
12 v_empty, v_wall, v_start, v_goal, v_path = 0, 1, 2, 3, 4
13
14
15 def create_maze(h=11, w=17, nb_walls=8):
16     assert h % 2 == 1 and w % 2 == 1
17
18     a, k = 0, 0
19
20     while k < nb_walls:
21         while True:
22             if a == 0:
23                 m = torch.zeros(h, w, dtype=torch.int64)
24                 m[0, :] = 1
25                 m[-1, :] = 1
26                 m[:, 0] = 1
27                 m[:, -1] = 1
28
29             r = torch.rand(4)
30
31             if r[0] <= 0.5:
32                 i1, i2, j = (
33                     int((r[1] * h).item()),
34                     int((r[2] * h).item()),
35                     int((r[3] * w).item()),
36                 )
37                 i1, i2, j = i1 - i1 % 2, i2 - i2 % 2, j - j % 2
38                 i1, i2 = min(i1, i2), max(i1, i2)
39                 if i2 - i1 > 1 and i2 - i1 <= h / 2 and m[i1 : i2 + 1, j].sum() <= 1:
40                     m[i1 : i2 + 1, j] = 1
41                     break
42             else:
43                 i, j1, j2 = (
44                     int((r[1] * h).item()),
45                     int((r[2] * w).item()),
46                     int((r[3] * w).item()),
47                 )
48                 i, j1, j2 = i - i % 2, j1 - j1 % 2, j2 - j2 % 2
49                 j1, j2 = min(j1, j2), max(j1, j2)
50                 if j2 - j1 > 1 and j2 - j1 <= w / 2 and m[i, j1 : j2 + 1].sum() <= 1:
51                     m[i, j1 : j2 + 1] = 1
52                     break
53             a += 1
54
55             if a > 10 * nb_walls:
56                 a, k = 0, 0
57
58         k += 1
59
60     return m
61
62
63 ######################################################################
64
65
66 def compute_distance(walls, goal_i, goal_j):
67     max_length = walls.numel()
68     dist = torch.full_like(walls, max_length)
69
70     dist[goal_i, goal_j] = 0
71     pred_dist = torch.empty_like(dist)
72
73     while True:
74         pred_dist.copy_(dist)
75         d = (
76             torch.cat(
77                 (
78                     dist[None, 1:-1, 0:-2],
79                     dist[None, 2:, 1:-1],
80                     dist[None, 1:-1, 2:],
81                     dist[None, 0:-2, 1:-1],
82                 ),
83                 0,
84             ).min(dim=0)[0]
85             + 1
86         )
87
88         dist[1:-1, 1:-1] = torch.min(dist[1:-1, 1:-1], d)
89         dist = walls * max_length + (1 - walls) * dist
90
91         if dist.equal(pred_dist):
92             return dist * (1 - walls)
93
94
95 ######################################################################
96
97
98 def compute_policy(walls, goal_i, goal_j):
99     distance = compute_distance(walls, goal_i, goal_j)
100     distance = distance + walls.numel() * walls
101
102     value = distance.new_full((4,) + distance.size(), walls.numel())
103     value[0, :, 1:] = distance[:, :-1]  # <
104     value[1, :, :-1] = distance[:, 1:]  # >
105     value[2, 1:, :] = distance[:-1, :]  # ^
106     value[3, :-1, :] = distance[1:, :]  # v
107
108     proba = (value.min(dim=0)[0][None] == value).float()
109     proba = proba / proba.sum(dim=0)[None]
110     proba = proba * (1 - walls) + walls.float() / 4
111
112     return proba
113
114
115 def stationary_densities(mazes, policies):
116     policies = policies * (mazes != v_goal)[:, None]
117     start = (mazes == v_start).nonzero(as_tuple=True)
118     probas = mazes.new_zeros(mazes.size(), dtype=torch.float32)
119     pred_probas = probas.clone()
120     probas[start] = 1.0
121
122     while not pred_probas.equal(probas):
123         pred_probas.copy_(probas)
124         probas.zero_()
125         probas[:, 1:, :] += pred_probas[:, :-1, :] * policies[:, 3, :-1, :]
126         probas[:, :-1, :] += pred_probas[:, 1:, :] * policies[:, 2, 1:, :]
127         probas[:, :, 1:] += pred_probas[:, :, :-1] * policies[:, 1, :, :-1]
128         probas[:, :, :-1] += pred_probas[:, :, 1:] * policies[:, 0, :, 1:]
129         probas[start] = 1.0
130
131     return probas
132
133
134 ######################################################################
135
136
137 def mark_path(walls, i, j, goal_i, goal_j, policy):
138     action = torch.distributions.categorical.Categorical(
139         policy.permute(1, 2, 0)
140     ).sample()
141     n, nmax = 0, walls.numel()
142     while i != goal_i or j != goal_j:
143         di, dj = [(0, -1), (0, 1), (-1, 0), (1, 0)][action[i, j]]
144         i, j = i + di, j + dj
145         assert walls[i, j] == 0
146         walls[i, j] = v_path
147         n += 1
148         assert n < nmax
149
150
151 def path_optimality(ref_paths, paths):
152     return (ref_paths == v_path).long().flatten(1).sum(1) == (
153         paths == v_path
154     ).long().flatten(1).sum(1)
155
156
157 def path_correctness(mazes, paths):
158     still_ok = (mazes - (paths * (paths != v_path))).view(mazes.size(0), -1).abs().sum(
159         1
160     ) == 0
161     reached = still_ok.new_zeros(still_ok.size())
162     current, pred_current = paths.clone(), paths.new_zeros(paths.size())
163     goal = (mazes == v_goal).long()
164     while not pred_current.equal(current):
165         pred_current.copy_(current)
166         u = (current == v_start).long()
167         possible_next = (
168             u[:, 2:, 1:-1] + u[:, 0:-2, 1:-1] + u[:, 1:-1, 2:] + u[:, 1:-1, 0:-2] > 0
169         ).long()
170         u = u[:, 1:-1, 1:-1]
171         reached += ((goal[:, 1:-1, 1:-1] * possible_next).sum((1, 2)) == 1) * (
172             (current == v_path).sum((1, 2)) == 0
173         )
174         current[:, 1:-1, 1:-1] = (1 - u) * current[:, 1:-1, 1:-1] + (
175             v_start - v_path
176         ) * (possible_next * (current[:, 1:-1, 1:-1] == v_path))
177         still_ok *= (current == v_start).sum((1, 2)) <= 1
178
179     return still_ok * reached
180
181
182 ######################################################################
183
184
185 def create_maze_data(
186     nb, height=11, width=17, nb_walls=8, dist_min=10, progress_bar=lambda x: x
187 ):
188     mazes = torch.empty(nb, height, width, dtype=torch.int64)
189     paths = torch.empty(nb, height, width, dtype=torch.int64)
190     policies = torch.empty(nb, 4, height, width)
191
192     for n in progress_bar(range(nb)):
193         maze = create_maze(height, width, nb_walls)
194         i = (maze == v_empty).nonzero()
195         while True:
196             start, goal = i[torch.randperm(i.size(0))[:2]]
197             if (start - goal).abs().sum() >= dist_min:
198                 break
199         start_i, start_j, goal_i, goal_j = start[0], start[1], goal[0], goal[1]
200
201         policy = compute_policy(maze, goal_i, goal_j)
202         path = maze.clone()
203         mark_path(path, start_i, start_j, goal_i, goal_j, policy)
204         maze[start_i, start_j] = v_start
205         maze[goal_i, goal_j] = v_goal
206         path[start_i, start_j] = v_start
207         path[goal_i, goal_j] = v_goal
208
209         mazes[n] = maze
210         paths[n] = path
211         policies[n] = policy
212
213     return mazes, paths, policies
214
215
216 ######################################################################
217
218
219 def save_image(
220     name,
221     mazes,
222     target_paths=None,
223     predicted_paths=None,
224     path_correct=None,
225     path_optimal=None,
226 ):
227     colors = torch.tensor(
228         [
229             [255, 255, 255],  # empty
230             [0, 0, 0],  # wall
231             [0, 255, 0],  # start
232             [127, 127, 255],  # goal
233             [255, 0, 0],  # path
234         ]
235     )
236
237     mazes = mazes.cpu()
238
239     c_mazes = (
240         colors[mazes.reshape(-1)].reshape(mazes.size() + (-1,)).permute(0, 3, 1, 2)
241     )
242
243     imgs = c_mazes.unsqueeze(1)
244
245     if target_paths is not None:
246         target_paths = target_paths.cpu()
247
248         c_target_paths = (
249             colors[target_paths.reshape(-1)]
250             .reshape(target_paths.size() + (-1,))
251             .permute(0, 3, 1, 2)
252         )
253
254         imgs = torch.cat((imgs, c_target_paths.unsqueeze(1)), 1)
255
256     if predicted_paths is not None:
257         predicted_paths = predicted_paths.cpu()
258         c_predicted_paths = (
259             colors[predicted_paths.reshape(-1)]
260             .reshape(predicted_paths.size() + (-1,))
261             .permute(0, 3, 1, 2)
262         )
263         imgs = torch.cat((imgs, c_predicted_paths.unsqueeze(1)), 1)
264
265     img = torch.tensor([255, 255, 0]).view(1, -1, 1, 1)
266
267     # NxKxCxHxW
268     if path_optimal is not None:
269         path_optimal = path_optimal.cpu().long().view(-1, 1, 1, 1)
270         img = (
271             img * (1 - path_optimal)
272             + torch.tensor([0, 255, 0]).view(1, -1, 1, 1) * path_optimal
273         )
274
275     if path_correct is not None:
276         path_correct = path_correct.cpu().long().view(-1, 1, 1, 1)
277         img = img * path_correct + torch.tensor([255, 0, 0]).view(1, -1, 1, 1) * (
278             1 - path_correct
279         )
280
281     img = img.expand(
282         -1, -1, imgs.size(3) + 2, 1 + imgs.size(1) * (1 + imgs.size(4))
283     ).clone()
284
285     print(f"{img.size()=} {imgs.size()=}")
286
287     for k in range(imgs.size(1)):
288         img[
289             :,
290             :,
291             1 : 1 + imgs.size(3),
292             1 + k * (1 + imgs.size(4)) : 1 + k * (1 + imgs.size(4)) + imgs.size(4),
293         ] = imgs[:, k]
294
295     img = img.float() / 255.0
296
297     torchvision.utils.save_image(img, name, nrow=4, padding=1, pad_value=224.0 / 256)
298
299
300 ######################################################################
301
302 if __name__ == "__main__":
303     device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
304     mazes, paths, policies = create_maze_data(8)
305     mazes, paths = mazes.to(device), paths.to(device)
306     save_image("test.png", mazes=mazes, target_paths=paths, predicted_paths=paths)
307     print(path_correctness(mazes, paths))
308
309 ######################################################################