def test_xyz_lut_angles(stream_digest: digest.StreamDigest, meta: client.SensorInfo) -> None: """Check that invalid beam angle dimensions are handled by xyzlut.""" meta1 = copy(meta) client.XYZLut(meta1) meta1.beam_azimuth_angles = meta1.beam_azimuth_angles + [0.0] with pytest.raises(ValueError): client.XYZLut(meta1) meta1.beam_azimuth_angles = meta1.beam_azimuth_angles[:-2] with pytest.raises(ValueError): client.XYZLut(meta1) meta1.beam_azimuth_angles = [] with pytest.raises(ValueError): client.XYZLut(meta1) meta2 = copy(meta) client.XYZLut(meta2) meta2.beam_azimuth_angles = meta1.beam_altitude_angles + [0.0] with pytest.raises(ValueError): client.XYZLut(meta2) meta2.beam_azimuth_angles = meta1.beam_altitude_angles[:-2] with pytest.raises(ValueError): client.XYZLut(meta2) meta2.beam_azimuth_angles = [] with pytest.raises(ValueError): client.XYZLut(meta2)
def pcap_to_las(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0, las_dir: str = ".", las_base: str = "las_out", las_ext: str = "las") -> None: "Write scans from a pcap to las files (one per lidar scan)." from itertools import islice import laspy # type: ignore # precompute xyzlut to save computation in a loop xyzlut = client.XYZLut(metadata) # create an iterator of LidarScans from pcap and bound it if num is specified scans = iter(client.Scans(source)) if num: scans = islice(scans, num) for idx, scan in enumerate(scans): xyz = xyzlut(scan) las = laspy.create() las.x = xyz[:, :, 0].flatten() las.y = xyz[:, :, 1].flatten() las.z = xyz[:, :, 2].flatten() las_path = os.path.join(las_dir, f'{las_base}_{idx:06d}.{las_ext}') print(f'write frame #{idx} to file: {las_path}') las.write(las_path)
def plot_xyz_points(hostname: str, lidar_port: int = 7502) -> None: """Display range from a single scan as 3D points Args: hostname: hostname of the sensor lidar_port: UDP port to listen on for lidar data """ import matplotlib.pyplot as plt # type: ignore # get single scan metadata, sample = client.Scans.sample(hostname, 1, lidar_port) scan = next(sample)[0] # set up figure plt.figure() ax = plt.axes(projection='3d') r = 3 ax.set_xlim3d([-r, r]) ax.set_ylim3d([-r, r]) ax.set_zlim3d([-r, r]) plt.title("3D Points from {}".format(hostname)) # [doc-stag-plot-xyz-points] # transform data to 3d points and graph xyzlut = client.XYZLut(metadata) xyz = xyzlut(scan) [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] ax.scatter(x, y, z, c=z / max(z), s=0.2) # [doc-etag-plot-xyz-points] plt.show()
def pcap_display_xyz_points(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0) -> None: """Plot point cloud using matplotlib.""" import matplotlib.pyplot as plt # type: ignore # [doc-stag-pcap-plot-xyz-points] from more_itertools import nth scan = nth(client.Scans(source), num) if not scan: print(f"ERROR: Scan # {num} in not present in pcap file") exit(1) # set up figure plt.figure() ax = plt.axes(projection='3d') r = 6 ax.set_xlim3d([-r, r]) ax.set_ylim3d([-r, r]) ax.set_zlim3d([-r, r]) plt.title("3D Points XYZ for scan") # transform data to 3d points and graph xyzlut = client.XYZLut(metadata) xyz = xyzlut(scan) key = scan.field(client.ChanField.SIGNAL) [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] ax.scatter(x, y, z, c=normalize(key.flatten()), s=0.2) plt.show()
def test_destagger_xyz(meta, scan) -> None: """Check that we can destagger the output of xyz projection.""" h = meta.format.pixels_per_column w = meta.format.columns_per_frame xyz = client.XYZLut(meta)(scan) destaggered = client.destagger(meta, xyz) assert destaggered.shape == (h, w, 3)
def pcap_3d_one_scan(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0) -> None: """Render one scan from a pcap file in the Open3D viewer. Args: source: PacketSource from pcap metadata: associated SensorInfo for PacketSource num: scan number in a given pcap file (satrs from *0*) """ try: import open3d as o3d # type: ignore except ModuleNotFoundError: print( "This example requires open3d, which may not be available on all " "platforms. Try running `pip3 install open3d` first.") exit(1) from more_itertools import nth # get single scan by index scan = nth(client.Scans(source), num) if not scan: print(f"ERROR: Scan # {num} in not present in pcap file") exit(1) # [doc-stag-open3d-one-scan] # compute point cloud using client.SensorInfo and client.LidarScan xyz = client.XYZLut(metadata)(scan) # create point cloud and coordinate axes geometries cloud = o3d.geometry.PointCloud( o3d.utility.Vector3dVector(xyz.reshape((-1, 3)))) # type: ignore axes = o3d.geometry.TriangleMesh.create_coordinate_frame( 1.0) # type: ignore # [doc-etag-open3d-one-scan] # initialize visualizer and rendering options vis = o3d.visualization.Visualizer() # type: ignore vis.create_window() vis.add_geometry(cloud) vis.add_geometry(axes) ropt = vis.get_render_option() ropt.point_size = 1.0 ropt.background_color = np.asarray([0, 0, 0]) # initialize camera settings ctr = vis.get_view_control() ctr.set_zoom(0.1) ctr.set_lookat([0, 0, 0]) ctr.set_up([1, 0, 0]) # run visualizer main loop print("Press Q or Excape to exit") vis.run() vis.destroy_window()
def test_xyz_range(stream_digest: digest.StreamDigest, scan: client.LidarScan, meta: client.SensorInfo) -> None: """Test that projection works on an ndarrays as well.""" xyzlut = client.XYZLut(meta) range = scan.field(client.ChanField.RANGE) xyz_from_scan = xyzlut(scan) xyz_from_range = xyzlut(range) assert np.array_equal(xyz_from_scan, xyz_from_range)
def test_xyz_calcs(stream_digest: digest.StreamDigest, scan: client.LidarScan, meta: client.SensorInfo) -> None: """Compare the optimized xyz projection to a reference implementation.""" # compute 3d points using reference implementation xyz_from_docs = reference.xyz_proj(meta, scan) # transform data to 3d points using optimized implementation xyzlut = client.XYZLut(meta) xyz_from_lut = xyzlut(scan) assert np.allclose(xyz_from_docs, xyz_from_lut)
def test_xyz_lut_scan_dims(stream_digest: digest.StreamDigest, meta: client.SensorInfo) -> None: """Check that (in)valid lidar scan dimensions are handled by xyzlut.""" w = meta.format.columns_per_frame h = meta.format.pixels_per_column xyzlut = client.XYZLut(meta) assert xyzlut(client.LidarScan(h, w)).shape == (h, w, 3) with pytest.raises(ValueError): xyzlut(client.LidarScan(h + 1, w)) with pytest.raises(ValueError): xyzlut(client.LidarScan(h, w - 1))
def pcap_3d_one_scan(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0) -> None: """Render one scan from a pcap file in the Open3D viewer. Args: pcap_path: path to the pcap file metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) num: scan number in a given pcap file (satrs from *0*) """ import open3d as o3d # get single scan by index scan = nth(client.Scans(source), num) if not scan: print(f"ERROR: Scan # {num} in not present in pcap file") exit(1) # [doc-stag-open3d-one-scan] # compute point cloud using client.SensorInfo and client.LidarScan xyz = client.XYZLut(metadata)(scan) # create point cloud and coordinate axes geometries cloud = o3d.geometry.PointCloud( o3d.utility.Vector3dVector(xyz.reshape((-1, 3)))) axes = o3d.geometry.TriangleMesh.create_coordinate_frame(1.0) # [doc-etag-open3d-one-scan] # initialize visualizer and rendering options vis = o3d.visualization.Visualizer() vis.create_window() vis.add_geometry(cloud) vis.add_geometry(axes) ropt = vis.get_render_option() ropt.point_size = 1.0 ropt.background_color = np.asarray([0, 0, 0]) # initialize camera settings ctr = vis.get_view_control() ctr.set_zoom(0.1) ctr.set_lookat([0, 0, 0]) ctr.set_up([1, 0, 0]) # run visualizer main loop print("Press Q or Excape to exit") vis.run() vis.destroy_window()
def filter_3d_by_range_and_azimuth(hostname: str, lidar_port: int = 7502, range_min: int = 2) -> None: """Easily filter 3D Point Cloud by Range and Azimuth Using the 2D Representation Args: hostname: hostname of sensor lidar_port: UDP port to listen on for lidar data range_min: range minimum in meters """ import matplotlib.pyplot as plt # type: ignore import math # set up figure plt.figure() ax = plt.axes(projection='3d') r = 3 ax.set_xlim3d([-r, r]) ax.set_ylim3d([-r, r]) ax.set_zlim3d([-r, r]) plt.title("Filtered 3D Points from {}".format(hostname)) metadata, sample = client.Scans.sample(hostname, 2, lidar_port) scan = next(sample)[1] # [doc-stag-filter-3d] # obtain destaggered range range_destaggered = client.destagger(metadata, scan.field(client.ChanField.RANGE)) # obtain destaggered xyz representation xyzlut = client.XYZLut(metadata) xyz_destaggered = client.destagger(metadata, xyzlut(scan)) # select only points with more than min range using the range data xyz_filtered = xyz_destaggered * (range_destaggered[:, :, np.newaxis] > (range_min * 1000)) # get first 3/4 of scan to_col = math.floor(metadata.mode.cols * 3 / 4) xyz_filtered = xyz_filtered[:, 0:to_col, :] # [doc-etag-filter-3d] [x, y, z] = [c.flatten() for c in np.dsplit(xyz_filtered, 3)] ax.scatter(x, y, z, c=z / max(z), s=0.2) plt.show()
def test_xyz_range_dtype(stream_digest: digest.StreamDigest, scan: client.LidarScan, meta: client.SensorInfo) -> None: """Test that projection works on an ndarrays of different dtypes.""" xyzlut = client.XYZLut(meta) range = scan.field(client.ChanField.RANGE) range[:] = range.astype(np.uint8) xyz_from_scan = xyzlut(scan) xyz_from_range_8 = xyzlut(range.astype(np.uint8)) xyz_from_range_16 = xyzlut(range.astype(np.uint16)) xyz_from_range_32 = xyzlut(range.astype(np.uint32)) xyz_from_range_64 = xyzlut(range.astype(np.uint64)) assert np.array_equal(xyz_from_scan, xyz_from_range_8) assert np.array_equal(xyz_from_scan, xyz_from_range_16) assert np.array_equal(xyz_from_scan, xyz_from_range_32) assert np.array_equal(xyz_from_scan, xyz_from_range_64)
def test_xyz_lut_dims(stream_digest: digest.StreamDigest, meta: client.SensorInfo) -> None: """Check that (in)valid dimensions are handled when creating xyzlut.""" w = meta.format.columns_per_frame h = meta.format.pixels_per_column client.XYZLut(meta) meta1 = copy(meta) client.XYZLut(meta1) # should fail when no. rows doesn't match beam angle arrays meta1.format.pixels_per_column = 0 with pytest.raises(ValueError): client.XYZLut(meta1) meta1.format.pixels_per_column = h + 1 with pytest.raises(ValueError): client.XYZLut(meta1) meta1.format.pixels_per_column = h - 1 with pytest.raises(ValueError): client.XYZLut(meta1) meta2 = copy(meta) client.XYZLut(meta2) # this is nonsensical, but still good to check for memory errors meta2.format.columns_per_frame = w + 1 client.XYZLut(meta2) meta2.format.columns_per_frame = w - 1 client.XYZLut(meta2) meta2.format.columns_per_frame = 0 with pytest.raises(ValueError): client.XYZLut(meta2)
def pcap_display_xyz_points(pcap_path: str, metadata_path: str, num: int = 0) -> None: """Display range from a specified scan number (*num*) as 3D points from pcap file located at *pcap_path* Args: pcap_path: path to the pcap file metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) num: scan number in a given pcap file (satrs from *0*) """ import matplotlib.pyplot as plt # type: ignore # [doc-stag-pcap-plot-xyz-points] metadata = read_metadata(metadata_path) source = pcap.Pcap(pcap_path, metadata) # get single scan scans = client.Scans(source) scan = nth(scans, num) if not scan: print(f'ERROR: Scan # {num} in not present in pcap file: {pcap_path}') return # set up figure plt.figure() ax = plt.axes(projection='3d') r = 6 ax.set_xlim3d([-r, r]) ax.set_ylim3d([-r, r]) ax.set_zlim3d([-r, r]) plt.title("3D Points XYZ for scan") # transform data to 3d points and graph xyzlut = client.XYZLut(metadata) xyz = xyzlut(scan) key = scan.field(client.ChanField.SIGNAL) [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] ax.scatter(x, y, z, c=ae(key.flatten()), s=0.2) plt.show()
def pcap_to_pcd(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0, pcd_dir: str = ".", pcd_base: str = "pcd_out", pcd_ext: str = "pcd") -> None: "Write scans from a pcap to pcd files (one per lidar scan)." from itertools import islice try: import open3d as o3d # type: ignore except ModuleNotFoundError: print( "This example requires open3d, which may not be available on all " "platforms. Try running `pip3 install open3d` first.") exit(1) if not os.path.exists(pcd_dir): os.makedirs(pcd_dir) # precompute xyzlut to save computation in a loop xyzlut = client.XYZLut(metadata) # create an iterator of LidarScans from pcap and bound it if num is specified scans = iter(client.Scans(source)) if num: scans = islice(scans, num) for idx, scan in enumerate(scans): xyz = xyzlut(scan) pcd = o3d.geometry.PointCloud() # type: ignore pcd.points = o3d.utility.Vector3dVector(xyz.reshape(-1, 3)) # type: ignore pcd_path = os.path.join(pcd_dir, f'{pcd_base}_{idx:06d}.{pcd_ext}') print(f'write frame #{idx} to file: {pcd_path}') o3d.io.write_point_cloud(pcd_path, pcd) # type: ignore
def write_xyz_to_csv(hostname: str, lidar_port: int = 7502, cloud_prefix: str = 'xyz', n_scans: int = 5) -> None: """Write xyz sample from live sensor to csv Args: hostname: hostname of the sensor lidar_port: UDP port to listen on for lidar data cloud_prefix: filename prefix for written csvs n_scans: number of scans to write """ metadata, sample = client.Scans.sample(hostname, n_scans, lidar_port) h = metadata.format.pixels_per_column w = metadata.format.columns_per_frame xyzlut = client.XYZLut(metadata) for count, scan in enumerate(next(sample)): out_name = "{}_{}.txt".format(cloud_prefix, count) print("writing {}..".format(out_name)) np.savetxt(out_name, xyzlut(scan).reshape(h * w, 3), delimiter=" ")
def pcap_to_csv(pcap_path: str, metadata_path: str, num: int = 0, csv_dir: str = ".", csv_prefix: str = "pcap_out", csv_ext: str = "csv") -> None: """Write scans from pcap file (*pcap_path*) to plain csv files (one per lidar scan). If the *csv_ext* ends in ``.gz``, the file is automatically saved in compressed gzip format. :func:`.numpy.loadtxt` can be used to read gzipped files transparently back to :class:`.numpy.ndarray`. Number of saved lines per csv file is always [H x W], which corresponds to a full 2D image representation of a lidar scan. Each line in a csv file is: RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (m), Y (m), Z (m) Args: pcap_path: path to the pcap file metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) num: number of scans to save from pcap to csv files csv_dir: path to the directory where csv files will be saved csv_prefix: the filename prefix that will be appended with frame number and *csv_ext* csv_ext: file extension to use. If it ends with ``.gz`` the output is gzip compressed """ from itertools import islice # ensure that base csv_dir exists if not os.path.exists(csv_dir): os.makedirs(csv_dir) metadata = read_metadata(metadata_path) source = pcap.Pcap(pcap_path, metadata) # [doc-stag-pcap-to-csv] field_names = 'RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (m), Y (m), Z (m)' field_fmts = ['%d', '%d', '%d', '%d', '%.8f', '%.8f', '%.8f'] channels = [ client.ChanField.RANGE, client.ChanField.SIGNAL, client.ChanField.NEAR_IR, client.ChanField.REFLECTIVITY ] with closing(pcap.Pcap(pcap_path, metadata)) as source: # precompute xyzlut to save computation in a loop xyzlut = client.XYZLut(metadata) # create an iterator of LidarScans from pcap and bound it if num is specified scans = iter(client.Scans(source)) if num: scans = islice(scans, num) for idx, scan in enumerate(scans): fields_values = [scan.field(ch) for ch in channels] xyz = xyzlut(scan) # get lidar data as one frame of [H x W x 7], "fat" 2D image frame = np.dstack((*fields_values, xyz)) frame = client.destagger(metadata, frame) csv_path = os.path.join(csv_dir, f'{csv_prefix}_{idx:06d}.{csv_ext}') header = '\n'.join([ f'pcap file: {pcap_path}', f'frame num: {idx}', f'metadata file: {metadata_path}', field_names ]) print(f'write frame #{idx}, to file: {csv_path}') np.savetxt(csv_path, np.reshape(frame, (-1, frame.shape[2])), fmt=field_fmts, delimiter=',', header=header)
def main(): """PointViz visualizer examples.""" parser = argparse.ArgumentParser( description=main.__doc__, formatter_class=argparse.RawTextHelpFormatter) parser.add_argument('pcap_path', nargs='?', metavar='PCAP', help='path to pcap file') parser.add_argument('meta_path', nargs='?', metavar='METADATA', help='path to metadata json') args = parser.parse_args() pcap_path = os.getenv("SAMPLE_DATA_PCAP_PATH", args.pcap_path) meta_path = os.getenv("SAMPLE_DATA_JSON_PATH", args.meta_path) if not pcap_path or not meta_path: print("ERROR: Please add SAMPLE_DATA_PCAP_PATH and SAMPLE_DATA_JSON_PATH to" + " environment variables or pass <pcap_path> and <meta_path>") sys.exit() print(f"Using:\n\tjson: {meta_path}\n\tpcap: {pcap_path}") # Getting data sources meta = client.SensorInfo(open(meta_path).read()) packets = pcap.Pcap(pcap_path, meta) scans = iter(client.Scans(packets)) # ============================== print("Ex 0: Empty Point Viz") # [doc-stag-empty-pointviz] # Creating a point viz instance point_viz = viz.PointViz("Example Viz") viz.add_default_controls(point_viz) # ... add objects here # update internal objects buffers and run visualizer point_viz.update() point_viz.run() # [doc-etag-empty-pointviz] # ========================================================================= print("Ex 1.0:\tImages and Labels: the Image object and 2D Image set_position() - height-normalized screen coordinates") label_top = viz.Label("[0, 1]", 0.5, 0.0, align_top=True) label_top.set_scale(2) point_viz.add(label_top) label_bot = viz.Label("[0, -1]", 0.5, 1, align_top=False) label_bot.set_scale(2) point_viz.add(label_bot) # [doc-stag-image-pos-center] img = viz.Image() img.set_image(np.full((10, 10), 0.5)) img.set_position(-0.5, 0.5, -0.5, 0.5) point_viz.add(img) # [doc-etag-image-pos-center] # visualize point_viz.update() point_viz.run() # ========================================================================= print("Ex 1.1:\tImages and Labels: Window-aligned images with 2D Image set_hshift() - width-normalized [-1, 1] horizontal shift") # [doc-stag-image-pos-left] # move img to the left img.set_position(0, 1, -0.5, 0.5) img.set_hshift(-1) # [doc-etag-image-pos-left] # visualize point_viz.update() point_viz.run() # [doc-stag-image-pos-right] # move img to the right img.set_position(-1, 0, -0.5, 0.5) img.set_hshift(1) # [doc-etag-image-pos-right] # visualize point_viz.update() point_viz.run() # [doc-stag-image-pos-right-bottom] # move img to the right bottom img.set_position(-1, 0, -1, 0) img.set_hshift(1) # [doc-etag-image-pos-right-bottom] # visualize point_viz.update() point_viz.run() # remove_objs(point_viz, [label_top, label_mid, label_bot, img]) remove_objs(point_viz, [label_top, label_bot, img]) # ======================================= print("Ex 1.2:\tImages and Labels: Lidar Scan Fields as Images") # [doc-stag-scan-fields-images] scan = next(scans) img_aspect = (meta.beam_altitude_angles[0] - meta.beam_altitude_angles[-1]) / 360.0 img_screen_height = 0.4 # [0..2] img_screen_len = img_screen_height / img_aspect # prepare field data ranges = scan.field(client.ChanField.RANGE) ranges = client.destagger(meta, ranges) ranges = np.divide(ranges, np.amax(ranges), dtype=np.float32) signal = scan.field(client.ChanField.SIGNAL) signal = client.destagger(meta, signal) signal = np.divide(signal, np.amax(signal), dtype=np.float32) # creating Image viz elements range_img = viz.Image() range_img.set_image(ranges) # top center position range_img.set_position(-img_screen_len / 2, img_screen_len / 2, 1 - img_screen_height, 1) point_viz.add(range_img) signal_img = viz.Image() signal_img.set_image(signal) img_aspect = (meta.beam_altitude_angles[0] - meta.beam_altitude_angles[-1]) / 360.0 img_screen_height = 0.4 # [0..2] img_screen_len = img_screen_height / img_aspect # bottom center position signal_img.set_position(-img_screen_len / 2, img_screen_len / 2, -1, -1 + img_screen_height) point_viz.add(signal_img) # [doc-etag-scan-fields-images] # visualize point_viz.update() point_viz.run() print("Ex 1.3:\tImages and Labels: Adding labels") # [doc-stag-scan-fields-images-labels] range_label = viz.Label(str(client.ChanField.RANGE), 0.5, 0, align_top=True) range_label.set_scale(1) point_viz.add(range_label) signal_label = viz.Label(str(client.ChanField.SIGNAL), 0.5, 1 - img_screen_height / 2, align_top=True) signal_label.set_scale(1) point_viz.add(signal_label) # [doc-etag-scan-fields-images-labels] # visualize point_viz.update() point_viz.run() # =============================================================== print("Ex 2.0:\tPoint Clouds: As Structured Points") # [doc-stag-scan-structured] cloud_scan = viz.Cloud(meta) cloud_scan.set_range(scan.field(client.ChanField.RANGE)) cloud_scan.set_key(signal) point_viz.add(cloud_scan) # [doc-etag-scan-structured] # visualize point_viz.update() point_viz.run() remove_objs(point_viz, [cloud_scan]) # =============================================================== print("Ex 2.1:\tPoint Clouds: As Unstructured Points") # [doc-stag-scan-unstructured] # transform scan data to 3d points xyzlut = client.XYZLut(meta) xyz = xyzlut(scan.field(client.ChanField.RANGE)) cloud_xyz = viz.Cloud(xyz.shape[0] * xyz.shape[1]) cloud_xyz.set_xyz(np.reshape(xyz, (-1, 3))) cloud_xyz.set_key(signal.ravel()) point_viz.add(cloud_xyz) # [doc-etag-scan-unstructured] point_viz.camera.dolly(150) # visualize point_viz.update() point_viz.run() # ======================================================= print("Ex 2.2:\tPoint Clouds: Custom Axes Helper as Unstructured Points") # [doc-stag-axes-helper] # basis vectors x_ = np.array([1, 0, 0]).reshape((-1, 1)) y_ = np.array([0, 1, 0]).reshape((-1, 1)) z_ = np.array([0, 0, 1]).reshape((-1, 1)) axis_n = 100 line = np.linspace(0, 1, axis_n).reshape((1, -1)) # basis vector to point cloud axis_points = np.hstack((x_ @ line, y_ @ line, z_ @ line)).transpose() # colors for basis vectors axis_color_mask = np.vstack(( np.full((axis_n, 4), [1, 0.1, 0.1, 1]), np.full((axis_n, 4), [0.1, 1, 0.1, 1]), np.full((axis_n, 4), [0.1, 0.1, 1, 1]))) cloud_axis = viz.Cloud(axis_points.shape[0]) cloud_axis.set_xyz(axis_points) cloud_axis.set_key(np.full(axis_points.shape[0], 0.5)) cloud_axis.set_mask(axis_color_mask) cloud_axis.set_point_size(3) point_viz.add(cloud_axis) # [doc-etag-axes-helper] point_viz.camera.dolly(50) # visualize point_viz.update() point_viz.run() remove_objs(point_viz, [ range_img, range_label, signal_img, signal_label, cloud_axis, cloud_xyz ]) # =============================================================== print("Ex 2.3:\tPoint Clouds: the LidarScanViz class") # [doc-stag-lidar-scan-viz] # Creating LidarScan visualizer (3D point cloud + field images on top) ls_viz = viz.LidarScanViz(meta, point_viz) # adding scan to the lidar scan viz ls_viz.scan = scan # refresh viz data ls_viz.draw() # visualize # update() is not needed for LidatScanViz because it's doing it internally point_viz.run() # [doc-etag-lidar-scan-viz] # =================================================== print("Ex 3.0:\tAugmenting point clouds with 3D Labels") # [doc-stag-lidar-scan-viz-labels] # Adding 3D Labels label1 = viz.Label("Label1: [1, 2, 4]", 1, 2, 4) point_viz.add(label1) label2 = viz.Label("Label2: [2, 1, 4]", 2, 1, 4) label2.set_scale(2) point_viz.add(label2) label3 = viz.Label("Label3: [4, 2, 1]", 4, 2, 1) label3.set_scale(3) point_viz.add(label3) # [doc-etag-lidar-scan-viz-labels] point_viz.camera.dolly(-100) # visualize point_viz.update() point_viz.run() # =============================================== print("Ex 4.0:\tOverlay 2D Images and 2D Labels") # [doc-stag-overlay-images-labels] # Adding image 1 with aspect ratio preserved img = viz.Image() img_data = make_checker_board(10, (2, 4)) mask_data = np.zeros((30, 30, 4)) mask_data[:15, :15] = np.array([1, 0, 0, 1]) img.set_mask(mask_data) img.set_image(img_data) ypos = (0, 0.5) xlen = (ypos[1] - ypos[0]) * img_data.shape[1] / img_data.shape[0] xpos = (0, xlen) img.set_position(*xpos, *ypos) img.set_hshift(-0.5) point_viz.add(img) # Adding Label for image 1: positioned at bottom left corner img_label = viz.Label("ARRrrr!", 0.25, 0.5) img_label.set_rgba((1.0, 1.0, 0.0, 1)) img_label.set_scale(2) point_viz.add(img_label) # Adding image 2: positioned to the right of the window img2 = viz.Image() img_data2 = make_checker_board(10, (4, 2)) mask_data2 = np.zeros((30, 30, 4)) mask_data2[15:25, 15:25] = np.array([0, 1, 0, 0.5]) img2.set_mask(mask_data2) img2.set_image(img_data2) ypos2 = (0, 0.5) xlen2 = (ypos2[1] - ypos2[0]) * img_data2.shape[1] / img_data2.shape[0] xpos2 = (-xlen2, 0) img2.set_position(*xpos2, *ypos2) img2.set_hshift(1.0) point_viz.add(img2) # Adding Label for image 2: positioned at top left corner img_label2 = viz.Label("Second", 1.0, 0.25, align_top=True, align_right=True) img_label2.set_rgba((0.0, 1.0, 1.0, 1)) img_label2.set_scale(1) point_viz.add(img_label2) # [doc-etag-overlay-images-labels] # visualize point_viz.update() point_viz.run() # =============================================================== print("Ex 5.0:\tAdding key handlers: 'R' for random camera dolly") # [doc-stag-key-handlers] def handle_dolly_random(ctx, key, mods) -> bool: if key == 82: # key R dolly_num = random.randrange(-15, 15) print(f"Random Dolly: {dolly_num}") point_viz.camera.dolly(dolly_num) point_viz.update() return True point_viz.push_key_handler(handle_dolly_random) # [doc-etag-key-handlers] # visualize point_viz.update() point_viz.run()
def viewer_3d(scans: client.Scans, paused: bool = False) -> None: """Render one scan in Open3D viewer from pcap file with 2d image. Args: pcap_path: path to the pcap file metadata_path: path to the .json with metadata (aka :class:`.SensorInfo`) num: scan number in a given pcap file (satrs from *0*) """ # visualizer state scans_iter = iter(scans) scan = next(scans_iter) metadata = scans.metadata xyzlut = client.XYZLut(metadata) fields = list(scan.fields) aes = {} for field_ind, field in enumerate(fields): if field in (client.ChanField.SIGNAL, client.ChanField.SIGNAL2): aes[field_ind] = _utils.AutoExposure(0.02, 0.1, 3) else: aes[field_ind] = _utils.AutoExposure() field_ind = 2 def next_field(vis): nonlocal field_ind field_ind = (field_ind + 1) % len(fields) update_data(vis) print(f"Visualizing: {fields[field_ind].name}") def toggle_pause(vis): nonlocal paused paused = not paused print(f"Paused: {paused}") def right_arrow(vis, action, mods): nonlocal scan if action == 1: print("Skipping forward 10 frames") scan = nth(scans_iter, 10) if scan is None: raise StopIteration update_data(vis) # create geometries axes = o3d.geometry.TriangleMesh.create_coordinate_frame(0.2) cloud = o3d.geometry.PointCloud() image = create_canvas(metadata.format.columns_per_frame, metadata.format.pixels_per_column) def update_data(vis: o3d.visualization.Visualizer): xyz = xyzlut(scan.field(range_for_field(fields[field_ind]))) key = scan.field(fields[field_ind]).astype(float) # apply colormap to field values aes[field_ind](key) color_img = colorize(key) # prepare point cloud for Open3d Visualiser cloud.points = o3d.utility.Vector3dVector(xyz.reshape((-1, 3))) cloud.colors = o3d.utility.Vector3dVector(color_img.reshape((-1, 3))) # prepare canvas for 2d image gray_img = np.dstack([key] * 3) canvas_set_image_data(image, client.destagger(metadata, gray_img)) # signal that point cloud and needs to be re-rendered vis.update_geometry(cloud) # initialize vis vis = o3d.visualization.VisualizerWithKeyCallback() try: vis.create_window() ropt = vis.get_render_option() ropt.point_size = 1.0 ropt.background_color = np.asarray([0, 0, 0]) ropt.light_on = False # populate point cloud before adding geometry to avoid camera issues update_data(vis) vis.add_geometry(cloud) vis.add_geometry(image) vis.add_geometry(axes) # initialize camera settings ctr = vis.get_view_control() ctr.set_constant_z_near(Z_NEAR) ctr.set_zoom(0.2) view_from(vis, np.array([2, 1, 2]), np.array([0, 0, 0])) # register keys vis.register_key_callback(ord(" "), toggle_pause) vis.register_key_callback(ord("M"), next_field) vis.register_key_action_callback(262, right_arrow) # main loop last_ts = 0.0 while vis.poll_events(): ts = time.monotonic() # update data at scan frequency to avoid blocking the rendering thread if not paused and ts - last_ts >= 1 / metadata.mode.frequency: scan = next(scans_iter) update_data(vis) last_ts = ts # always update 2d image to follow camera canvas_set_viewport(image, ctr.convert_to_pinhole_camera_parameters()) vis.update_geometry(image) vis.update_renderer() finally: # open3d 0.13.0 segfaults on macos during teardown without this o3d.visualization.Visualizer.clear_geometries(vis) vis.destroy_window()
def pcap_to_csv(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0, csv_dir: str = ".", csv_base: str = "pcap_out", csv_ext: str = "csv") -> None: """Write scans from a pcap to csv files (one per lidar scan). The number of saved lines per csv file is always H x W, which corresponds to a full 2D image representation of a lidar scan. Each line in a csv file is (for LEGACY profile): TIMESTAMP, RANGE (mm), SIGNAL, NEAR_IR, REFLECTIVITY, X (mm), Y (mm), Z (mm) If ``csv_ext`` ends in ``.gz``, the file is automatically saved in compressed gzip format. :func:`.numpy.loadtxt` can be used to read gzipped files transparently back to :class:`.numpy.ndarray`. Args: source: PacketSource from pcap metadata: associated SensorInfo for PacketSource num: number of scans to save from pcap to csv files csv_dir: path to the directory where csv files will be saved csv_base: string to use as the base of the filename for pcap output csv_ext: file extension to use, "csv" by default """ # ensure that base csv_dir exists if not os.path.exists(csv_dir): os.makedirs(csv_dir) # construct csv header and data format def get_fields_info(scan: client.LidarScan) -> Tuple[str, List[str]]: field_names = 'TIMESTAMP (ns)' field_fmts = ['%d'] for chan_field in scan.fields: field_names += f', {chan_field}' if chan_field in [client.ChanField.RANGE, client.ChanField.RANGE2]: field_names += ' (mm)' field_fmts.append('%d') field_names += ', X (mm), Y (mm), Z (mm)' field_fmts.extend(3 * ['%d']) return field_names, field_fmts field_names: str = '' field_fmts: List[str] = [] # [doc-stag-pcap-to-csv] from itertools import islice # precompute xyzlut to save computation in a loop xyzlut = client.XYZLut(metadata) # create an iterator of LidarScans from pcap and bound it if num is specified scans = iter(client.Scans(source)) if num: scans = islice(scans, num) for idx, scan in enumerate(scans): # initialize the field names for csv header if not field_names or not field_fmts: field_names, field_fmts = get_fields_info(scan) # copy per-column timestamps for each channel timestamps = np.tile(scan.timestamp, (scan.h, 1)) # grab channel data fields_values = [scan.field(ch) for ch in scan.fields] # use integer mm to avoid loss of precision casting timestamps xyz = (xyzlut(scan) * 1000).astype(np.int64) # get all data as one H x W x 8 int64 array for savetxt() frame = np.dstack((timestamps, *fields_values, xyz)) # not necessary, but output points in "image" vs. staggered order frame = client.destagger(metadata, frame) # write csv out to file csv_path = os.path.join(csv_dir, f'{csv_base}_{idx:06d}.{csv_ext}') print(f'write frame #{idx}, to file: {csv_path}') header = '\n'.join([f'frame num: {idx}', field_names]) np.savetxt(csv_path, frame.reshape(-1, frame.shape[2]), fmt=field_fmts, delimiter=',', header=header)