feat: mask-based pruning filter (vendor independent) (#250)

* chore(nebula_decoders): add utility types for angle pairs, ranges, and FoVs

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(downsample_mask): add downsample_mask filter class

The filter takes a path to a PNG image (will be converted to grayscale) and expected resolution/channel count of the mask.

The grayscale mask is then dithered into a black/white mask of the same dimensions, and an `excluded(NebulaPoint const&)` function is provided to test whether a point shall be excluded according to the mask.

For debug purposes, the dithered mask is written to the same directory as the input mask, with the file ending changed to `_dithered.png`. If this fails, the filter will not throw but log a warning.

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(nebula_ros): add schema for point filters, and the downsample mask

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(downsample_mask): add dependencies to nebula_decoders/package.xml

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(downsample_mask): make debug mask output optional, clean up code

Signed-off-by: Max SCHMELLER <[email protected]>

* test(downsample_mask): add unit tests for dithering and filtering

Signed-off-by: Max SCHMELLER <[email protected]>

* docs(downsample_filter): add downsample filter docs

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(cspell): add milli-degrees (`mdeg`) to dictionary

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(downsample_mask): remove `.` before the exported mask's suffix

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(downsample_mask): explicitly cast instead of implicit conversions

Signed-off-by: Max SCHMELLER <[email protected]>

* fix(downsample_mask): make `excluded()` function resilient to rounding errors

Signed-off-by: Max SCHMELLER <[email protected]>

* fix(downsample_mask): add input validation for constructor parameters

Add input validation checks for azimuth peak resolution, azimuth range extent, and number of channels to prevent divide-by-zeros and other issues

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(downsample_mask): fix imports

Signed-off-by: Max SCHMELLER <[email protected]>

* chore(angles): introduce `AngleUnit`s, make angle ranges support `end < start`angle.

Signed-off-by: Max SCHMELLER <[email protected]>

---------

Signed-off-by: Max SCHMELLER <[email protected]>

.cspell.json

@@ -25,6 +25,7 @@
     "Idat",
     "ipaddr",
     "manc",
+    "mdeg",
     "memcpy",
     "mkdoxy",
     "Msop",

docs/filters.md

@@ -0,0 +1,97 @@
+# Point Filters
+
+Point filters run for every decoded point, reducing pointcloud size right at the decode stage.
+This can speed up the later parts of pointcloud processing pipelines by reducing the number of points copied between and processed by modules.
+
+## Configuration
+
+Filters are configured via the ROS parameter namespace `point_filters`.
+
+```yaml
+{
+  point_filters:
+    filter_type_a:
+      parameter_1: 5
+      parameter_2: "abc"
+    filter_type_b:
+      # ...
+}
+```
+
+Where each `filter_type` can be specified at most once.
+The configuration options available depend on the respective filter type.
+
+Filters can also be set during runtime, e.g. via:
+
+```shell
+# replace <vendor> with the name of a supported vendor
+ros2 param set /<vendor>_ros_wrapper_node point_filters.filter_type_a ...'
+```
+
+## Supported Filters
+
+The following filter types are supported:
+
+| Filter Name            | Filter Type       | Hesai | Robosense | Velodyne |
+| ---------------------- | ----------------- | :---: | :-------: | :------: |
+| Downsample Mask Filter | `downsample_mask` |  ❌   |    ❌     |    ❌    |
+
+Compatibility:  
+✅: compatible  
+❌: incompatible
+
+Below, each filter type is documented in detail.
+
+### Downsample Mask Filter
+
+This filter takes a greyscale PNG image that represents polar coordinates (x=azimuth, y=elevation)
+and downsamples the pointcloud according to the lightness values of the image's pixels.
+
+<!-- prettier-ignore-start -->
+!!! note
+    For ring-based sensors, `y` represents the `channel` as a proxy for `elevation`.
+    The image height has to be equal to the sensor's number of channels.
+<!-- prettier-ignore-end -->
+
+The input image is dithered to a boolean mask:
+
+| Stage                                      |                         Image                         |
+| :----------------------------------------- | :---------------------------------------------------: |
+| Input greyscale mask                       |     ![Greyscale mask](filters/at128_test_roi.png)     |
+| Internal dithered mask generated by Nebula | ![Dithered mask](filters/at128_test_roi_dithered.png) |
+
+The decoded points are then kept/discarded based on that mask:
+
+| ![Pointcloud density](filters/at128_test_roi_cloud.png)                                                                                               | ![Pointcloud closeup](filters/at128_test_roi_cloud_closeup.png)                                       |
+| ----------------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------- |
+| Pointcloud output. The original pointcloud had uniform density (all white). 2D azimuth-elevation view, points are blurred to better visualize density | Close-up view of a region with multiple different downsampling levels (bottom left of the pointcloud) |
+
+#### Configuration Options
+
+Configuration is done in the following format:
+
+```yaml
+downsample_mask:
+  path: /path/to/mask.png
+```
+
+Or, during runtime, by setting:
+
+```shell
+ros2 param set /<vendor>_ros_wrapper_node point_filters.downsample_mask.path /path/to/mask.png'
+```
+
+The filter can be disabled by omitting the `downsample_mask` config item, or by setting `path` to an empty string:
+
+```shell
+ros2 param set /<vendor>_ros_wrapper_node point_filters.downsample_mask.path ""'
+```
+
+#### Behavior
+
+- Greyscale values are quantized to the nearest 10th (yielding 11 quantization levels in total)
+- Mask resolution is dictated by the sensor's maximum FoV, the number of channels (for rotational LiDARs) and the peak angular resolution:
+  - For a 40-channel LiDAR with `360 deg` FoV and `0.1 deg` peak azimuth resolution, the mask has to be `(360 / 0.1, 40) = (3600, 40)` pixels
+  - Currently, non-rotational LiDARs are not yet supported
+- Image editors like GIMP use perceptual color profiles, which can lead to unexpected results (more/less downsampling than expected). Check the generated `_dithered.png` mask to see if you are affected.
+- Dithering performed by Nebula is spatial only, meaning that it stays constant over time. Decoded points are checked against the nearest pixel in the dithered mask

docs/index.md

@@ -25,6 +25,7 @@ Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware](
 - [Design](design.md)
 - [Parameters](parameters.md)
 - [Point cloud types](point_types.md)
+- [Point filters](filters.md)
 
 ## Supported sensors
 

docs/parameters.md

@@ -94,3 +94,7 @@ This parameter is used to set this preference.
 ### `dual_return_distance_threshold`
 
 For multiple returns that are close together, the points will be fused into one if they are below this threshold (in meters).
+
+### `point_filters`
+
+Filters that are applied while decoding the pointcloud. For the full reference, see [Point filters](filters.md).

nebula_decoders/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(robosense_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(velodyne_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(PNG REQUIRED)
 
 include_directories(PUBLIC
     include
@@ -125,6 +126,17 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     ament_lint_auto_find_test_dependencies()
+
+    find_package(ament_cmake_gtest REQUIRED)
+
+    add_definitions(-D_TEST_RESOURCES_PATH="${PROJECT_SOURCE_DIR}/test_resources/")
+
+    ament_add_gtest(test_downsample_mask tests/point_filters/test_downsample_mask.cpp)
+    target_link_libraries(test_downsample_mask
+        ${PNG_LIBRARIES})
+    target_include_directories(test_downsample_mask PUBLIC
+        include
+        ${PNG_INCLUDE_DIRS})
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp

@@ -15,9 +15,59 @@
 #pragma once
 
 #include <cmath>
+#include <cstdint>
+
 namespace nebula::drivers
 {
 
+struct Radians
+{
+  static constexpr double circle_modulus = 2 * M_PI;
+};
+
+template <uint32_t Subdivisions>
+struct ScaledDegrees
+{
+  static constexpr double circle_modulus = 360 * Subdivisions;
+};
+
+using Degrees = ScaledDegrees<1>;
+using DeciDegrees = ScaledDegrees<10>;
+using CentiDegrees = ScaledDegrees<100>;
+using MilliDegrees = ScaledDegrees<1000>;
+
+template <typename T, typename AngleUnit>
+struct AnglePair
+{
+  T azimuth;
+  T elevation;
+};
+
+/// @brief A range defined by a start and end angle. Crossing the 0/circle_modulus boundary (end <
+/// start) is allowed.
+/// @tparam T The type of the angle.
+/// @tparam AngleUnit The unit of the angle.
+template <typename T, typename AngleUnit>
+struct AngleRange
+{
+  T start;
+  T end;
+
+  /// @brief The extent of the range, taking into account the 0/circle_modulus boundary. Will always
+  /// be positive.
+  [[nodiscard]] T extent() const
+  {
+    return (end < start) ? AngleUnit::circle_modulus - start + end : end - start;
+  }
+};
+
+template <typename T, typename AngleUnit>
+struct FieldOfView
+{
+  AngleRange<T, AngleUnit> azimuth;
+  AngleRange<T, AngleUnit> elevation;
+};
+
 /**
  * @brief Tests if `angle` is in the region of the circle defined by `start_angle` and `end_angle`.
  * Notably, `end_angle` can be smaller than `start_angle`, in which case the region passes over the

nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/downsample_mask.hpp

@@ -0,0 +1,153 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_common/angles.hpp"
+
+#include <nebula_common/loggers/logger.hpp>
+#include <nebula_common/nebula_common.hpp>
+#include <nebula_common/point_types.hpp>
+#include <nebula_common/util/string_conversions.hpp>
+#include <png++/error.hpp>
+#include <png++/gray_pixel.hpp>
+#include <png++/image.hpp>
+
+#include <Eigen/src/Core/Matrix.h>
+#include <sys/types.h>
+
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <filesystem>
+#include <memory>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+namespace nebula::drivers::point_filters
+{
+
+namespace impl
+{
+
+inline void dither(
+  const png::image<png::gray_pixel> & in, png::image<png::gray_pixel> & out,
+  uint8_t quantization_levels)
+{
+  if (in.get_width() != out.get_width() || in.get_height() != out.get_height()) {
+    std::stringstream ss;
+    ss << "Expected downsample mask of size "
+       << "(" << out.get_width() << ", " << out.get_height() << ")";
+    ss << ", got "
+       << "(" << in.get_width() << ", " << in.get_height() << ")";
+
+    throw std::runtime_error(ss.str());
+  }
+
+  uint32_t denominator = quantization_levels;
+
+  auto should_keep = [denominator](uint32_t numerator, uint32_t pos) {
+    for (uint32_t i = 0; i < numerator; ++i) {
+      auto dithered_pos =
+        static_cast<size_t>(std::round(denominator / static_cast<double>(numerator) * i));
+      if (dithered_pos == pos) return true;
+    }
+    return false;
+  };
+
+  for (size_t y = 0; y < out.get_height(); ++y) {
+    for (size_t x = 0; x < out.get_width(); ++x) {
+      const auto & pixel = in.get_pixel(x, y);
+      uint32_t numerator = static_cast<uint32_t>(pixel) * denominator / 255;
+      size_t pos = (x + y) % denominator;
+      bool keep = should_keep(numerator, pos);
+      out.set_pixel(x, y, keep * 255);
+    }
+  }
+}
+
+}  // namespace impl
+
+class DownsampleMaskFilter
+{
+  static const uint8_t g_quantization_levels = 10;
+
+public:
+  DownsampleMaskFilter(
+    const std::string & filename, AngleRange<int32_t, MilliDegrees> azimuth_range_mdeg,
+    uint32_t azimuth_peak_resolution_mdeg, size_t n_channels,
+    const std::shared_ptr<loggers::Logger> & logger, bool export_dithered_mask = false)
+  : azimuth_range_{
+      deg2rad(azimuth_range_mdeg.start / 1000.), deg2rad(azimuth_range_mdeg.end / 1000.)}
+  {
+    if (azimuth_peak_resolution_mdeg == 0) {
+      throw std::invalid_argument("azimuth_peak_resolution_mdeg must be positive");
+    }
+    if (azimuth_range_.extent() <= 0) {
+      throw std::invalid_argument("azimuth range extent must be positive");
+    }
+    if (n_channels == 0) {
+      throw std::invalid_argument("n_channels must be positive");
+    }
+
+    png::image<png::gray_pixel> factors(filename);
+
+    size_t mask_cols = azimuth_range_mdeg.extent() / azimuth_peak_resolution_mdeg;
+    size_t mask_rows = n_channels;
+
+    png::image<png::gray_pixel> dithered(mask_cols, mask_rows);
+    impl::dither(factors, dithered, g_quantization_levels);
+
+    mask_ = Eigen::MatrixX<uint8_t>(mask_rows, mask_cols);
+
+    for (size_t y = 0; y < dithered.get_height(); ++y) {
+      for (size_t x = 0; x < dithered.get_width(); ++x) {
+        mask_.coeffRef(static_cast<int32_t>(y), static_cast<int32_t>(x)) = dithered.get_pixel(x, y);
+      }
+    }
+
+    if (export_dithered_mask) {
+      std::filesystem::path out_path{filename};
+      out_path = out_path.replace_filename(
+        out_path.stem().string() + "_dithered" + out_path.extension().string());
+
+      try {
+        dithered.write(out_path);
+        logger->info("Wrote dithered mask to " + out_path.native());
+      } catch (const png::std_error & e) {
+        logger->warn("Could not write " + out_path.native() + ": " + e.what());
+      }
+    }
+  }
+
+  bool excluded(const NebulaPoint & point)
+  {
+    double azi_normalized = (point.azimuth - azimuth_range_.start) / azimuth_range_.extent();
+
+    auto x = static_cast<ssize_t>(std::round(azi_normalized * static_cast<double>(mask_.cols())));
+    auto y = point.channel;
+
+    bool x_out_of_bounds = x < 0 || x >= mask_.cols();
+    bool y_out_of_bounds = y >= mask_.rows();
+
+    return x_out_of_bounds || y_out_of_bounds || !mask_.coeff(y, x);
+  }
+
+private:
+  AngleRange<double, Radians> azimuth_range_;
+  Eigen::MatrixX<uint8_t> mask_;
+};
+
+}  // namespace nebula::drivers::point_filters

nebula_decoders/package.xml

@@ -16,6 +16,8 @@
   <depend>continental_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>libpcl-all-dev</depend>
+  <depend>libpng++-dev</depend>
+  <depend>libpng-dev</depend>
   <depend>nebula_common</depend>
   <depend>nebula_msgs</depend>
   <depend>pandar_msgs</depend>