forked from pinae/BresenhamLidar
-
Notifications
You must be signed in to change notification settings - Fork 0
/
plot_helpers.py
72 lines (68 loc) · 3.14 KB
/
plot_helpers.py
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
from mpl_toolkits import mplot3d
import matplotlib.pyplot as plt
import numpy as np
def voxel_lines(v, step_size):
xl = [v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2 + step_size,
v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2,
v[0] * step_size - step_size / 2]
yl = [v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2 + step_size,
v[1] * step_size - step_size / 2]
zl = [v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2,
v[2] * step_size - step_size / 2 + step_size,
v[2] * step_size - step_size / 2 + step_size]
return xl, yl, zl
def plot_voxel_line(voxels, step_size, a, start, target):
fig = plt.figure(figsize=[9.5, 8])
ax = plt.axes(projection='3d', box_aspect=(1.0, 1.0, 1.0))
ax.set_xlim3d(0, a.shape[0] * step_size)
ax.set_xlabel('x')
ax.set_ylim3d(0, a.shape[1] * step_size)
ax.set_ylabel('y')
ax.set_zlim3d(0, a.shape[2] * step_size)
ax.set_zlabel('z')
ax.plot3D(*np.column_stack([start, target]), 'red')
# ax.scatter3D(*np.column_stack([start_voxel*step_size, start_voxel*step_size+0.5]))
for v in voxels:
ax.plot3D(*voxel_lines(v, step_size), 'blue')
# ax.plot3D(*voxel_lines(np.array([0, 0, 0]), step_size), 'green')
return fig