def populate_frustum_voxels(planes: Sequence[np.ndarray], fig: Figure, axis_pair: str) -> Figure:
"""
Generate grid in xy plane, and then treat it as grid in xz (ground) plane
in camera coordinate system.
Args:
planes: list of length 5. Each list element is a Numpy array
of shape (4,) representing the equation of a plane,
e.g. (a,b,c,d) in ax+by+cz=d
fig: Mayavi figure to draw on
axis_pair: Either "xz" or "yz"
Returns:
Mayavi figure
"""
sparse_xz_voxel_grid = get_mesh_grid_as_point_cloud(-20, 20, 0, 40, downsample_factor=0.1)
sparse_voxel_grid = np.zeros((sparse_xz_voxel_grid.shape[0], 3))
if axis_pair == "xz":
sparse_voxel_grid[:, 0] = sparse_xz_voxel_grid[:, 0]
sparse_voxel_grid[:, 2] = sparse_xz_voxel_grid[:, 1]
elif axis_pair == "yz":
sparse_voxel_grid[:, 1] = sparse_xz_voxel_grid[:, 0]
sparse_voxel_grid[:, 2] = sparse_xz_voxel_grid[:, 1]
# keep only the points that have signed distance > 0 (inside the frustum, plane
# normals also point into the frustum)
for plane in planes:
signed_d = np.matmul(sparse_voxel_grid, plane[:3]) + plane[3]
sparse_voxel_grid = sparse_voxel_grid[np.where(signed_d > 0)]
plot_points_3D_mayavi(sparse_voxel_grid, fig, fixed_color=(1, 0, 0))
return fig
def test_plot_points_3D_mayavi() -> None:
"""Visualize 3D point cloud with Mayavi.
Note
-----
To see result, simply add the following line:
.. code-block:: python
mayavi.mlab.show()
"""
fig = mayavi.mlab.figure(bgcolor=(1, 1, 1), size=(2000, 1000))
point_arr = np.array([[0, 1, 2], [1, -1, 4], [-1, -1, 4], [0, -1, 2]])
fig = plot_points_3D_mayavi(point_arr, fig, fixed_color=(1, 0, 0))
point_arr = np.random.randn(100000, 3)
fig = plot_points_3D_mayavi(point_arr, fig, fixed_color=(1, 0, 0))
mayavi.mlab.close(fig)
def plot_frustum_planes_and_normals(
planes: List[np.ndarray],
cuboid_verts: Optional[np.ndarray] = None,
near_clip_dist: float = 0.5,
) -> None:
"""
Args:
planes: list of length 5. Each list element is a Numpy array
of shape (4,) representing the equation of a plane,
e.g. (a,b,c,d) in ax+by+cz=d
cuboid_verts: Numpy array of shape (N,3) representing
cuboid vertices
Returns:
None
"""
fig = mayavi_wrapper.mlab.figure(bgcolor=(1, 1, 1),
size=(2000, 1000)) # type: ignore
if cuboid_verts is not None:
# fig = plot_bbox_3d_mayavi(fig, cuboid_verts)
fig = plot_3d_clipped_bbox_mayavi(fig, planes, cuboid_verts)
P = np.array([0.0, 0.0, 0.0])
for i, plane in enumerate(planes):
(a, b, c, d) = plane
if i == 0:
color = (1, 0, 0) # red left
elif i == 1:
color = (0, 0, 1) # blue right
elif i == 2:
color = (1, 1, 0) # near yellow
P = np.array([0.0, 0.0, near_clip_dist])
elif i == 3:
color = (0, 1, 0) # low is green
elif i == 4:
color = (0, 1, 1) # top is teal
plane_pts = generate_grid_on_plane(a, b, c, d, P)
fig = plot_points_3D_mayavi(plane_pts, fig, color)
# plot the normals at (0,0,0.5) and normal vector (u,v,w) given by (a,b,c)
mayavi_wrapper.mlab.quiver3d( # type: ignore
0,
0,
0.5,
a * 1000,
b * 1000,
c * 1000,
color=color,
figure=fig,
line_width=8,
)
# draw teal line at top below the camera
pt1 = np.array([-5, 0, -5])
pt2 = np.array([5, 0, -5])
color = (0, 1, 1)
draw_mayavi_line_segment(fig, [pt1, pt2], color=color, line_width=8)
# draw blue line in middle
pt1 = np.array([-5, 5, -5])
pt2 = np.array([5, 5, -5])
color = (0, 0, 1)
draw_mayavi_line_segment(fig, [pt1, pt2], color=color, line_width=8)
# draw yellow, lowest line (+y axis is down)
pt1 = np.array([-5, 10, -5])
pt2 = np.array([5, 10, -5])
color = (1, 1, 0)
draw_mayavi_line_segment(fig, [pt1, pt2], color=color, line_width=8)
fig = populate_frustum_voxels(planes, fig, "xz")
fig = populate_frustum_voxels(planes, fig, "yz")
mayavi_wrapper.mlab.view(distance=200) # type: ignore
mayavi_wrapper.mlab.show() # type: ignore
def test_plot_points_3d_argoverse() -> None:
"""Render a LiDAR sweep from Argoverse, loaded from a .txt file."""
fig = mayavi.mlab.figure(bgcolor=(1, 1, 1), size=(2000, 1000))
point_arr = np.loadtxt(_TEST_DIR / "test_data/sample_argoverse_sweep.txt")
fig = plot_points_3D_mayavi(point_arr, fig, fixed_color=(1, 0, 0))
mayavi.mlab.close(fig)