diff --git a/examples/convert_recording.py b/examples/convert_recording.py index a7ffc8c..1402cd5 100644 --- a/examples/convert_recording.py +++ b/examples/convert_recording.py @@ -80,11 +80,15 @@ def make_filename(header: rip.Header, extension: str) -> str: if isinstance(msg, rip.RangeImage): filename = make_filename(msg.header, "xyz") path = os.path.join(args.output_folder, filename) + wrote = 0 with open(path, "w", encoding="ascii") as f_xyz: - xyz = wlsonar.range_image_to_xyz(msg) - for x, y, z in xyz: - f_xyz.write(f"{x} {y} {z}\n") - print(f"Wrote {filename} with {len(xyz)} points") + voxels = wlsonar.range_image_to_xyz(msg) + for voxel in voxels: + if voxel is not None: + x, y, z = voxel + f_xyz.write(f"{x} {y} {z}\n") + wrote += 1 + print(f"Wrote {filename} with {wrote} voxels") elif isinstance(msg, rip.BitmapImageGreyscale8): if msg.type != rip.BitmapImageType.SIGNAL_STRENGTH_IMAGE: print( diff --git a/pyproject.toml b/pyproject.toml index f9e8228..4fb6ff2 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "wlsonar" -version = "0.4.0" +version = "0.5.0" description = "Python client and Range Image Protocol utilities for Water Linked Sonar 3D-15." readme = "README.md" authors = [ diff --git a/src/wlsonar/__init__.py b/src/wlsonar/__init__.py index f5a3bc5..f901fdf 100644 --- a/src/wlsonar/__init__.py +++ b/src/wlsonar/__init__.py @@ -11,13 +11,18 @@ UdpConfig, VersionException, ) +from ._msg_helper import ( + bitmap_image_to_strength_linear, + bitmap_image_to_strength_log, + range_image_to_distance, + range_image_to_xyz, +) from ._udp_helper import ( DEFAULT_MCAST_GRP, DEFAULT_MCAST_PORT, open_sonar_udp_multicast_socket, open_sonar_udp_unicast_socket, ) -from ._xyz_helper import range_image_to_xyz __all__ = [ "DEFAULT_MCAST_GRP", @@ -27,7 +32,10 @@ "Sonar3D", "UdpConfig", "VersionException", + "bitmap_image_to_strength_linear", + "bitmap_image_to_strength_log", "open_sonar_udp_multicast_socket", "open_sonar_udp_unicast_socket", + "range_image_to_distance", "range_image_to_xyz", ] diff --git a/src/wlsonar/_msg_helper.py b/src/wlsonar/_msg_helper.py new file mode 100644 index 0000000..4ad2fba --- /dev/null +++ b/src/wlsonar/_msg_helper.py @@ -0,0 +1,106 @@ +"""Helpers for RangeImage.""" + +import math +from typing import List, Tuple + +from .range_image_protocol import BitmapImageGreyscale8, RangeImage + + +def range_image_to_distance(range_image: RangeImage) -> List[float]: + """range_image_to_distance returns distances of RangeImage pixels in meters. + + Args: + range_image: the RangeImage + + Returns: + List of distances in meters corresponding to each pixel of range_image. If there is no data + for a pixel, the distance for that pixel is 0.0. + """ + # perform the conversion documented in RangeImage.image_pixel_scale + return [ + pixel_value * range_image.image_pixel_scale for pixel_value in range_image.image_pixel_data + ] + + +def range_image_to_xyz(range_image: RangeImage) -> List[None | Tuple[float, float, float]]: + """range_image_to_xyz returns voxels of RangeImage as x,y,z in meters. + + Args: + range_image: the RangeImage + + Returns: + List of (x,y,z) tuples in meters corresponding to each pixel of range_image. If there is no + data for a pixel, the list contains None for that pixel. + """ + xyz: list[None | tuple[float, float, float]] = [] + + max_pixel_x = range_image.width - 1 + max_pixel_y = range_image.height - 1 + + fov_h = math.radians(range_image.fov_horizontal) + fov_v = math.radians(range_image.fov_vertical) + + for pixel_idx, pixel_value in enumerate(range_image.image_pixel_data): + pixel_x = pixel_idx % range_image.width + pixel_y = pixel_idx // range_image.width + if pixel_value == 0: + # No data for this pixel + xyz.append(None) + else: + distance_meters = pixel_value * range_image.image_pixel_scale + yaw_rad = (pixel_x / max_pixel_x) * fov_h - fov_h / 2 + pitch_rad = (pixel_y / max_pixel_y) * fov_v - fov_v / 2 + + x = distance_meters * math.cos(pitch_rad) * math.cos(yaw_rad) + y = distance_meters * math.cos(pitch_rad) * math.sin(yaw_rad) + z = -distance_meters * math.sin(pitch_rad) + xyz.append((x, y, z)) + + return xyz + + +def bitmap_image_to_strength_log(bitmap_image: BitmapImageGreyscale8) -> List[int]: + """bitmap_image_to_strength_log returns log signal strength from a BitmapImageGreyscale8. + + BitmapImageGreyscale8 of type SIGNAL_STRENGTH_IMAGE contains a logarithmic mapping of signal + strength to 8-bit values. This function returns that logarithmic signal strength. + + Args: + bitmap_image: a BitmapImageGreyscale8 of type SIGNAL_STRENGTH_IMAGE + + Returns: + List of ints corresponding to logarithmic 8-bit signal strength for each pixel of range + image. + """ + # image_pixel_data is the logarithmic signal strength + return [int(pixel_value) for pixel_value in bitmap_image.image_pixel_data] + + +def bitmap_image_to_strength_linear(signal_strength_image: BitmapImageGreyscale8) -> List[int]: + """bitmap_image_to_strength_linear returns linear signal strength from a BitmapImageGreyscale8. + + BitmapImageGreyscale8 of type SIGNAL_STRENGTH_IMAGE contains a logarithmic mapping of signal + strength to 8-bit values. This function returns the linear 15-bit signal strength from those + 8-bit values. + + Args: + signal_strength_image: a BitmapImageGreyscale8 of type SIGNAL_STRENGTH_IMAGE + + Returns: + List of ints corresponding to linear 15-bit signal strength for each pixel of range image. + """ + linear: list[int] = [] + + for pixel_value in signal_strength_image.image_pixel_data: + if pixel_value == 0: + # No data for this pixel + linear.append(0) + else: + # pixel_value has the following relation to the original linear signal strength: + # + # pixel_value = 100.0 * log10(linear/30.0) + # + # invert to obtain strength: + linear.append(round(30.0 * 10.0 ** (pixel_value / 100.0))) + + return linear diff --git a/src/wlsonar/_xyz_helper.py b/src/wlsonar/_xyz_helper.py deleted file mode 100644 index b5a8aee..0000000 --- a/src/wlsonar/_xyz_helper.py +++ /dev/null @@ -1,43 +0,0 @@ -"""Helpers for RangeImage.""" - -import math -from typing import List, Tuple - -from .range_image_protocol import RangeImage - - -def range_image_to_xyz(range_image: RangeImage) -> List[Tuple[float, float, float]]: - """range_image_to_xyz returns voxels of RangeImage as x,y,z in meters. - - Args: - range_image: the RangeImage - - Returns: - List of (x, y, z) tuples in meters - """ - voxels = [] - - max_pixel_x = range_image.width - 1 - max_pixel_y = range_image.height - 1 - - fov_h = math.radians(range_image.fov_horizontal) - fov_v = math.radians(range_image.fov_vertical) - - for pixel_x in range(range_image.width): - for pixel_y in range(range_image.height): - pixel_idx = pixel_y * range_image.width + pixel_x - pixel_value = range_image.image_pixel_data[pixel_idx] - if pixel_value == 0: - # No data for this pixel - continue - - distance_meters = pixel_value * range_image.image_pixel_scale - yaw_rad = (pixel_x / max_pixel_x) * fov_h - fov_h / 2 - pitch_rad = (pixel_y / max_pixel_y) * fov_v - fov_v / 2 - - x = distance_meters * math.cos(pitch_rad) * math.cos(yaw_rad) - y = distance_meters * math.cos(pitch_rad) * math.sin(yaw_rad) - z = -distance_meters * math.sin(pitch_rad) - voxels.append((x, y, z)) - - return voxels diff --git a/tests/test_msg_helpers.py b/tests/test_msg_helpers.py new file mode 100644 index 0000000..11e0df9 --- /dev/null +++ b/tests/test_msg_helpers.py @@ -0,0 +1,145 @@ +import wlsonar.range_image_protocol as rip +from wlsonar import ( + bitmap_image_to_strength_linear, + bitmap_image_to_strength_log, + range_image_to_distance, + range_image_to_xyz, +) +from wlsonar.range_image_protocol import BitmapImageGreyscale8, BitmapImageType, RangeImage + + +def test_range_image_helpers() -> None: + # basic sanity checks of the helper functions with the example data file + + # find first range image in example data file + with open("tests/data/ship_short.sonar", "rb") as f: + while True: + try: + msg = rip.unpack(f) + except rip.UnknownProtobufTypeError: + # silently skip unknown packet types + continue + except EOFError: + raise AssertionError("Expected at least one RangeImage in example data") + + if isinstance(msg, RangeImage): + example_range_image = msg + break + + distances = range_image_to_distance(example_range_image) + xyz = range_image_to_xyz(example_range_image) + expected_len = example_range_image.width * example_range_image.height + + assert len(example_range_image.image_pixel_data) == expected_len, ( + "Unexpected number of pixels in example range image" + ) + assert len(distances) == expected_len + assert len(xyz) == expected_len + + # check types + assert all(isinstance(distance, float) for distance in distances) + assert all( + voxel is None + or ( + isinstance(voxel, tuple) + and len(voxel) == 3 + and all(isinstance(coord, float) for coord in voxel) + ) + for voxel in xyz + ) + + # smoke test data + assert any(distance > 0.0 for distance in distances), ( + "Expected at least one valid distance in example range image" + ) + assert any(voxel is None for voxel in xyz), ( + "Expected at least one pixel with no data in example range image" + ) + assert any(voxel is not None for voxel in xyz), ( + "Expected at least one valid voxel in example range image" + ) + + +def test_bitmap_image_helpers() -> None: + # basic sanity checks of the helper functions with the example data file + + # find first bitmap image in example data file + with open("tests/data/ship_short.sonar", "rb") as f: + while True: + try: + msg = rip.unpack(f) + except rip.UnknownProtobufTypeError: + # silently skip unknown packet types + continue + except EOFError: + raise AssertionError("Expected at least one BitmapImage in example data") + + if ( + isinstance(msg, BitmapImageGreyscale8) + and msg.type == BitmapImageType.SIGNAL_STRENGTH_IMAGE + ): + example_bitmap_image = msg + break + + strength_log = bitmap_image_to_strength_log(example_bitmap_image) + strength_linear = bitmap_image_to_strength_linear(example_bitmap_image) + expected_len = example_bitmap_image.width * example_bitmap_image.height + + assert len(example_bitmap_image.image_pixel_data) == expected_len, ( + "Unexpected number of pixels in example bitmap image" + ) + assert len(strength_log) == expected_len + assert len(strength_linear) == expected_len + + # check types + assert all(isinstance(strength, int) for strength in strength_log) + assert all(isinstance(strength, int) for strength in strength_linear) + + # smoke test data + assert any(strength > 0.0 for strength in strength_log), ( + "Expected at least one positive strength value in example bitmap image" + ) + assert any(strength == 0.0 for strength in strength_log), ( + "Expected at least one zero strength value in example bitmap image" + ) + assert any(strength > 0.0 for strength in strength_linear), ( + "Expected at least one positive strength value in example bitmap image" + ) + assert any(strength == 0.0 for strength in strength_linear), ( + "Expected at least one zero strength value in example bitmap image" + ) + + +def test_bitmap_image_to_strength_linear__output() -> None: + # construct bitmap image with minimal fields only + image_pixel_data = [0 for _ in range(256 * 64)] + image_pixel_data[1] = 100 + image_pixel_data[2] = 255 + bitmap_image = rip.BitmapImageGreyscale8( + type=BitmapImageType.SIGNAL_STRENGTH_IMAGE, + image_pixel_data=bytes(image_pixel_data), + ) + strength_linear = bitmap_image_to_strength_linear(bitmap_image) + + assert len(strength_linear) == 256 * 64, ( + "Unexpected output length from bitmap_image_to_strength_linear" + ) + assert all(isinstance(strength, int) for strength in strength_linear), ( + "Expected strength_linear to be ints" + ) + + # smoke tests: + + assert strength_linear[0] == 0, "Expected input of 0 to produce output of 0" + + assert strength_linear[1] > 0, "Expected input of 100 to produce positive output" + assert strength_linear[1] < 2**15 - 1, ( + "Expected input of 100 to produce output of less than 2**15-1" + ) + + assert strength_linear[2] > strength_linear[1], ( + "Expected input of 255 to produce stronger output than input of 100" + ) + assert strength_linear[2] <= 2**15 - 1, ( + "Expected input of 255 to produce output of at most 2**15-1" + ) diff --git a/uv.lock b/uv.lock index e89e1fb..97b1679 100644 --- a/uv.lock +++ b/uv.lock @@ -634,7 +634,7 @@ wheels = [ [[package]] name = "wlsonar" -version = "0.4.0" +version = "0.5.0" source = { editable = "." } dependencies = [ { name = "protobuf" },