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)