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)
